CN111805536A - Self-adaptive sliding mode control method for fruit sorting parallel robot mechanism considering coupling effect - Google Patents

Self-adaptive sliding mode control method for fruit sorting parallel robot mechanism considering coupling effect Download PDF

Info

Publication number
CN111805536A
CN111805536A CN202010529155.XA CN202010529155A CN111805536A CN 111805536 A CN111805536 A CN 111805536A CN 202010529155 A CN202010529155 A CN 202010529155A CN 111805536 A CN111805536 A CN 111805536A
Authority
CN
China
Prior art keywords
parallel robot
robot mechanism
fruit sorting
sliding mode
sorting parallel
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
Application number
CN202010529155.XA
Other languages
Chinese (zh)
Inventor
高国琴
王哲
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Jiangsu University
Original Assignee
Jiangsu University
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Jiangsu University filed Critical Jiangsu University
Priority to CN202010529155.XA priority Critical patent/CN111805536A/en
Publication of CN111805536A publication Critical patent/CN111805536A/en
Pending legal-status Critical Current

Links

Images

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1628—Programme controls characterised by the control loop
    • B25J9/163—Programme controls characterised by the control loop learning, adaptive, model based, rule based expert control
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/003—Programme-controlled manipulators having parallel kinematics
    • 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
    • G05B2219/00—Program-control systems
    • G05B2219/30—Nc systems
    • G05B2219/40—Robotics, robotics mapping to robotics vision
    • G05B2219/40496—Hierarchical, learning, recognition level controls adaptation, servo level

Landscapes

  • Engineering & Computer Science (AREA)
  • Robotics (AREA)
  • Mechanical Engineering (AREA)
  • Feedback Control In General (AREA)

Abstract

The invention discloses a fruit sorting parallel robot mechanism self-adaptive sliding mode control method considering coupling effect. Firstly, aiming at a fruit sorting parallel robot mechanism, establishing a joint space dynamic model according to a virtual work principle, and providing a parallel robot driving joint equivalent inertia calculation method considering inter-branch coupling by performing coupling analysis on an inertia matrix in the dynamic model; secondly, designing a sliding mode controller according to mathematical models of branches of the fruit sorting parallel robot; then, an adaptive law is designed, wherein the adaptive law can quickly adapt to uncertainty changes such as time change of mechanism parameters of the parallel robot and external interference without uncertainty prior information, buffeting caused by sliding mode control switching gain overestimation is effectively inhibited, and finally, the fruit sorting parallel robot mechanism is self-adaptive sliding mode controlled by considering the coupling effect through software programming. The invention realizes the high-performance control of the kinematics of the fruit sorting parallel robot mechanism.

Description

