CN114840947A - Three-degree-of-freedom mechanical arm dynamic model with constraint - Google Patents

Three-degree-of-freedom mechanical arm dynamic model with constraint Download PDF

Info

Publication number
CN114840947A
CN114840947A CN202210569131.6A CN202210569131A CN114840947A CN 114840947 A CN114840947 A CN 114840947A CN 202210569131 A CN202210569131 A CN 202210569131A CN 114840947 A CN114840947 A CN 114840947A
Authority
CN
China
Prior art keywords
mechanical arm
particle
optimal
particles
dynamics model
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.)
Withdrawn
Application number
CN202210569131.6A
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.)
Qingdao University of Science and Technology
Original Assignee
Qingdao University of Science and Technology
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 Qingdao University of Science and Technology filed Critical Qingdao University of Science and Technology
Priority to CN202210569131.6A priority Critical patent/CN114840947A/en
Publication of CN114840947A publication Critical patent/CN114840947A/en
Withdrawn legal-status Critical Current

Links

Images

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F30/00—Computer-aided design [CAD]
    • G06F30/10—Geometric CAD
    • G06F30/17—Mechanical parametric or variational design
    • 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
    • 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
    • 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
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F30/00—Computer-aided design [CAD]
    • G06F30/20—Design optimisation, verification or simulation
    • G06F30/27—Design optimisation, verification or simulation using machine learning, e.g. artificial intelligence, neural networks, support vector machines [SVM] or training a model
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06N—COMPUTING ARRANGEMENTS BASED ON SPECIFIC COMPUTATIONAL MODELS
    • G06N3/00—Computing arrangements based on biological models
    • G06N3/004—Artificial life, i.e. computing arrangements simulating life
    • G06N3/006—Artificial life, i.e. computing arrangements simulating life based on simulated virtual individual or collective life forms, e.g. social simulations or particle swarm optimisation [PSO]
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F2111/00—Details relating to CAD techniques
    • G06F2111/04—Constraint-based CAD
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F2111/00—Details relating to CAD techniques
    • G06F2111/08—Probabilistic or stochastic CAD
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F2119/00—Details relating to the type or aim of the analysis or the optimisation
    • G06F2119/14—Force analysis or force optimisation, e.g. static or dynamic forces
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T90/00—Enabling technologies or technologies with a potential or indirect contribution to GHG emissions mitigation

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • Theoretical Computer Science (AREA)
  • Geometry (AREA)
  • General Physics & Mathematics (AREA)
  • Evolutionary Computation (AREA)
  • General Engineering & Computer Science (AREA)
  • Robotics (AREA)
  • Mechanical Engineering (AREA)
  • Software Systems (AREA)
  • Artificial Intelligence (AREA)
  • Computer Hardware Design (AREA)
  • Health & Medical Sciences (AREA)
  • General Health & Medical Sciences (AREA)
  • Mathematical Optimization (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Mathematical Analysis (AREA)
  • Biomedical Technology (AREA)
  • Biophysics (AREA)
  • Computational Linguistics (AREA)
  • Data Mining & Analysis (AREA)
  • Pure & Applied Mathematics (AREA)
  • Molecular Biology (AREA)
  • Computing Systems (AREA)
  • Mathematical Physics (AREA)
  • Computational Mathematics (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Medical Informatics (AREA)
  • Feedback Control In General (AREA)

Abstract

The invention relates to a three-degree-of-freedom mechanical arm dynamics model with constraint, which is established by utilizing an Euler-Lagrange method, gives a dynamic equation expression form of a state space of the mechanical arm dynamics model, and calculates the motion planning of the mechanical arm dynamics model based on a particle swarm algorithm. The invention has the capability of quick response; accurate position deviation can be obtained; the control method has stronger robustness and adaptivity; the speed regulation range and the torque output range are large enough.

Description

Three-degree-of-freedom mechanical arm dynamic model with constraint
Technical Field
The invention relates to the technical field of industrial robots, in particular to a three-degree-of-freedom mechanical arm dynamic model with constraints.
Background
The mechanical arm is widely used in production and life, can efficiently complete various complex and dangerous operations as electromechanical integrated equipment, improves the production efficiency, and is widely applied to the fields of industry, aerospace, medical treatment and the like. The mechanical arm motion control is essentially a multi-axis position follow-up system for controlling the motion of a mechanical arm joint, the controlled quantity (output quantity) of the multi-axis position follow-up system is the angular displacement of the spatial position of a load machine, and when the given quantity (input quantity) of the position is changed for a specific task, the system can enable the output quantity to quickly and accurately track the change of the given quantity. Therefore, the mechanical arm motion control system comprises a track planning part and a track tracking part, and the task of the mechanical arm motion control system is to plan the walking track of the mechanical arm and carry out closed-loop control on the angular displacement, the angular velocity and the angular acceleration of the servo driving mechanism of each axis of the mechanical arm so as to realize the integral pose combined control of the mechanical arm. The mechanical arm track planning is a generation method for calculating a track according to task requirements and researching the displacement, the angular velocity and the angular acceleration of a mechanical arm joint in motion. These trajectories can be generated in joint space or cartesian space. The trajectory planning is carried out in the joint space, the motion of the mechanical arm is described by using joint variables, the real-time performance is good, but the postures of a connecting rod and an end effector are difficult to determine; and (3) planning the track in the Cartesian space, namely mapping the path constraint into joint space coordinates, and then determining the joint track meeting the parameter constraint. In comparison, the track planning calculation amount in the joint space is small, the real-time performance is high, and the singular point phenomenon can be avoided.
In the prior art, the influence of the model on the control precision is partially researched from the accuracy of the model and the uncertainty of the model parameters, such as a model uncertainty mechanical arm motion control method based on a multilayer neural network and a flexible control method and system of a multi-degree-of-freedom mechanical arm; some researches are only carried out on whether the track design is reasonable, such as 'a mechanical arm multipoint motion track planning method' and the like, but in the track planning process, the discussion of a track function meeting the specific task requirements and constraint conditions thereof needs to be further refined; some patents specially study the design of the trajectory tracking controller, such as "a trajectory tracking algorithm for mechanical arm decentralized neural robust control", these patents do not consider the factors of trajectory rationality, ignore the influence of the inertia force of the mechanical arm, reduce the applicability thereof, and meanwhile, need to be further improved in the aspects of tracking accuracy and system stability.
Disclosure of Invention
In order to overcome the defects of the prior art, the invention designs a reasonable three-degree-of-freedom mechanical arm dynamic model with constraints, and the model has the following four basic characteristics: the device has the capability of quick response; accurate position deviation can be obtained; the control method has stronger robustness and adaptivity; the speed regulation range and the torque output range are large enough.
The technical scheme of the invention is as follows: a three-degree-of-freedom mechanical arm dynamics model with constraint is established by utilizing an Euler-Lagrange method, a dynamics equation expression form of a state space of the mechanical arm dynamics model is given, and a motion planning of the mechanical arm dynamics model is calculated based on a particle swarm algorithm, wherein the dynamics equation of the mechanical arm dynamics model is as follows:
Figure BDA0003659500420000021
wherein: q ═ q 1 ,q 2 ,q 3 ] T Is the angular vector of the system; τ ═ τ [ τ ] 1 ,τ 2 ,τ 3 ] T Is a control moment vector; m (q) epsilon R 3×3 Is an inertia matrix with positive nature and symmetry;
Figure BDA0003659500420000022
is the combined vector of the coriolis force and the centrifugal force;
the state space form of the mechanical arm dynamics model is:
Figure BDA0003659500420000031
wherein:
f(x)＝[x 4 ,x 5 ,x 6 ,f 1 ,f 2 ,f 3 ]
g(x)＝[g 1(x) ,g 2(x) ,g 3(x) ] T
Figure BDA0003659500420000032
x is a state vector variable, f (x) is a state objective function, g (x) is a non-linear constrained correlation function with respect to x, g 1(x) Is a zero matrix of order 3, g ij Is a component function of (a) with respect to g, (x); order to
Figure BDA0003659500420000033
Then there are:
Figure BDA0003659500420000034
said M (q) and
Figure BDA0003659500420000035
are in the form of:
Figure BDA0003659500420000036
Figure BDA0003659500420000041
Figure BDA0003659500420000042
Figure BDA0003659500420000043
the particle swarm optimization algorithm is a random optimization algorithm based on a population, each individual in the population is called a particle, the size of the population is N, and the position vector of each particle in a D-dimensional space is X i ＝(x i1 ,x i2 ,x i3 ,x i4 ,...,x id ,...,x iD ) (ii) a Velocity vector is V i ＝(v i1 ,v i2 ,v i3 ,v i4 ,...,v id ,...,v iD ) (ii) a The optimal position of the individual is P i ＝(p i1 ,p i2 ,p i3 ,p i4 ,...,p id ,...,p iD ) (ii) a The best position of the population is denoted P g ＝(p g1 ,p g2 ,p g3 ,p g4 ,...,p gd ,...,p gD ) The updating model of the individual optimal position is as follows:
Figure BDA0003659500420000044
the optimal position of the population is the optimal position of all individuals, and the updated model of the velocity and position is shown as follows:
Figure BDA0003659500420000051
wherein, ω is an inertia weight, which determines the convergence rate of the algorithm. The larger ω is, the stronger the convergence speed is, and vice versa, the weaker the convergence speed is. Experiments prove that the particle swarm algorithm with omega between 0.8 and 1.2 has faster convergence rate. c. C 1 And c 2 Is a learning factor, also called acceleration constant; r is 1 And r 2 Is [0,1 ]]A uniform random number within the range.
The time optimal particle swarm planning under multiple constraints comprises the following steps:
(1) determining the dimension of particles, determining the initial population number of the particle swarm, initializing the position and the speed of the particles, and setting an optimal fitness value;
(2) setting a fitness function, preliminarily screening particles with better fitness values, and calculating coefficients of a polynomial;
(3) calculating joint velocity and acceleration tracks by the polynomial coefficients, and checking whether the set maximum limit values are exceeded, namely, the formula (7) and the formula (8) are satisfied;
(4) and traversing all the particles, calculating the fitness value of each particle, checking whether the fitness value of each time is smaller than the updated optimal fitness value, and simultaneously checking whether the joint velocity and acceleration tracks corresponding to the polynomial coefficients calculated by the particles have values exceeding the limit. If not, updating the optimal particles and updating the optimal fitness value; if yes, skipping the particle, and detecting the next particle until all the particles are detected;
(5) comparing each particle with the updated fitness value of the optimal particle to obtain the current overall optimal particle, then comparing the current overall optimal particle with the global optimal particle experienced by the group, and if the fitness value is smaller, updating the global optimal particle and the optimal fitness value;
(6) and iterating until the number of termination times, stopping updating, obtaining a corresponding polynomial coefficient according to the global optimal particles, and calculating corresponding joint angles, angular velocities and angular acceleration tracks.
The invention has the beneficial effects that:
1) the model has the rapid response capability;
2) the model of the invention can obtain accurate position deviation;
3) the model control method has stronger robustness and adaptivity;
4) the model speed regulation range and the torque output range are large enough.
Description of the drawings:
FIG. 1 is a schematic diagram of the structure of a system embodiment of the present invention;
FIG. 2 is a three joint angular trajectory diagram of the present invention;
FIG. 3 is a flow chart of the particle swarm algorithm in the trajectory planning method of the present invention;
FIG. 4 is a three joint angular velocity trajectory diagram of the present invention;
FIG. 5 is a three joint acceleration trace diagram of the present invention;
FIG. 6 is a graph of the three joint moments of the present invention;
FIG. 7 is a graph of three joint trajectory tracking error of the present invention.
The specific implementation mode is as follows:
referring to fig. 1 to 7, the invention relates to a three-degree-of-freedom mechanical arm dynamics model with constraints, which is established by using an euler-lagrange method, a state space expression form of the mechanical arm dynamics model is given, a motion plan of the mechanical arm dynamics model is calculated based on a particle swarm algorithm, and a dynamics equation of the mechanical arm dynamics model is as follows:
Figure BDA0003659500420000061
wherein: q ═ q 1 ,q 2 ,q 3 ] T Is the angular vector of the system; τ ═ τ [ τ ] 1 ,0,τ 3 ] T Is a control moment vector; m (q) epsilon R 3 ×3 Is an inertia matrix with positive nature and symmetry;
Figure BDA0003659500420000062
is the combined vector of the coriolis force and the centrifugal force.
The state space form of the system model is then:
Figure BDA0003659500420000071
wherein:
f(x)＝[x 4 ,x 5 ,x 6 ,f 1 ,f 2 ,f 3 ]
g(x)＝[g 1(x) ,g 2(x) ,g 3(x) ] T
Figure BDA0003659500420000072
x is a state vector variable, f (x) is a state objective function, g (x) is a non-linear constrained correlation function with respect to x, g 1(x) Is a zero matrix of order 3, g ij Is a component function of (x) with respect to g; order to
Figure BDA0003659500420000073
Then there are:
Figure BDA0003659500420000074
preferably, said M (q) and
Figure BDA0003659500420000075
are in the form of:
Figure BDA0003659500420000076
Figure BDA0003659500420000081
Figure BDA0003659500420000082
Figure BDA0003659500420000083
the particle swarm optimization algorithm is a random optimization algorithm based on a population, each individual in the population is called a particle, the size of the population is N, and the position vector of each particle in a D-dimensional space is X i ＝(x i1 ,x i2 ,x i3 ,x i4 ,...,x id ,...,x iD ) (ii) a Velocity vector is V i ＝(v i1 ,v i2 ,v i3 ,v i4 ,...,v id ,...,v iD ) (ii) a The optimal position of the individual is P i ＝(p i1 ,p i2 ,p i3 ,p i4 ,...,p id ,...,p iD ) (ii) a The optimal position of the population is denoted as P g ＝(p g1 ,p g2 ,p g3 ,p g4 ,...,p gd ,...,p gD ) The updating model of the individual optimal position is as follows:
Figure BDA0003659500420000084
the optimal position of the population is the optimal position of all individuals, and the updated model of the velocity and position is shown as follows:
Figure BDA0003659500420000091
wherein, ω is an inertia weight, which determines the convergence rate of the algorithm. The larger ω is, the stronger the convergence speed is, and vice versa, the weaker the convergence speed is. Experiments prove that the particle swarm algorithm with omega between 0.8 and 1.2 has faster convergence rate. c. C 1 And c 2 Is a learning factor, also called acceleration constant; r is 1 And r 2 Is [0,1 ]]A uniform random number within the range.
The invention designs a track tracking controller according to the constraint relation between the connecting rods of the mechanical arm system.
The sliding mode surface is constructed according to the formula:
Figure BDA0003659500420000092
derivation of the sliding mode surface can give the formula:
Figure BDA0003659500420000093
Figure BDA0003659500420000094
Figure BDA0003659500420000095
wherein:
Figure BDA0003659500420000096
β 1 ，β 2 ，β 3 is a constant.
Designing a sliding mode approach law as a formula:
Figure BDA0003659500420000097
wherein
Figure BDA0003659500420000098
k i ＞1，η i ＞0，ζ i > 0, double power approximation law by | S i The system is divided into two stages by 1:
the first stage is as follows: when | S i The system function mainly depends on the former item when | < 1
Figure BDA0003659500420000101
At this time
Figure BDA0003659500420000102
The larger the approach speed is, the faster the approach speed is, but the buffeting is correspondingly increased, eta i The smaller the system speed and buffeting are simultaneously reduced.
And a second stage: when | S i With 1, the system function depends mainly on the second term
Figure BDA0003659500420000103
k i The larger the approach speed is, the faster the approach speed is, and the buffeting is increased; zeta i The smaller the buffeting and velocity become simultaneously smaller. Therefore, the use of this method requires the selection of appropriate parameters according to experiments. Meanwhile, a boundary layer is introduced to control the system state within a certain range, and a saturation function is used for defining, wherein the selected saturation function is a formula:
Figure BDA0003659500420000104
wherein, in order to ensure the stability of the system, the Gamma is more than 1 and less than 2. At this time, the overall approach law of the system is as follows:
Figure BDA0003659500420000105
the control law τ ═ τ [ τ ] of each joint can be obtained from the above formula 1 ,τ 2 ,τ 3 ] Τ 。
The following setting conditions are given:
|v i,j |≤1.57rad/s
|a i,j |≤2.09rad/s 2
the starting points of the joints 1,2,3 and the joint vectors of the three fixed points are:
q 1 ＝[0.50,1.54,0.32,-0.55]rad，
q 2 ＝[-1.20,-1.51,-1.02,0.2798]rad，
q 3 ＝[1.70,0.04,-2.20,-1.09]rad；
the number of iterations of the particle swarm algorithm was 200.
The angles, angular velocities and angular acceleration traces of the three joints are obtained as shown in fig. 3-5. The structure block diagram of the particle swarm optimization-based three-five-three combined polynomial interpolation method adopted by the patent is shown in fig. 2.
The invention utilizes a cubic polynomial function to plan the track for every two fixed points.
Firstly, determining the starting point of the mechanical arm and the spatial positions of three fixed points in a Cartesian space, and then obtaining the corresponding joint positions through an inverse kinematics model of the mechanical arm. Because the track has a starting point and three path points, three sections of track interpolation are provided. The following parameters were set: i is 1,2,3 represents the number of joints; j is 1,2,3 to indicate the number of interpolation track segments; theta i,j The interpolated angle vector representing the ith joint.
Establishing a 3-5-3 polynomial function of the ith joint angle with respect to time as shown in equation (3):
Figure BDA0003659500420000111
wherein l i,1 (t) a first segment cubic polynomial interpolation trajectory representing the ith joint angle; l i,2 (t) a second fifth polynomial interpolation trajectory for the ith joint angle; l i,3 (t) a third-stage cubic polynomial interpolation trajectory representing the ith joint angle; coefficient a i Is the correlation coefficient corresponding to the polynomial.
Let coefficient a of multiple form i Is given as vector a ═ a i,13 ,a i,12 ,a i,11 ,a i,10 ,a i,25 ,...,a i,20 ,a i,33 ,...,a i,30 ] Τ If the time matrix of the polynomial function is T, the corresponding matrix is solved as T 14×14 a 14×1 ＝θ 14×1 Wherein T and θ are shown in equations (4) and (5). When the initial velocity and acceleration of the joint are consistent with the velocity and acceleration of the joint at the tail end of the first section of track during the second section of track planning, the first section of track is required to be completed before the second section of fifth-order polynomial interpolation track planning. In the time matrix T, each row corresponds to an element of the column vector a: one to three lines represent the interpolation of the first section of cubic polynomial, and the track of the initial time corresponding to the second section of cubic polynomial corresponding to the initial time needs to be eliminated, and the same principle is applied from four lines to nine lines.
Figure BDA0003659500420000121
θ＝[0,0,0,0,0,0,x 3 ,0,0,x 0 ,0,0,x 2 ,x 1 ] Τ
Wherein x is 3 Corresponding to the seventh line of T and the seventh element of a, representing the displacement of the third-stage cubic polynomial interpolation trajectory plan; x is the number of 0 Initial displacement representing a first-stage cubic polynomial interpolation plan with time 0；x 2 Representing the initial displacement of the third-stage cubic polynomial interpolation program when the time is 0; x is the number of 1 Representing the initial displacement of the second fifth order polynomial interpolation plan at time 0.
The invention designs a combined trajectory planning method based on time optimization under multiple constraint conditions, and performs time optimization planning on trajectories among fixed path points. The specific scheme is as follows:
for the multi-fixed-path-point trajectory planning task, the optimal time index is the minimum sum of the multiple sections of time, and then the fitness function is defined as the following formula (6):
f(t)＝(t 1 +t 2 +t 3 ) min
due to various reasons such as the structure of a mechanical arm system, the material structure and the type selection of a motor, the mechanical arm has limits of joint speed and acceleration. If such limiting factors are not fully considered in the trajectory planning, the obtained trajectory usually has the phenomenon of rapid change of speed and acceleration within a short time, and does not accord with the actual working condition of the mechanical arm. The unstable track not only can generate potential safety hazard to the surrounding environment in the actual execution process, but also can generate irreversible influence or even damage to the system structure of the mechanical arm, and meanwhile, the requirement on track tracking control is higher, and the control difficulty is higher. In order to ensure the safety and the accuracy of the task and simultaneously meet the actual requirements of the mechanical arm system, double constraints of speed and acceleration are introduced into the trajectory planning task. Setting a speed constraint:
Figure BDA0003659500420000131
wherein v is i,j Velocity, V, of the j-th track representing the i-th joint i,j The maximum limit of the velocity of the j-th track of the i-th joint. And (3) setting acceleration restraint:
Figure BDA0003659500420000132
wherein a is i,j Acceleration of the j-th segment of the trajectory representing the i-th joint aa i,j The acceleration maximum limit of the j section track of the i joint.
The time optimal particle swarm planning under multiple constraints comprises the following steps:
(1) determining the dimension of particles, determining the initial population number of the particle swarm, initializing the position and the speed of the particles, and setting an optimal fitness value;
(2) setting a fitness function, preliminarily screening particles with better fitness values, and calculating coefficients of a polynomial;
(3) calculating joint velocity and acceleration tracks by the polynomial coefficients, and checking whether the set maximum limit values are exceeded, namely, the formula (7) and the formula (8) are satisfied;
(4) and traversing all the particles, calculating the fitness value of each particle, checking whether the fitness value of each time is smaller than the updated optimal fitness value, and simultaneously checking whether the joint velocity and acceleration tracks corresponding to the polynomial coefficients calculated by the particles have values exceeding the limit. If not, updating the optimal particles and updating the optimal fitness value; if yes, skipping the particle, and detecting the next particle until all the particles are detected;
(5) comparing each particle with the updated fitness value of the optimal particle to obtain the current overall optimal particle, then comparing the current overall optimal particle with the global optimal particle experienced by the group, and if the fitness value is smaller, updating the global optimal particle and the optimal fitness value;
(6) and iterating until the number of termination times, stopping updating, obtaining a corresponding polynomial coefficient according to the global optimal particles, and calculating corresponding joint angles, angular velocities and angular acceleration tracks.
As can be seen from the experimental results shown in fig. 3 to 5: the angular velocity and the angular acceleration track of each joint do not exceed the maximum absolute value, and the joint track meets all constraint conditions. The joint angular velocity track and the joint angular acceleration track are closer to the limited maximum value in the second section of track, which shows that the whole second section of track of the joint is accelerated and converged. In addition, because the cubic polynomial interpolation method and the quintic polynomial interpolation method require time to be set, if the time is improperly set, the joint speed and the acceleration are unstable, the system stability is reduced, and the task completion efficiency is low or even cannot be completed. According to the particle swarm optimization-based trajectory planning method, on the premise that the shortest time is guaranteed, the smoothness and the continuity of the angle, the angular velocity and the angular acceleration trajectory of each joint can be guaranteed, and the efficiency and the accuracy of task completion are improved.
For the expected track given in fig. 3, the controller of the improved synovial membrane is used for tracking control, and the obtained control law is shown in fig. 6, and the track tracking error curve is shown in fig. 7.
It can be seen from fig. 6 that the torque obtained by the sliding mode control of the double power saturation approach law is relatively stable and has no obvious buffeting, so that the output of the motor can be kept within the rated power, and the precision of the mechanical arm in executing tasks is ensured.
As can be seen from FIG. 7, the tracking errors of the three joints are small, the maximum amplitude is 0.015rad, and the tracking errors converge to be close to 0 within a certain time, so that the tracking precision is high.