Self-adaptive sliding mode control method for fruit sorting parallel robot mechanism considering coupling effect
Technical Field
The invention relates to the technical field of agricultural parallel robot control, in particular to a fruit sorting parallel robot mechanism self-adaptive sliding mode control method considering coupling effect.
Background
The parallel mechanism adopts a closed chain structure and has the performance advantages of high rigidity, high precision and strong bearing capacity. Is particularly suitable for fruit sorting operation with higher requirement on grabbing stability. However, the kinematics control research of the parallel robot mechanism has some difficulties, mainly including: the problem of coupling between the branches of the parallel robot mechanism; uncertainty problems of time varying, external disturbance and the like exist in model parameters of each branch motor in the parallel robot system.
An inertial parameter estimation method considering inter-chain coupling of a high-speed parallel robot is designed in the text of Zhao Qing, Wang Pan, Huangtian, Tianjin university school newspaper, 2017, volume 50, phase 8, page 868-876). However, in the PID control, the inertia on a motor shaft is changed, and the high-performance control of each branch motor is difficult to realize under the influence of uncertainties such as external disturbance and the like in the actual control.
The text entitled "delay estimation adaptive sliding mode control for a series-parallel automobile electrophoretic coating conveying mechanism" (Gao national organ, Zhou Hui, Shi Zhi Ming, automotive engineering, 2018, volume 40, No. 12, page 1405 and 1412) designs a delay estimation adaptive sliding mode control method for a series-parallel automobile electrophoretic coating conveying mechanism. According to the sliding mode variable design self-adaptive law, however, the method cannot adapt to uncertainty change rapidly, and therefore the problem of buffeting caused by over-estimation of switching gain still exists in a sliding mode control system.
Disclosure of Invention
Aiming at overcoming the defects of the prior art, the invention provides a coupling effect-considering self-adaptive sliding mode control method for a fruit sorting parallel robot mechanism. According to the method, under the problems of inter-branch coupling effect and branch uncertainty of a fruit sorting parallel robot, firstly, a parallel robot driving joint equivalent inertia calculation method considering inter-branch coupling is provided, equivalent load inertia of each motor is considered through conversion, then, an adaptive law capable of rapidly adapting to uncertainty changes of time variation of parallel robot mechanism parameters, external interference and the like without uncertainty prior information and effectively inhibiting buffeting brought by sliding mode control switching gain overestimation is designed aiming at single-branch sliding mode control switching gain of a fruit sorting parallel robot mechanism, and finally, a fruit sorting parallel robot mechanism adaptive sliding mode controller considering coupling effect is obtained, so that the effects of inter-branch coupling effect and branch uncertainty are overcome and the buffeting controlled by sliding modes is effectively inhibited.
A fruit sorting parallel robot mechanism self-adaptive sliding mode control method considering coupling effect comprises the following steps:
1) the fruit sorting parallel robot mechanism is subjected to kinematic analysis, and a fruit sorting parallel robot mechanism position inverse solution equation and a Jacobian matrix are obtained;
2) establishing a spatial dynamic model of the joint of the fruit sorting parallel robot mechanism by adopting a virtual work principle;
3) based on the spatial dynamics model of the joint of the fruit sorting parallel robot mechanism in the step 2), researching a driving joint equivalent inertia calculation method considering coupling among branches, and converting the driving joint equivalent inertia into equivalent load inertia of each motor for consideration;
4) designing a sliding mode controller of the fruit sorting parallel robot mechanism by combining equivalent load inertia of each motor in the step 3);
5) aiming at the sliding mode control switching gain of the fruit sorting parallel robot mechanism in the step 4), a self-adaptive law is designed, wherein the self-adaptive law can quickly adapt to uncertainty changes such as time variation of parallel robot mechanism parameters, external interference and the like without uncertainty prior information, and can effectively inhibit buffeting caused by overestimation of sliding mode control switching gain;
6) a fruit sorting parallel robot mechanism is formed based on the step 4) and the step 5), and a coupling effect is considered, so that the self-adaptive sliding mode controller is adopted;
7) and the fruit sorting parallel robot mechanism realizes the self-adaptive sliding mode control considering the coupling effect through software programming.
The invention provides a fruit sorting parallel robot mechanism self-adaptive sliding mode control method considering coupling effect for the first time, which is applied to realizing the motion control of the fruit sorting parallel robot mechanism and has the characteristics and beneficial effects that:
1. establishing a fruit sorting parallel robot mechanism joint space dynamic model according to a virtual work principle, providing a parallel robot driving joint equivalent inertia calculation method considering inter-branch coupling by performing coupling analysis on an inertia matrix in the dynamic model, and considering equivalent load inertia of each motor so as to overcome the influence of the coupling action among the branches in kinematic control and improve the kinematic control performance of the fruit sorting parallel robot;
2. aiming at the problems of inter-branch coupling effect and branch uncertainty of a fruit sorting parallel robot, a sliding mode control method is introduced, and an adaptive law capable of quickly adapting to uncertainty changes such as time variation of mechanism parameters of the parallel robot and external interference is designed based on sliding mode variables, so that the effects of the inter-branch coupling effect and the branch uncertainty are overcome under the condition of not needing uncertainty prior information, and meanwhile buffeting caused by sliding mode control switching gain overestimation is effectively inhibited.
Drawings
Fig. 1 is a structure diagram of a fruit sorting parallel robot.
Fig. 2 is a structure diagram of the movable platform.
FIG. 3 is a schematic diagram of an adaptive sliding mode control system that takes into account coupling.
Fig. 4 is a structural diagram of a fruit sorting parallel robot mechanism.
Fig. 5 is a general structure diagram of a fruit sorting parallel robot mechanism control system.
Fig. 6 is a graph of the tracking trajectory of the motor of the fruit sorting parallel robot mechanism under the conditions of inter-branch coupling and branch uncertainty. (a) Tracking a track curve for the branch 1 motor; (b) the trajectory curve is tracked for branch 3 motor.
Fig. 7 is a graph of motor tracking error of a fruit sorting parallel robot mechanism under the conditions of inter-branch coupling and branch uncertainty. (a) A motor tracking error curve of the branch 1; (b) the motor tracking error curve for branch 3.
Fig. 8 is a comparison graph of controller outputs of a fruit sorting parallel robot mechanism under inter-branch coupling and branch uncertainty conditions. (a) The controller outputs a comparison for branch 1; (b) the controller outputs a comparison for leg 3.
Fig. 9 is a graph of the tracking track of the motor of the fruit sorting parallel robot mechanism under the condition of coupling between branches. (a) Tracking a track curve for the branch 1 motor; (b) the trajectory curve is tracked for branch 3 motor.
Fig. 10 is a graph of motor tracking error of a fruit sorting parallel robot mechanism under the condition of coupling between branches. (a) A motor tracking error curve of the branch 1; (b) the motor tracking error curve for branch 3.
Fig. 11 is a graph comparing controller outputs for a fruit sorting parallel robot mechanism under inter-branch coupling conditions. (a) The controller outputs a comparison for branch 1; (b) the controller outputs a comparison for leg 3.
Detailed Description
The following further describes the embodiments of the present invention with reference to the drawings.
Firstly, performing kinematic analysis on the fruit sorting parallel robot mechanism to obtain an inverse solution equation and a Jacobian matrix of the fruit sorting parallel robot mechanism position; secondly, establishing a spatial dynamic model of the joint of the fruit sorting parallel robot mechanism according to the virtual work principle; then, coupling analysis is carried out on an inertia matrix in the dynamic model, an inertia matrix diagonal dominance characteristic index is defined, a driving joint equivalent inertia calculation method considering coupling among branches is further provided, and equivalent load inertia of each motor is considered through conversion; then, according to a single-branch mathematical model of a fruit sorting parallel robot mechanism, designing a sliding mode controller; further, aiming at the switching gain of the sliding mode controller, a self-adaptive law is designed, wherein the self-adaptive law can quickly adapt to uncertainty changes such as mechanism parameter time variation of the parallel robot and external interference under the condition of no uncertainty prior information, and can effectively inhibit buffeting caused by overestimation of the sliding mode control switching gain, so that the design of the self-adaptive sliding mode controller considering the coupling effect is completed; and finally, realizing the self-adaptive sliding mode control of the fruit sorting parallel robot mechanism in consideration of the coupling effect through software programming.
The specific method comprises the following steps:
1) the fruit sorting parallel robot mechanism is subjected to kinematic analysis to obtain the fruit sorting parallel robot mechanism position inverse solution equation and the Jacobian matrix
Selecting a midpoint of the static platform to establish a reference coordinate system O-xyz, and regarding the active platform as a particle P1The auxiliary platform is regarded as particle P2Point P on the main (auxiliary) of the mobile platform1(P2) Is [ x, y, z ] is given as the position vector r]TCan be expressed as
Figure BDA0002534743130000041
In which s is P1Point to P2Point distance (in m); e is vector OAiThe mold of (4); gamma rayiDenotes the structural angle (in rad/s) of the static platform, gammai＝(i-1)π/2；l1、l2、ui、wiRespectively expressed as the rod length (in m) and the unit vector of a driving arm and a driven arm in the ith branched chain; and is
Figure BDA0002534743130000042
ei＝e(cosγisinγ0)T
ui＝(cosγicosθisinγicosθi-sinθi)T
In the formula, thetaiThe rotation angle of the driving arm i is the rotation angle of the driving joint (unit is rad).
Rewriting formula (1) to
Figure BDA0002534743130000043
And two sides are simultaneously dot-multiplied by respective transposing to obtain
Figure BDA0002534743130000044
Will uiSubstituted for formula (2) to give
Eisinθi+Ficosθi+Gi＝0 (3)
In the formula
Figure BDA0002534743130000045
Figure BDA0002534743130000046
Figure BDA0002534743130000047
Is provided with
Figure BDA0002534743130000048
Can obtain the product
(Gi-Fi)t2+2Eit+Fi+Gi＝0 (4)
The inverse solution equation of the position of the mechanism can be obtained as
Figure BDA0002534743130000049
In the formula (I), the compound is shown in the specification,
Figure BDA0002534743130000051
w is obtained from the following equations (1) and (5) as unit vectors of each coordinate axis of the coordinate system O-xyziComprises the following steps:
Figure BDA0002534743130000052
the equal sign of the formula (1) is simultaneously subjected to time derivation to obtain
Figure BDA0002534743130000053
In the formula (I), the compound is shown in the specification,
Figure BDA00025347431300000512
is a point P1Velocity vector (in m/s);
Figure BDA0002534743130000055
the angular velocity (unit is rad/s) of the driving arm in the ith branched chain; omegaiIs the angular velocity vector (in rad/s) of the driven arm in the ith branched chain; v isiDenotes perpendicular to eiAnd uiA unit vector stretched into a plane and having
vi＝(-sinγicosγi0)T
Multiplying both sides of equal sign of formula (6) by w at the same timei TIs obtained by
Figure BDA0002534743130000056
The equation (7) is rewritten into a matrix form to obtain a speed model of the mechanism as
Figure BDA0002534743130000057
Figure BDA0002534743130000058
In the formula, JθIs a direct Jacobian matrix; j. the design is a squarexIs an indirect Jacobian matrix; j is a Jacobian matrix;
Figure BDA0002534743130000059
Figure BDA00025347431300000510
2) a kinetic model of the fruit sorting parallel robot mechanism is established by adopting a virtual work principle. To build a fruit sorting parallel robot mechanism dynamics model, the following assumptions were made: 1. the kinematic pair has no energy dissipation caused by friction; 2. because the driven arm is a light rod, the mass of the driven arm is distributed to the movable platform and the driving arm according to the ratio of 1:2 by neglecting the moment of inertia.
According to the principle of work deficiency, the method can be obtained
Figure BDA00025347431300000511
Changing theta to Jr and thetasThe simplified rigid body dynamics model of the 4-R (2-SS) parallel robot mechanism is obtained by substituting (2 pi/p) s (p is a screw pitch) into formula (9)
τ＝τa+τv+τg(10)
Figure BDA0002534743130000061
τv＝IAf，τg＝τAg+τgplate
Figure BDA0002534743130000062
Figure BDA0002534743130000063
Wherein τ is a drive torque (unit is N.m) for driving the joint, and τ isaIs the term of inertia (in N.m), tau in the drive torquevIs a speed term (unit is N.m), taugIs a gravity term (unit is N.m), tauAgIs the gravitational moment (unit is N.m) of the driving arm (including the equivalent mass of the driven arm); m is1＝ms1+mscrew+2/3mrod+mloadAnd m2＝ms2+2/3mrodRespectively converted to P1And P2The mass (in kg) of the mass (c) of the cell,
Figure BDA0002534743130000066
is the moment of inertia (in kg. m) equivalent to the active arm to its axis of rotation2)，Is＝Iscrew+IloadIs the moment of inertia of the screw (unit kg.m)2)、mara＝marmrarm+2/3mrodl1Is the mass-diameter product (in kg.m) of the driving arm (including the equivalent mass of the driven arm), wherein m iss1And ms2The self mass (unit kg) and the m of the active platform and the auxiliary platform are respectivelyloadIs the load mass (unit is kg), marmThe mass (unit is kg) of the driving arm and marmrarmThe mass product (in kg. m) of the active arm to its axis of rotation.
3) On the basis of a fruit sorting parallel robot mechanism joint space dynamics model, a driving joint equivalent inertia calculation method considering coupling among branches is researched and is converted into equivalent load inertia of each motor for consideration; in order to research and analyze the coupling between the branches of the parallel robot mechanism, a dynamic model of the parallel robot mechanism in joint space needs to be established, so that the dynamic model expressed by the formula (10) under the generalized coordinate is converted into the dynamic model of the joint space to obtain the dynamic model
Figure BDA0002534743130000064
In the formula
M(θ)＝J-TmJ-1+IA
Figure BDA0002534743130000065
G(θ)＝τgplate+τAg
Equation (11) represents the relationship between the drive joint torque and the mechanism position, velocity, acceleration. For a fruit sorting parallel robot mechanism, the inertia matrix M (theta) is a symmetrical positive definite matrix of 4 multiplied by 4. And if the coupling action among the branches is not considered, the equivalent inertia parameters of the driving joints are corresponding main diagonal elements in the inertia matrix. The strength of the coupling action can be described by the diagonal dominance characteristic of the inertia matrix, which is defined as: if the absolute value of each main diagonal element of the matrix is greater than the sum of the absolute values of the other elements of the row, the matrix is called 'strictly diagonal dominant'.
Figure BDA0002534743130000071
In order to consider the coupling effect among branches in the kinematic control, an inertia matrix Diagonal Dominant Index (DDI) is defined as
Figure BDA0002534743130000072
Because the inertia matrix of the fruit sorting parallel robot mechanism is a symmetrical positive definite matrix, main diagonal elements of the fruit sorting parallel robot mechanism are positive and meet strict diagonal dominance, DDI is greater than 1, and the larger the value is, the more obvious the diagonal dominance characteristic of the inertia matrix is. From formula (13)
Figure BDA0002534743130000073
Where tr (M) is the trace of the inertia matrix.
The inertia term in the driving joint torque of the formula (11) is expanded
Figure BDA0002534743130000074
Then there is
Figure BDA0002534743130000075
Obviously, must exist
Figure BDA0002534743130000076
And satisfy
Figure BDA0002534743130000077
So that
Figure BDA0002534743130000078
Can be obtained by combining formula (14)
Figure BDA0002534743130000079
In summary, the equivalent inertia of the driving joint considering the coupling between the branches is
Figure BDA0002534743130000081
In the formula, λiIs the eigenvalue of the inertia matrix. So that the equivalent load inertia including the coupling inertia applied to the motor shaft can be expressed as
Figure BDA0002534743130000082
4) Design of sliding mode controller of fruit sorting parallel robot mechanism
Using fruit sorting parallel robot mechanism branch AC servo motor and driver as controlled object, and taking state variable x of system1＝θ、
Figure BDA0002534743130000083
The fruit sorting parallel robot mechanism branch mathematical model can be obtained by respectively representing the angular displacement, the angular velocity and the angular acceleration of a servo driving motor:
Figure BDA0002534743130000084
wherein x is [ x ]1,x2,x3]T∈RnRepresents a system state variable, wherein x1＝θ、
Figure BDA0002534743130000085
Respectively angular displacement (in rad), angular velocity (in rad/s) and angular acceleration (in rad/s) of the servo drive motor2) (ii) a u and y respectively represent system control input and output; function(s)
Figure BDA0002534743130000086
Function(s)
Figure BDA0002534743130000087
Wherein R isphIs stator winding resistance, KpiFor current loop gain, KiiAs a current feedback coefficient, KTIs the torque coefficient, KETo induce an electromotive constant, KpvIs the speed loop gain, alphaTFor feedback coefficient of velocity measurement, LDIs armature inductance, and J' is motor inertia parameter; d (t) represents an external perturbation.
Fruit sorting parallel robot mechanism sliding mode control law is
Figure BDA0002534743130000088
In the formula ueqEquivalent control is adopted; u. ofsFor switching control;
Figure BDA0002534743130000089
is a sliding mode variable; e-thetad，
Figure BDA00025347431300000813
And
Figure BDA00025347431300000811
respectively an angular displacement error, an angular velocity error and an angular acceleration error of the servo drive motor, wherein theta,
Figure BDA00025347431300000812
And
Figure BDA0002534743130000091
is angular displacement (in rad), angular velocity (in rad/s) and angular acceleration (in rad/s) of the servo drive motor2)，θd、
Figure BDA0002534743130000092
And
Figure BDA0002534743130000093
a desired angular displacement, a desired angular velocity, and a desired angular acceleration for the servo drive motor; c. C1And c2Is an adjustable parameter and meets the Hall Woltz stable condition;
Figure BDA0002534743130000094
and
Figure BDA0002534743130000095
nominal values for functions f (x) and g (x), respectively; k (t) is the switching gain.
5) Aiming at the sliding mode control switching gain of the fruit sorting parallel robot mechanism, a self-adaptive law is designed, which can quickly adapt to uncertainty changes such as time variation of parallel robot mechanism parameters and external interference without uncertainty prior information and effectively inhibit buffeting caused by overestimation of sliding mode control switching gain;
an adaptation law is designed for the switching gain K (t)
Figure BDA0002534743130000096
Wherein α, β and μ are positive parameters; the parameter gamma is a positive integer.
6) Self-adaptive sliding mode controller considering coupling effect and formed by combining sliding mode control of fruit sorting parallel robot mechanism and self-adaptive law of switching gain
Aiming at a fruit sorting parallel robot mechanism, on the basis of a sliding mode control law u of the fruit sorting parallel robot mechanism, a self-adaptive law of a designed sliding mode control switching gain K (t) is combined to form the following self-adaptive sliding mode control law considering the coupling effect
Figure BDA0002534743130000097
7) And the fruit sorting parallel robot mechanism realizes the self-adaptive sliding mode control considering the coupling effect through software programming.
A software program of a coupling effect-considering self-adaptive sliding mode control algorithm based on the fruit sorting parallel robot mechanism is compiled, voltage analog quantity obtained by the digital/analog conversion of the driving quantity through a numerical control system is sent to a servo driver corresponding to the motor, and each motor is controlled to drive a corresponding active joint, so that an end effector of the fruit sorting parallel robot mechanism is driven to realize expected movement.
Examples of the invention are provided below:
example 1
The invention mainly aims to improve the motion control performance of the fruit sorting parallel robot mechanism by considering the coupling action and adapting to the sliding mode control technology. A schematic block diagram of a coupling-considered adaptive sliding mode control principle of a fruit sorting parallel robot mechanism is shown in fig. 3, and the specific implementation manner of the control method is as follows:
1) the fruit sorting parallel robot mechanism is subjected to kinematic analysis to obtain the fruit sorting parallel robot mechanism position inverse solution equation and the Jacobian matrix
In FIG. 4, the midpoint of the stationary platform is selected to establish the reference coordinate system O-xyz, and the active platform is considered as the particle P1The auxiliary platform is regarded as particle P2Point P on the main (auxiliary) of the mobile platform1(P2) Is [ x, y, z ] is given as the position vector r]TCan be expressed as:
Figure BDA0002534743130000101
in which s is P1Point to P2Point distance (in m); e is vector OAiThe mold of (4); gamma rayiDenotes the structural angle (in rad/s) of the static platform, gammai＝(i-1)π/2；l1、l2、ui、wiRespectively expressed as the rod length (in m) and the unit vector of a driving arm and a driven arm in the ith branched chain; and is
Figure BDA0002534743130000102
ei＝e(cosγisinγ 0)T
ui＝(cosγicosθisinγicosθi-sinθi)T
In the formula, thetaiThe rotation angle of the driving arm i is the rotation angle of the driving joint (unit is rad).
Rewriting formula (25) to
Figure BDA0002534743130000103
And two sides are simultaneously dot-multiplied by respective transposing to obtain
Figure BDA0002534743130000104
Will uiSubstituted for formula (26) to give
Eisinθi+Ficosθi+Gi＝0 (27)
In the formula
Figure BDA0002534743130000105
Figure BDA0002534743130000106
Figure BDA0002534743130000107
Is provided with
Figure BDA0002534743130000108
Can obtain the product
(Gi-Fi)t2+2Eit+Fi+Gi＝0 (28)
The inverse solution equation of the position of the mechanism can be obtained as
Figure BDA0002534743130000111
In the formula (I), the compound is shown in the specification,
Figure BDA0002534743130000112
w is obtained from the following equations (25) and (29) as unit vectors of each coordinate axis of the coordinate system O-xyziComprises the following steps:
Figure BDA0002534743130000113
the equal sign of the formula (25) is derived from the time at the same time
Figure BDA0002534743130000114
In the formula (I), the compound is shown in the specification,
Figure BDA00025347431300001112
is a point P1Velocity vector (in m/s);
Figure BDA0002534743130000116
the angular velocity (unit is rad/s) of the driving arm in the ith branched chain; omegaiIs the angular velocity vector (in rad/s) of the driven arm in the ith branched chain; v isiDenotes perpendicular to eiAnd uiA unit vector stretched into a plane and having
vi＝(-sinγicosγi0)T
Multiplying both sides of equal sign of formula (30) by wi TIs obtained by
Figure BDA0002534743130000117
The equation (31) is rewritten into a matrix form to obtain a velocity model of the mechanism as
Figure BDA0002534743130000118
Figure BDA0002534743130000119
In the formula, JθIs a direct Jacobian matrix; j. the design is a squarexIs an indirect Jacobian matrix; j is a Jacobian matrix;
Figure BDA00025347431300001110
Figure BDA00025347431300001111
2) a kinetic model of the fruit sorting parallel robot mechanism is established by adopting a virtual work principle.
To build a fruit sorting parallel robot mechanism dynamics model, the following assumptions were made: 1. the kinematic pair has no energy dissipation caused by friction; 2. because the driven arm is a light rod, the mass of the driven arm is distributed to the movable platform and the driving arm according to the ratio of 1:2 by neglecting the moment of inertia.
According to the principle of work deficiency, the method can be obtained
Figure BDA0002534743130000121
Changing theta to Jr and thetasThe simplified rigid body dynamic model of the fruit sorting parallel robot mechanism is obtained by substituting (2 pi/p) s (p is a screw pitch) into formula (33)
τ＝τa+τv+τg(34)
Figure BDA0002534743130000122
τv＝IAf，τg＝τAg+τgplate
Figure BDA0002534743130000123
Figure BDA0002534743130000124
Wherein τ is a drive torque (unit is N.m) for driving the joint, and τ isaIs the term of inertia (in N.m), tau in the drive torquevIs a speed term (unit is N.m), taugIs a gravity term (unit is N.m), tauAgIs the gravitational moment (unit is N.m) of the driving arm (including the equivalent mass of the driven arm); m is1＝ms1+mscrew+2/3mrod+mloadAnd m2＝ms2+2/3mrodRespectively converted to P1And P2The mass (in kg) of the mass (c) of the cell,
Figure BDA0002534743130000126
is the moment of inertia (in kg. m) equivalent to the active arm to its axis of rotation2)，Is＝Iscrew+IloadIs the moment of inertia of the screw (unit kg.m)2)、mara＝marmrarm+2/3mrodl1Is the mass-diameter product (in kg.m) of the driving arm (including the equivalent mass of the driven arm), wherein m iss1And ms2The self mass (unit kg) and the m of the active platform and the auxiliary platform are respectivelyloadIs the load mass (unit is kg), marmThe mass (unit is kg) of the driving arm and marmrarmThe mass product (in kg. m) of the active arm to its axis of rotation.
3) Based on a fruit sorting parallel robot mechanism joint space dynamics model, a driving joint equivalent inertia calculation method considering coupling among branches is researched and is converted into equivalent load inertia of each motor for consideration.
In order to research and analyze the coupling between the branches of the parallel robot mechanism, a dynamic model of the parallel robot mechanism in joint space needs to be established, so that the dynamic model expressed by the formula (34) under the generalized coordinate is converted into the dynamic model of the joint space to obtain the dynamic model
Figure BDA0002534743130000125
In the formula
M(θ)＝J-TmJ-1+IA
Figure BDA0002534743130000131
G(θ)＝τgplate+τAg
Equation (35) represents the relationship between the drive joint torque and the mechanism position, velocity, acceleration. For a fruit sorting parallel robot mechanism, the inertia matrix M (theta) is a symmetrical positive definite matrix of 4 multiplied by 4. And if the coupling action among the branches is not considered, the equivalent inertia parameters of the driving joints are corresponding main diagonal elements in the inertia matrix. The strength of the coupling action can be described by the diagonal dominance characteristic of the inertia matrix, which is defined as: if the absolute value of each main diagonal element of the matrix is greater than the sum of the absolute values of the other elements of the row, the matrix is called 'strictly diagonal dominant'.
Figure BDA0002534743130000132
In order to consider the coupling effect among branches in the kinematic control, an inertia matrix Diagonal Dominant Index (DDI) is defined as
Figure BDA0002534743130000133
Because the inertia matrix of the fruit sorting parallel robot mechanism is a symmetrical positive definite matrix, main diagonal elements of the fruit sorting parallel robot mechanism are positive and meet strict diagonal dominance, DDI is greater than 1, and the larger the value is, the more obvious the diagonal dominance characteristic of the inertia matrix is. From the formula (37)
Figure BDA0002534743130000134
Where tr (M) is the trace of the inertia matrix.
The inertia term in the driving joint torque of the formula (35) can be expanded
Figure BDA0002534743130000135
Then there is
Figure BDA0002534743130000136
Obviously, must exist
Figure BDA0002534743130000141
And satisfy
Figure BDA0002534743130000142
So that
Figure BDA0002534743130000143
Can be combined with (38)
Figure BDA0002534743130000144
In summary, the equivalent inertia of the driving joint considering the coupling between the branches is
Figure BDA0002534743130000145
In the formula, λiIs the eigenvalue of the inertia matrix. So that the equivalent load inertia including the coupling inertia applied to the motor shaft can be expressed as
Figure BDA0002534743130000146
4) Design of sliding mode controller of fruit sorting parallel robot mechanism
Using fruit sorting parallel robot mechanism branch AC servo motor and driver as controlled object, and taking state variable x of system1＝θ、
Figure BDA00025347431300001413
Figure BDA00025347431300001412
The fruit sorting parallel robot mechanism branch mathematical model can be obtained by respectively representing the angular displacement, the angular velocity and the angular acceleration of a servo driving motor:
Figure BDA0002534743130000148
wherein x is [ x ]1,x2,x3]T∈RnRepresents a system state variable, wherein x1＝θ、
Figure BDA0002534743130000149
Respectively angular displacement (in rad), angular velocity (in rad/s) and angular acceleration (in rad/s) of the servo drive motor2) (ii) a u and y respectively represent system control input and output; function(s)
Figure BDA00025347431300001410
Function(s)
Figure BDA00025347431300001411
Wherein R isphIs stator winding resistance (unit is omega), KpiFor current loop gain, KiiAs a current feedback coefficient, KTIs a torque coefficient (unit is N.m/A), KETo induce an electromotive constant, KpvIs the speed loop gain, alphaTFor feedback coefficient of velocity measurement, LDIs the armature inductance (in units of H) and J' is the motor inertia parameter (in units of kg2) (ii) a d (t) represents an external perturbation.
Designing a sliding mode variable s as follows:
Figure BDA0002534743130000151
wherein e is θ - θd,
Figure BDA0002534743130000152
c1，c2Is an adjustable parameter and satisfies the Hall Woltz stable condition, so when the sliding mode variable s converges, the following error e also converges.
From the formulae (45) and (46)
Figure BDA0002534743130000153
Where ρ (t) is the lumped uncertainty term.
Figure BDA0002534743130000154
Then the sliding mode variable dynamic equation is
Figure BDA0002534743130000155
The control law u is designed as follows
u＝ueq+us(50)
In the formula, u is equivalently controlledeqFor maintaining the system in motion on the sliding surface, switching the control usFor driving the system trajectory to the slip form face. When in use
Figure BDA0002534743130000156
Time can deduce ueqThe following were used:
Figure BDA0002534743130000157
then there is
Figure BDA0002534743130000158
By substituting formula (52) for formula (49):
Figure BDA0002534743130000159
the control target can thus be converted into a feedback switching control law u designed according to the feedback controlsMaking the sliding mode variable s to zero or its domain. Switching control law usIs defined as follows
us＝-K(t)·sgn(s) (54)
In summary, the overall control law is
Figure BDA00025347431300001510
5) Aiming at the sliding mode control switching gain of the fruit sorting parallel robot mechanism, a self-adaptive law is designed, which can quickly adapt to uncertainty changes such as time variation of parallel robot mechanism parameters and external interference without uncertainty prior information and effectively inhibit buffeting caused by overestimation of sliding mode control switching gain;
an adaptation law is designed for the switching gain K (t)
Figure BDA0002534743130000161
Wherein α, β and μ are positive parameters; the parameter gamma is a positive integer.
6) Self-adaptive sliding mode controller considering coupling effect and formed by combining sliding mode control of fruit sorting parallel robot mechanism and self-adaptive law of switching gain
Aiming at a fruit sorting parallel robot mechanism, on the basis of a sliding mode control law u of the fruit sorting parallel robot mechanism, a self-adaptive law of a designed sliding mode control switching gain K (t) is combined to form the following self-adaptive sliding mode control law considering the coupling effect
Figure BDA0002534743130000162
7) And the fruit sorting parallel robot mechanism realizes the self-adaptive sliding mode control considering the coupling effect through software programming.
The fruit sorting parallel robot adopts a distributed control system of an upper computer (PC) and a lower computer (UMAC multi-axis motion controller), and the overall structural schematic diagram of the control system is shown in FIG. 5. The control system operation process: the upper computer (PC) finishes tasks such as system initialization, code compiling and the like, a serial port of the upper computer (PC) reads an actual collected pressure value in real time, an attitude adjusting instruction is sent to the UMAC controller in real time through an Ethernet port (Ethernet) according to an instruction requirement sent by a main control center, the UMAC processes related instructions in real time, differential pulse instruction control of a servo driver and reading of six paths of differential encoder information are achieved through an ACC-24E2A board card, then corresponding joints of the fruit sorting parallel robot are controlled to generate corresponding displacement and rotation at an instruction speed, finally, position and speed information of the active joints are fed back to the UMAC through an encoder, and a result is returned to the PC after the UMAC finishes a control function.
And (3) obtaining the drive control quantity of each motor of the fruit sorting parallel robot mechanism according to the formula (57), compiling a self-adaptive sliding mode control algorithm software program based on the consideration coupling effect of the fruit sorting parallel robot mechanism, sending the voltage analog quantity obtained by the digital/analog conversion of the drive quantity through a numerical control system to a servo driver corresponding to the motor, and controlling each motor to drive a corresponding active joint so as to drive the end executor of the fruit sorting parallel robot mechanism to realize expected movement.
By MATLAB simulation and a prototype system experiment of a fruit sorting parallel robot mechanism, comparing the control effects of the proposed fruit sorting parallel robot branch adaptive sliding mode control method (IEG-ASMC) considering the coupling effect with the control effects of a fixed switching gain Sliding Mode Control (SMC) and an exponential-free integral adaptive sliding mode control (IG-ASMC); and with the control effect of the coupling effect considered adaptive sliding mode control (IEG-ASMC) without considering the coupling effect, respectively obtaining a motor tracking trajectory curve of the fruit sorting parallel robot mechanism shown in fig. 6 under the inter-branch coupling and branch uncertainty conditions, a motor tracking error curve of the fruit sorting parallel robot mechanism shown in fig. 7 under the inter-branch coupling and branch uncertainty conditions, and a controller output curve of the fruit sorting parallel robot mechanism shown in fig. 8 under the inter-branch coupling and branch uncertainty conditions; and the fruit sorting parallel robot mechanism shown in fig. 9 outputs a motor tracking curve under the inter-branch coupling condition, the fruit sorting parallel robot mechanism shown in fig. 10 outputs a motor tracking error curve under the inter-branch coupling condition, and the fruit sorting parallel robot mechanism shown in fig. 11 outputs a curve under the inter-branch coupling condition.
As can be seen from fig. 6 and 7, under the condition that the system has coupling between branches and uncertainty in the branches, the adaptive sliding mode control method considering the coupling effect provided by the present invention can enable the system to have higher tracking accuracy. Fig. 8 shows that the adaptive sliding mode control method considering the coupling effect provided by the invention can effectively weaken the buffeting control of the sliding mode. As can be seen from fig. 9 and 10, in the case of inter-branch coupling, the adaptive sliding mode control method considering coupling effect according to the present invention has higher tracking accuracy than the adaptive sliding mode control method not considering inter-branch coupling. Fig. 11 shows that the proposed coupling-effect-considered adaptive sliding mode control method of the present invention has relatively small output jitter compared to an adaptive sliding mode control method that does not consider inter-branch coupling.
In conclusion, the fruit sorting parallel robot mechanism provided by the invention considers the coupling effect and is self-adaptive to the sliding mode control method. Obviously, under the problems of inter-branch coupling effect and branch uncertainty of the fruit sorting parallel robot mechanism, the influence of the inter-branch coupling effect and the branch uncertainty can be overcome, the sliding mode control buffeting can be effectively inhibited, and finally the high-performance kinematic control of the fruit sorting parallel robot mechanism is realized.
It should be understood that the above-described embodiments are illustrative only and are not limiting upon the scope of the invention, which is to be given the full breadth of the appended claims and any and all equivalent modifications thereto that may occur to those skilled in the art upon reading the present disclosure.

Claims (5)

1. A fruit sorting parallel robot mechanism considering coupling action self-adaptive sliding mode control method is characterized by comprising the following steps:
1) the fruit sorting parallel robot mechanism is subjected to kinematic analysis, and a fruit sorting parallel robot mechanism position inverse solution equation and a Jacobian matrix are obtained;
2) establishing a spatial dynamic model of the joint of the fruit sorting parallel robot mechanism by adopting a virtual work principle;
3) based on the spatial dynamics model of the joint of the fruit sorting parallel robot mechanism in the step 2), researching a driving joint equivalent inertia calculation method considering coupling among branches, and converting the driving joint equivalent inertia into equivalent load inertia of each motor for consideration;
4) combining the equivalent load inertia of each motor in the step 3), solving a single-branch mathematical model of the fruit sorting parallel robot mechanism, and designing a sliding mode controller;
5) through the fruit sorting parallel robot mechanism sliding mode control switching gain obtained in the step 4), an adaptive law is designed, wherein the adaptive law can quickly adapt to uncertainty changes such as parallel robot mechanism parameter time variation and external interference under the condition of no uncertainty prior information, and meanwhile buffeting caused by overestimation of sliding mode control switching gain is effectively inhibited;
6) a fruit sorting parallel robot mechanism is formed based on the step 4) and the step 5), and a coupling effect is considered, so that the self-adaptive sliding mode controller is adopted;
7) and the fruit sorting parallel robot mechanism realizes the self-adaptive sliding mode control considering the coupling effect through software programming.
2. The fruit sorting parallel robot mechanism coupling-considered adaptive sliding mode control method according to claim 1, characterized in that: in the step 3), the equivalent inertia of the driving joint coupled among the branches is considered
Figure FDA0002534743120000011
Comprises the following steps:
Figure FDA0002534743120000012
in the formula, DDI is an inertia matrix diagonal dominance characteristic index (DDI); tr (M) is the trace of the inertia matrix; lambda [ alpha ]iCharacteristic values of the inertia matrix;
equivalent load inertia J including coupling inertia applied to motor shaftDDICan be expressed as
Figure FDA0002534743120000013
In the formula, n is a reduction ratio.
3. The fruit sorting parallel robot mechanism coupling-considered adaptive sliding mode control method according to claim 1, characterized in that: in the step 4), the fruit sorting parallel robot mechanism single branch mathematical model is
Figure FDA0002534743120000021
Wherein x is [ x ]1,x2,x3]T∈RnRepresents a system state variable, wherein x1＝θ、
Figure FDA0002534743120000022
Angular displacement, angular velocity and angular acceleration of the servo drive motor respectively; u and y respectively represent system control input and output; function f (x) ═ f
Figure FDA0002534743120000023
Function(s)
Figure FDA0002534743120000024
Wherein R isphIs stator winding resistance, KpiFor current loop gain, KiiAs a current feedback coefficient, KTIs the torque coefficient, KETo induce an electromotive constant, KpvIs the speed loop gain, alphaTFor feedback coefficient of velocity measurement, LDIs armature inductance, and J' is motor inertia parameter; d (t) represents an external perturbation;
fruit sorting parallel robot mechanism sliding mode control law is
Figure FDA0002534743120000025
In the formula ueqEquivalent control is adopted; u. ofsFor switching control;
Figure FDA0002534743120000026
is a sliding mode variable; e-thetad，
Figure FDA0002534743120000027
And
Figure FDA0002534743120000028
respectively an angular displacement error, an angular velocity error and an angular acceleration error of the servo drive motor, wherein theta,
Figure FDA0002534743120000029
And
Figure FDA00025347431200000210
for angular displacement, angular velocity and angular acceleration, theta, of the servo-drive motord、
Figure FDA00025347431200000211
And
Figure FDA00025347431200000212
a desired angular displacement, a desired angular velocity, and a desired angular acceleration for the servo drive motor; c. C1And c2Is an adjustable parameter and meets the Hall Woltz stable condition;
Figure FDA00025347431200000213
and
Figure FDA00025347431200000214
nominal values for functions f (x) and g (x), respectively; k (t) is the switching gain.
4. The fruit sorting parallel robot mechanism coupling-considered adaptive sliding mode control method according to claim 1, characterized in that: in the step 5), the slip form control switching gain self-adaption law of the designed fruit sorting parallel robot mechanism is
Figure FDA00025347431200000215
Wherein α, β and μ are positive parameters; the parameter gamma is a positive integer.
5. The fruit sorting parallel robot mechanism coupling-considered adaptive sliding mode control method according to claim 1, characterized in that: in the step 6), the sliding mode control law u and the switching gain self-adaptive law K are combined, and the self-adaptive sliding mode control law of the designed fruit sorting parallel robot mechanism is
Figure FDA0002534743120000031
CN202010529155.XA 2020-06-11 2020-06-11 Self-adaptive sliding mode control method for fruit sorting parallel robot mechanism considering coupling effect Pending CN111805536A (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010529155.XA CN111805536A (en) 2020-06-11 2020-06-11 Self-adaptive sliding mode control method for fruit sorting parallel robot mechanism considering coupling effect

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010529155.XA CN111805536A (en) 2020-06-11 2020-06-11 Self-adaptive sliding mode control method for fruit sorting parallel robot mechanism considering coupling effect

Publications (1)

Publication Number Publication Date
CN111805536A true CN111805536A (en) 2020-10-23

Family

ID=72845840

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010529155.XA Pending CN111805536A (en) 2020-06-11 2020-06-11 Self-adaptive sliding mode control method for fruit sorting parallel robot mechanism considering coupling effect

Country Status (1)

Country Link
CN (1) CN111805536A (en)

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112847372A (en) * 2021-01-12 2021-05-28 河南科技大学 Delta robot control system based on PMAC
CN113787519A (en) * 2021-09-08 2021-12-14 伯朗特机器人股份有限公司 Delta type parallel robot design method based on complete dynamic model
CN115488876A (en) * 2022-06-22 2022-12-20 湖北商贸学院 Robot sorting method and device based on machine vision

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
王哲等: "4-R(2-SS）并联机器人机构积分指数自适应滑模控制研究", 《自动化与仪表》 *

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112847372A (en) * 2021-01-12 2021-05-28 河南科技大学 Delta robot control system based on PMAC
CN113787519A (en) * 2021-09-08 2021-12-14 伯朗特机器人股份有限公司 Delta type parallel robot design method based on complete dynamic model
CN113787519B (en) * 2021-09-08 2022-12-06 伯朗特机器人股份有限公司 Delta type parallel robot design method based on complete dynamic model
CN115488876A (en) * 2022-06-22 2022-12-20 湖北商贸学院 Robot sorting method and device based on machine vision

Similar Documents

Publication Publication Date Title
CN111805536A (en) Self-adaptive sliding mode control method for fruit sorting parallel robot mechanism considering coupling effect
CN110007594B (en) Self-adaptive robust sliding mode control method of series-parallel robot for automobile electrophoretic coating and conveying
Yen et al. Dynamic modeling and control of a 3-DOF Cartesian parallel manipulator
CN108227490A (en) A kind of model-free adaption sliding-mode control of New-type mixed-coupled formula automobile electrophoretic coating conveyor structure
CN114536334A (en) High-order sliding mode anti-interference control method for flexible mechanical arm system
CN110597051A (en) Stewart stable platform control method based on RBF neural network
Jun-Pei et al. Neural network control of space manipulator based on dynamic model and disturbance observer
Bai et al. Robust control of a spherical motor in moving frame
Wu et al. A study on tracking error based on mechatronics model of a 5-DOF hybrid spray-painting robot
Ye et al. Finite-time stable robust sliding mode dynamic control for parallel robots
You et al. Dynamic control of a 3-DOF cable-driven robot based on backstepping technique
CN116252300A (en) Tracking control method and equipment for rope-driven snake-shaped flexible arm based on friction force compensation
Lima-Pérez et al. Robust control of a two-wheeled self-balancing mobile robot
CN111687832B (en) System and method for controlling inverse priority impedance of redundant mechanical arm of space manipulator
Seraji Motion control of mobile manipulators
CN111687834B (en) System and method for controlling reverse priority impedance of redundant mechanical arm of mobile mechanical arm
Shi et al. A smooth sliding mode controller using a state observer for position control of the Stewart-Gough Platform
Jung et al. The operational space formulation on humanoids considering joint stiffness and bandwidth limit
Meng et al. Adaptive vector sliding mode fault-tolerant control of the uncertain Stewart platform based on position measurements only
Kim et al. Concurrent control of position/orientation of a redundant manipulator based on virtual spring-damper hypothesis
Zouari et al. Comparative study between PI and sliding mode controllers for flexible joint manipulator driving by brushless DC motor
CN111293949A (en) Control method of anti-interference electric six-degree-of-freedom parallel mechanism
CN117697767B (en) Space station elastic joint space mechanical arm control method and device
Bai et al. Research of the torque compensation method for the vibration suppression of the industrial robot
Song et al. Joint torque detection based on motor current and singular perturbation control for cleaning room 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
RJ01 Rejection of invention patent application after publication
RJ01 Rejection of invention patent application after publication

Application publication date: 20201023