Claims (4)

1. A three-degree-of-freedom mechanical arm dynamics model with constraint is established by utilizing an Euler-Lagrange method, a dynamics equation expression form of a state space of the mechanical arm dynamics model is given, and a motion plan of the mechanical arm dynamics model is calculated based on a particle swarm algorithm, and the three-degree-of-freedom mechanical arm dynamics model with constraint is characterized in that: the kinetic equation of the mechanical arm kinetic model is as follows:
Figure FDA0003659500410000011
wherein: q ═ q 1 ,q 2 ,q 3 ] T Is the angular vector of the system; τ ═ τ [ τ ] 1 ,τ 2 ,τ 3 ] T Is a control moment vector; m (q) epsilon R 3×3 Is an inertia matrix with positive nature and symmetry;
Figure FDA0003659500410000012
is the combined vector of the coriolis force and the centrifugal force;
the state space form of the mechanical arm dynamics model is as follows:
Figure FDA0003659500410000013
wherein:
f(x)＝[x 4 ,x 5 ,x 6 ,f 1 ,f 2 ,f 3 ]
g(x)＝[g 1(x) ,g 2(x) ,g 3(x) ] T
Figure FDA0003659500410000014
x is a state vector variable, f (x) is a state objective function, g (x) is a non-linear constrained correlation function with respect to x, g 1(x) Is a zero matrix of order 3, g ij Is a component function of (x) with respect to g; order to
Figure FDA0003659500410000015
Then there are:
Figure FDA0003659500410000021
2. the three-degree-of-freedom mechanical arm dynamics model with constraints of claim 1, wherein:
said M (q) and
Figure FDA0003659500410000022
are in the form of:
Figure FDA0003659500410000023
Figure FDA0003659500410000024
Figure FDA0003659500410000025
Figure FDA0003659500410000031
3. the three-degree-of-freedom mechanical arm dynamics model with constraints of claim 2, wherein: the particle swarm optimization algorithm is a random optimization algorithm based on a population, each individual in the population is called a particle, the size of the population is N, and the position vector of each particle in a D-dimensional space is X i ＝(x i1 ,x i2 ,x i3 ,x i4 ,...,x id ,...,x iD ) (ii) a Velocity vector is V i ＝(v i1 ,v i2 ,v i3 ,v i4 ,...,v id ,...,v iD ) (ii) a The optimal position of the individual is P i ＝(p i1 ,p i2 ,p i3 ,p i4 ,...,p id ,...,p iD ) (ii) a The optimal position of the population is denoted as P g ＝(p g1 ,p g2 ,p g3 ,p g4 ,...,p gd ,...,p gD ) The updating model of the individual optimal position is as follows:
Figure FDA0003659500410000032
the optimal position of the population is the optimal position of all individuals, and the updated model of the velocity and position is shown as follows:
Figure FDA0003659500410000033
wherein, omega is inertia weight, and the particle swarm algorithm has faster convergence speed when omega is between 0.8 and 1.2; c. C 1 And c 2 Is a learning factor, also called acceleration constant; r is 1 And r 2 Is [0,1 ]]A uniform random number within the range.
4. The three-degree-of-freedom mechanical arm dynamics model with constraints of claim 3, wherein: the particle swarm optimization algorithm comprises the following steps:
(1) determining the dimension of particles, determining the initial population number of the particle swarm, initializing the position and the speed of the particles, and setting an optimal fitness value;
(2) setting a fitness function, preliminarily screening particles with better fitness values, and calculating coefficients of a polynomial;
(3) calculating the joint speed and acceleration tracks by the polynomial coefficients, and checking whether the set maximum limit value is exceeded;
(4) traversing all the particles, calculating the fitness value of each particle, checking whether the fitness value of each time is smaller than the updated optimal fitness value, and simultaneously checking whether joint velocity and acceleration tracks corresponding to polynomial coefficients calculated by the particles have values exceeding the limit; if not, updating the optimal particles and updating the optimal fitness value; if yes, skipping the particle, and detecting the next particle until all the particles are detected;
(5) comparing each particle with the updated fitness value of the optimal particle to obtain the current overall optimal particle, then comparing the current overall optimal particle with the global optimal particle experienced by the group, and if the fitness value is smaller, updating the global optimal particle and the optimal fitness value;
(6) and iterating until the number of termination times, stopping updating, obtaining a corresponding polynomial coefficient according to the global optimal particles, and calculating corresponding joint angles, angular velocities and angular acceleration tracks.
CN202210569131.6A 2022-05-24 2022-05-24 Three-degree-of-freedom mechanical arm dynamic model with constraint Withdrawn CN114840947A (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210569131.6A CN114840947A (en) 2022-05-24 2022-05-24 Three-degree-of-freedom mechanical arm dynamic model with constraint

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210569131.6A CN114840947A (en) 2022-05-24 2022-05-24 Three-degree-of-freedom mechanical arm dynamic model with constraint

Publications (1)

Publication Number Publication Date
CN114840947A true CN114840947A (en) 2022-08-02

Family

ID=82571819

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210569131.6A Withdrawn CN114840947A (en) 2022-05-24 2022-05-24 Three-degree-of-freedom mechanical arm dynamic model with constraint

Country Status (1)

Country Link
CN (1) CN114840947A (en)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117590754A (en) * 2024-01-18 2024-02-23 北京理工大学 Intelligent learning output regulation and control method of robot system

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117590754A (en) * 2024-01-18 2024-02-23 北京理工大学 Intelligent learning output regulation and control method of robot system
CN117590754B (en) * 2024-01-18 2024-05-03 北京理工大学 Intelligent learning output regulation and control method of robot system

Similar Documents

Publication Publication Date Title
CN106647282B (en) Six-degree-of-freedom robot trajectory planning method considering tail end motion error
CN112757306B (en) Inverse solution multi-solution selection and time optimal trajectory planning algorithm for mechanical arm
CN108942924B (en) Model uncertainty mechanical arm motion control method based on multilayer neural network
CN112904728B (en) Mechanical arm sliding mode control track tracking method based on improved approach law
CN108628172B (en) Mechanical arm high-precision motion control method based on extended state observer
CN109782601B (en) Design method of self-adaptive neural network synchronous robust controller of coordinated mechanical arm
CN109343350A (en) A kind of underwater robot path tracking control method based on Model Predictive Control
CN105772917B (en) A kind of three joint spot welding robot&#39;s Trajectory Tracking Control methods
CN107457783B (en) Six-degree-of-freedom mechanical arm self-adaptive intelligent detection method based on PD controller
CN109352656B (en) Multi-joint mechanical arm control method with time-varying output constraint
CN108227506A (en) A kind of robot admittance control system based on adaptive optimization method
CN107145640B (en) Dynamic scale planning method for floating base and mechanical arm in neutral buoyancy experiment
CN108267952B (en) Self-adaptive finite time control method for underwater robot
CN112455726B (en) Low-orbit optical remote sensing satellite multi-point imaging rapid maneuvering control method
CN107414827A (en) Sixdegree-of-freedom simulation self-adapting detecting method based on linear feedback controller
Li et al. On-line self tuning of contouring control for high accuracy robot manipulators under various operations
CN112809666A (en) 5-DOF mechanical arm force and position tracking algorithm based on neural network
CN118288294B (en) Robot vision servo and man-machine cooperative control method based on image variable admittance
CN114840947A (en) Three-degree-of-freedom mechanical arm dynamic model with constraint
Zhou et al. Position-based visual servoing control for multi-joint hydraulic manipulator
CN114800521A (en) Three-degree-of-freedom mechanical arm fixed path point motion control system with constraint
CN107398903B (en) Track control method for industrial mechanical arm execution end
Tangpattanakul et al. Optimal trajectory of robot manipulator using harmony search algorithms
Rezapour et al. Path following control of planar snake robots using virtual holonomic constraints
CN113867157B (en) Optimal trajectory planning method and device for control compensation and storage device

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
WW01 Invention patent application withdrawn after publication

Application publication date: 20220802

WW01 Invention patent application withdrawn after publication