CN113290555A - Optimization method for time optimal control trajectory of industrial robot - Google Patents

Optimization method for time optimal control trajectory of industrial robot Download PDF

Info

Publication number
CN113290555A
CN113290555A CN202110499901.XA CN202110499901A CN113290555A CN 113290555 A CN113290555 A CN 113290555A CN 202110499901 A CN202110499901 A CN 202110499901A CN 113290555 A CN113290555 A CN 113290555A
Authority
CN
China
Prior art keywords
constraint
robot
socp
variable
time
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.)
Granted
Application number
CN202110499901.XA
Other languages
Chinese (zh)
Other versions
CN113290555B (en
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.)
Zhejiang University ZJU
Original Assignee
Zhejiang University ZJU
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 Zhejiang University ZJU filed Critical Zhejiang University ZJU
Priority to CN202110499901.XA priority Critical patent/CN113290555B/en
Publication of CN113290555A publication Critical patent/CN113290555A/en
Application granted granted Critical
Publication of CN113290555B publication Critical patent/CN113290555B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

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/1656—Programme controls characterised by programming, planning systems for manipulators
    • B25J9/1664—Programme controls characterised by programming, planning systems for manipulators characterised by motion, path, trajectory planning
    • 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/1602—Programme controls characterised by the control system, structure, architecture
    • B25J9/1605—Simulation of manipulator lay-out, design, modelling of manipulator

Abstract

The invention discloses an optimization method of an industrial robot time optimal control track, which is mainly characterized in that the constraints of kinematics and dynamics of a mechanical arm in the motion process are considered on the premise of ensuring that the robot rapidly reaches an appointed position, and the mechanical arm can be exerted to the maximum extent. The method comprises the steps of constructing a generalized path variable containing a time scale and a time optimal control constraint model established based on a convex optimization theory, and finally optimizing the solving efficiency of the model by utilizing a linear programming method. The method has the advantages that through the planning track after comprehensive improvement, compared with the method adopting a single algorithm, the method has obvious advantages in the aspect of solving efficiency.

Description

Optimization method for time optimal control trajectory of industrial robot
Technical Field
The invention relates to the field of industrial robot trajectory planning and application, in particular to an optimization method for an industrial robot time optimal control trajectory.
Background
The process of trajectory planning and design optimization of the industrial robot has important significance in the novel hot door field such as automation integration. For example, an emerging unmanned warehouse, the processing of micro electronic components, the automated integration of medical instruments, and the like, the track design and optimization process of industrial robots in these fields needs to simultaneously consider the problems of constraint conditions and the construction of constraint targets existing in different scenarios. In the robot level, multiple constraint limits of the robot kinematics and dynamics level need to be comprehensively considered, multiple indexes are introduced, and the trajectory planning efficiency of the robot in the aspects of the robot kinematics and dynamics is improved.
Disclosure of Invention
In order to solve the defects of the prior art and achieve the purposes that the industrial robot carries out trajectory planning and reaches a target position under the conditions of an optimal trajectory and high solving efficiency, the invention adopts the following technical scheme:
the invention mainly provides a feasibility scheme verified by a simulation experiment aiming at the following two problems: the method solves the problem of discretization constraint construction of robot kinematics and dynamics models. And secondly, the efficiency problem of solving the time optimal control track of the industrial robot by introducing a convex optimization method is solved.
The invention provides an optimization method of an industrial robot time optimal control track, which comprises the following steps of firstly establishing a DH parameter table of a robot model, establishing a DH parameter table as a geometric parameter modeling for the robot, mainly comprising information such as connecting rod length information and joint deflection angles of the robot, establishing the DH parameter table for the robot according to the precondition of inverse kinematics, and then constructing the discretization constraint of the kinematics and dynamics model and improving the track planning solving efficiency, wherein the related steps are as follows:
s1: constructing a robot kinematics and dynamics model;
the data input is an arc transition track in an industrial scene, the information of the track comprises information such as three-dimensional space position coordinates of the track, and the coordinates of each point on the track are resolved through inverse kinematics to obtain each joint angle of the robot. The DH parameters are geometric parameter modeling of the robot, and mainly comprise information such as connecting rod length information and joint deflection angles of the robot, and a DH parameter table needs to be established for the robot under the precondition of inverse kinematics.
For a kinematic model of a robot, a value which is equivalent to discretization is obtained mainly through forward and inverse kinematics of the robot by calculation, namely, a trajectory corresponding to a plan is obtained, wherein each point of discretization optimization needs forward and inverse kinematics calculation, and the forward and inverse kinematics are expressed as follows:
Figure BDA0003056000480000011
whereinjqiSix joint angles in joint space representing the robot, j represents the j-th joint angle, i represents the i-th discretized point, riA matrix of poses representing the robot end effector,
Figure BDA0003056000480000021
and
Figure BDA0003056000480000022
represents the forward and inverse solution processes of the robot kinematics;
the kinetic model for the robot was constructed as follows:
Figure BDA0003056000480000023
wherein M (q) epsilon Rn×nIs a mass matrix of the mechanical arm which is absolutely not negative,
Figure BDA0003056000480000024
is the effect matrix of the Coriolis force and the centrifugal force of the robot arm, Fs(q)∈Rn×nIs the Coulomb friction torque component, G (q) e RnIs an additional external force component (here, mainly the constant of universal gravitation), and tau epsilon RnIs the moment component of the mechanical arm joint,
Figure BDA0003056000480000025
is a joint variable (each representingAngle, angular velocity, angular acceleration).
S2: constructing a target constraint function after generalized coordinate transformation;
the calculated joint angle is used as a target value (i.e., target constraint) for motion control. Establishing a corresponding target constraint function on the basis of the data; the target value is processed by a target constraint function, and the calculated actual joint angle is output.
S21: introducing a generalized path variable containing time, redefining a robot joint space track to obtain a new constraint form;
considering that the track or Cartesian space path of the robot is a geometric characteristic parameter without any time scale information, so that a time variable cannot be directly introduced into the track control of the robot, considering the path in the robot configuration space, establishing a connection between an original time optimal control problem and a generalized path variable s (t) through integral transformation, and taking the generalized path variable s (t) as a new function independent variable through the idea of coordinate transformation:
Figure BDA0003056000480000026
Figure BDA0003056000480000027
wherein s ═ s (t) represents the generalized path variable after introduction of the time scale,
Figure BDA0003056000480000028
denotes the first derivative of s, tfRepresenting a track time;
the redefined robot joint space trajectory may be expressed in q (s (t)). After introducing the time variable s, the kinematic and kinetic models of the robot are obtained by substituting q (s (t)) into equation (2) and performing chain derivation as follows:
Figure BDA0003056000480000029
wherein M is the derived form of M,
Figure BDA00030560004800000210
the second derivative of s is represented by,
Figure BDA00030560004800000211
representing the square of the first derivative of s, C being the form after C derivation, G being the form after G derivation, the friction moment F being multiplied by a sign function sign, the derivation being 0;
movement time t for trajectory control of a robotfThe minimum can be reached, representing the robot constraints as follows:
Figure BDA0003056000480000031
wherein the content of the first and second substances,
Figure BDA0003056000480000032
is the formula of the robot dynamics model described above,
Figure BDA0003056000480000033
represents a kinematic and dynamic constraint set of the robot end effector, and gamma represents a joint space; after the generalized path coordinate transformation, the basic form of time optimal trajectory optimization is expanded, and head and tail end constraints are added to obtain a new constraint form:
Figure BDA0003056000480000034
wherein the content of the first and second substances,τ(s (t)) and
Figure BDA0003056000480000035
are the upper and lower limits of the robot arm moments, which are functions of the generalized path variable s (t), and in most cases,
Figure BDA0003056000480000036
and
Figure BDA0003056000480000037
the boundary case may take 0 (since most trajectories are stationary from an initial state to an end state, i.e. from one location to another).
S22: constructing a new variable to replace the generalized path variable s (t), the related differential components are as follows:
Figure BDA0003056000480000038
Figure BDA0003056000480000039
directly considering the transformed a(s) and b(s) as optimization variables to obtain a reconstructed constraint form:
Figure BDA00030560004800000310
wherein b'(s) represents the first derivative of b;
s23: constructing an objective constraint function
Figure BDA0003056000480000041
Is a real-valued function defined in some real vector space, whose domain is defined
Figure BDA0003056000480000042
Two arbitrary points x on1And x2All satisfy:
f((1-α)x1+αx2)≤(1-α)f(x1)+αf(x2) (11)
wherein x is an independent variable and represents an introduced variable, in the method, the introduced variable is b(s), alpha is epsilon R, and alpha is more than or equal to 0 and less than or equal to 1
Figure BDA0003056000480000043
Visualization of the objective constraint function is shown in fig. 2.
S3: a second-order cone programming SOCP form is introduced to optimize a target constraint function, and the main purpose is to increase the solving efficiency in the data processing process;
at each discrete point, defining by a normalized unit second order cone form; to transform the problem into a cone plan, new variables are introduced
Figure BDA0003056000480000044
For convex optimization model formula (10)
Figure BDA0003056000480000045
Is replaced due to the fact that
Figure BDA0003056000480000046
All have b(s), c(s) > 0, so that there are the following equivalent substitutions:
Figure BDA0003056000480000047
because f (b, c) ═ c2B is a form of a convex function with respect to the variables b, c, the inequality c being therefore among them2(s) -b(s). ltoreq.0 is in the form of a convex cone constraint, on the other hand, -c2(s) + b(s) ≦ 0 is the corresponding concave-cone form constraint, indicating that current time-optimal trajectory optimization is a non-convex problem. Discarding concave constraint therein-c2(s) + b(s). ltoreq.0, transforming the nonlinear equality constraint into a convex constraint as follows:
Figure BDA0003056000480000048
wherein, compare
Figure BDA0003056000480000051
The meaning of c(s) is unchanged, except that c(s) becomes two-dimensional after the second-order cone SOCP planning is introduced;
the cone constraint which conforms to the basic form of cone planning is obtained through the design of convex relaxation transformation, and
Figure BDA0003056000480000052
all satisfy:
Figure BDA0003056000480000053
in order to simplify the objective of the SOCP convex optimization solution, a variable d(s) is newly introduced, and the basic cone constraint transformation of the SOCP is performed for one more time:
Figure BDA0003056000480000054
at this point, the convex relaxation transformation and the SOCP transformation of equation (14) are completed, resulting in a basic form that conforms to SOCP optimization.
S4: solving a convex programming algorithm of a second-order cone, performing discretization treatment, and performing discretization by using a high-order symmetrical Runge Kutta formula (formulas (22) and (23)); the higher-order symmetrical Runge Kutta formula is used for further optimizing the second-order cone SOCP, and mainly has the function of obtaining a discretized iterative solution form (which is convenient for MATLAB to solve);
in order to solve the problem that the motion trajectory and the constraint conditions of the mechanical arm in a general scene are difficult to quantify, a discretization method is used, corresponding thresholds are set, and iterative computation is performed to obtain an optimal solution. Due to the discontinuous motion process of the robot, a discrete point s is assumediAnd si+1The track speed related variable b(s) changes in a piecewise linear way, and the first differential of the formula (9) is obtained, and b'(s) is in siAnd si+1Constant over the interval, giving:
Figure BDA0003056000480000055
since b'(s) is at siAnd si+1Constant over the interval, so that the above formula is directly based on the basic linear parameter equation, where bi＝b(si)，bi+1＝b(si+1) (ii) a Introduction of new variables d(s) ═ b(s)-1/2Substitution of the constraint targets for two particular discretized target points 0+And 1-And (3) introducing an adjusting variable epsilon, wherein epsilon is more than 0 and less than 1, discretizing the corresponding target point as follows:
Figure BDA0003056000480000056
is obtained under the assumption siAnd si+1When a(s) is constant within the interval, the formula (10) passes through the variable d(s) ═ b(s)-1/2The constraint objective problem after replacement is transformed into the following form:
Figure BDA0003056000480000061
the generalized path variable s, the constraint variables a(s), b(s), tau(s) are discretized into N points, g(s) are not used as an object of iterative computation and do not need discretization, and the boundary condition is s0＝0,s N-11, and for
Figure BDA0003056000480000068
All have si＜si+1Wherein is to
Figure BDA0003056000480000069
The formula of discretization value is as follows:
Figure BDA0003056000480000062
Figure BDA0003056000480000063
Figure BDA0003056000480000064
discretization of the derivative relationships q'(s) and q "(s) between joint space and generalized path variables is also required in the basic form of the optimization problem of equation (10), assuming that discretized q(s) has been obtained at i-0, 1,2,3i) Using discretized intermediate values as
Figure BDA00030560004800000610
Is expressed as follows:
Figure BDA0003056000480000065
Figure BDA0003056000480000066
Figure BDA0003056000480000067
through the discretization, all discretization processes conforming to SOCP optimization are determined.
S5: linearizing the SOCP second-order cone programming problem;
linearization, namely Linear Programming (LP), further optimizing the discretized SOCP form, and mainly improving the solving efficiency of the SOCP; solving the initial convex optimization problem through a linear solver LP, wherein the main advantage of the solving is that the calculation performance is improved, and for solving the convex optimization problem, the best solution is the solution of LP, so that the calculation time sum of the track optimization process is tested under the conditions of different paths and different numbers of path reference points;
s51: the method (14) is simplified, and when only constraint conditions at the kinematic level are considered, the optimal solution of LP is proved:
Figure BDA0003056000480000071
wherein
Figure BDA0003056000480000072
ΔsiDenotes si+1To siThe amount of change in the amount of change,
Figure BDA0003056000480000073
the maximum value in the discrete value of the joint angle q is represented, mu and xi are variables defined in the linearization process, the value range is between 0 and 1, the actual parameter selection depends on different experimental environments of the embodiment, the constraint conditions are further simplified, and the analysis is carried out by taking the condition that mu is 1 and xi is 0:
Figure BDA0003056000480000074
get the final feasible solution of
Figure BDA0003056000480000075
Figure BDA0003056000480000076
In the case of only kinematic constraints, the LP form is transformed from the SOCP form, and therefore boptSimultaneously, the method is a feasible solution of two solving forms; LP is compared to SOCP form, only the functional form in the constraint objective is different;
s52: demonstration boptWhether it is the optimal solution, the constraint target problem forms of LP and SOCP are first compared:
Figure BDA0003056000480000077
ySOCP(b) the constraint objective function is monotonically decreasing, and yLP(b) The constraint objective function is monotonically increasing, thus satisfying the following condition:
Figure BDA0003056000480000078
due to ySOCP(b) And yLP(b) Are all monotonic functions within the constraints, so boptIs a locally optimal solution to the SOCP and LP problems; in the SOCP problem, the objective is to minimize the convexity of the objective function, while in the LP problem, the objective is to maximize the linear objective function under the same convex constraint environment, so the local optimal solution of the two is also the global optimal solution.
And finally, modeling and solving the method by using a YALMIP optimization solving tool in MATLAB so as to realize the method at a code level.
The invention has the advantages and beneficial effects that:
according to the time optimal control track optimization method for the industrial robot, provided by the invention, on the premise that the robot rapidly reaches the designated position, the constraints of kinematics and a dynamics level of the robot in the motion process are considered, so that the industrial robot carries out track planning under the conditions of an optimal track and high solving efficiency and reaches the target position.
Drawings
FIG. 1 is a flow chart of the method of the present invention.
FIG. 2 is a schematic diagram of image visualization of a convex optimization constraint objective function according to the present invention.
FIG. 3 is a diagram of the minimum time for SOCP and LP solution trajectories in the present invention.
FIG. 4 is a statistical plot of the SOCP and LP solution iteration number contours of the present invention.
FIG. 5 is a diagram of SOCP and LP solution times and discrete point numbers in accordance with the present invention.
Detailed Description
The following detailed description of embodiments of the invention refers to the accompanying drawings. It should be understood that the detailed description and specific examples, while indicating the present invention, are given by way of illustration and explanation only, not limitation.
The first embodiment is as follows:
as shown in FIG. 1, the simulation experiment test of the invention is mainly based on the kinematics and dynamics model data of the open source of an industrial robot KUKA361, KU Leuven laboratory, and the equipment and environment of the experiment test are Intel (R) core (TM) i7-8750H CPU @2.20GHz MATLAB R2018 b.
The method comprises the following steps: establishing a DH parameter table of a KUKA361 robot model;
the DH parameters of the KUKA361 mechanical arm are shown in Table 1, the distance unit is mm, and the angle unit is rad; more specifically, the 5-axis of the wrist portion of the KUKA361 arm consists of two joints, so an additional frame is added to separate the 5a and 5b analysis, but their rotation angles around the 5-axis are always equal, but opposite in sign.
Figure BDA0003056000480000081
TABLE 1
Step two: and establishing a kinematics and dynamics model of the KUKA361 robot.
Step three: and constructing the target constraint function after the generalized coordinate transformation.
Step four: and constructing a second-order cone programming SOCP basic form, and improving the efficiency of optimization solution.
Step five: solving a second-order cone convex programming algorithm for discretization;
according to the constraint target and the constraint condition after the discretization analysis, a convex optimization basic form which accords with the constraint condition of the second-order cone programming can be obtained, if a non-convex item which does not meet the convex optimization exists, convex relaxation transformation is correspondingly carried out, and a basic equation for solving the convex optimization problem is obtained:
Figure BDA0003056000480000091
Figure BDA0003056000480000092
Figure BDA0003056000480000093
Figure BDA0003056000480000094
Figure BDA0003056000480000095
Figure BDA0003056000480000096
bi+1-bi＝2aiΔsi,
bi≥0,ci≥0,
for i＝0,...,N-1,
Figure BDA0003056000480000097
Figure BDA0003056000480000098
Figure BDA0003056000480000099
step six: the SOCP second order cone programming problem is linearized.
Step seven: carrying out modeling solution by using a YALMIP optimization solving tool in MATLAB; the use of YALMIP is an implementation of the above process, the code level.
The current optimization problem mainly tests the most commonly used circle of the robot in the industrial scenarioArc transition track, and considering that the track of the robot end effector is mainly in consideration of the spatial position information of the robot, the joint moment of the rear three shafts changes in a small range, so that the track state information of the front three joints of the KUKA361 robot is mainly monitored; the robot model joint space working range according to the kinematics and dynamic model data of the KU Leuven laboratory KUKA361 industrial robot open source is limited as
Figure BDA00030560004800000910
Visualizing joint angles and joint angular velocities of the front three joints in the track running process of the robot; in a general view, the optimal solution of the robot under the current track condition is obtained through the SOCP-based robot time optimal track optimization process. In the test case within 1000 path discrete points, the method of the invention can control the solving time to be about 1 s-2 s.
Example two:
as shown in fig. 1, the simulation experiment example 2 of the present invention is mainly based on open source kinematics and dynamics model data of the elbow joint robot. The equipment and environment for the experimental tests was Intel (R) core (TM) i7-8750H CPU @2.20GHz MATLAB R2018 b.
The method comprises the following steps: and establishing a DH parameter table of the elbow joint robot model of the elbow.
Step two: and establishing a kinematics and dynamics model of the KUKA361 robot.
Step three: and constructing the target constraint function after the generalized coordinate transformation.
Step four: and constructing a second-order cone programming SOCP basic form, and improving the efficiency of optimization solution.
Step five: and solving the second-order cone convex programming algorithm to carry out discretization processing.
Step six: the SOCP second order cone programming problem is linearized.
Step seven: modeling solution was performed using the YALMIP optimization solver in MATLAB.
According to the experimental result of the test, from the condition of track test with a plurality of discrete points of 200-4000, the track running time comparison of the obtained optimal solution is shown in fig. 3, the SOCP and LP solution results are both distributed between 1.41s and 1.44s, the optimal solution time of LP solution is slightly larger than the optimal solution time of SOCP solution, the error is approximately about 0.01s, for the whole part of the time optimal track, the optimal solution accounts for approximately 0.7% of the proportion, and the optimal solution can be almost ignored, and the optimal solution obtained based on LP linear optimization and the optimal solution obtained based on the SOCP second-order cone are considered to be equivalent.
For comparison of numerical iteration times, as shown in fig. 4, LP has an obvious advantage compared with SOCP, the iteration time contour is always located inside the SOCP algorithm iteration contour, and when the number of discrete points is below 1000, the optimal solution is obtained by almost only 50% of the iteration times compared with the SOCP algorithm, which directly results in the advantage of LP in the solution time.
The solving time comparison of the SOCP algorithm and the LP algorithm can be seen, as shown in FIG. 5, from the track solving condition of the number of the discrete points of 200-4000, the LP algorithm always keeps a good solving state, the optimal solution is basically obtained in about 1s, the increasing of the number of the discrete points does not have too large influence on the LP algorithm, the solving speed of the inverse SOCP algorithm is increased almost exponentially along with the increasing of the number of the discrete points, and reaches 9s at the 4000 discrete points, so that the requirement can not be basically met in the application of the mechanical arm needing real-time planning and control, and the feasibility of solving based on LP linear optimization is also proved.
The above examples are only intended to illustrate the technical solution of the present invention, but not to limit it; although the present invention has been described in detail with reference to the foregoing embodiments, it will be understood by those of ordinary skill in the art that: the technical solutions described in the foregoing embodiments may still be modified, or some or all of the technical features may be equivalently replaced; and the modifications or the substitutions do not make the essence of the corresponding technical solutions depart from the scope of the technical solutions of the embodiments of the present invention.

Claims (10)

1. An optimization method for an industrial robot time optimal control track is characterized by comprising the following steps:
s1, constructing a robot kinematics and dynamics model;
s2, constructing a target constraint function after generalized coordinate transformation, establishing the target constraint function by taking the joint angle obtained by the model in the step 1 as a target value of motion control, and outputting an actual joint angle by processing the target value through the target constraint function, wherein the method comprises the following steps:
s21, introducing a generalized path variable containing time, redefining the joint space track of the robot, and obtaining a new constraint form;
s22, constructing a new variable to replace the generalized path variable;
and S23, constructing an objective constraint function.
2. The method for optimizing the time-optimal control trajectory of the industrial robot according to claim 1, wherein the kinematics model in the step S1 is solved to obtain a value corresponding to a planned trajectory through a forward and inverse kinematics solution of the robot, wherein each point on the trajectory requires a forward and inverse kinematics solution, which is expressed as follows:
Figure FDA0003056000470000011
whereinjqiRepresents the joint angle of the robot joint space, j represents the j-th joint angle, i represents the i-th discretized point, riA pose matrix representing the robot end effector,
Figure FDA0003056000470000012
and
Figure FDA0003056000470000013
representing the forward and inverse solution processes of the robot kinematics.
3. The method for optimizing a time-optimal control trajectory of an industrial robot according to claim 1, wherein the dynamic model in step S1 is constructed as follows:
Figure FDA0003056000470000014
wherein M (q) epsilon Rn×nIs a mass matrix of the mechanical arm which is absolutely not negative,
Figure FDA0003056000470000015
is the effect matrix of the Coriolis force and the centrifugal force of the robot arm, Fs(q)∈Rn×nIs the Coulomb friction torque component, G (q) e RnIs an additional external force component, tau epsilon RnIs the moment component of the mechanical arm joint,
Figure FDA0003056000470000016
is a joint variable.
4. The method for optimizing the time-optimal control trajectory of the industrial robot according to claim 3, wherein the step S21 links the time-optimal control with the generalized path variable S (t) through integral transformation, and uses the generalized path variable S (t) as a new function argument through coordinate transformation:
Figure FDA0003056000470000017
Figure FDA0003056000470000018
wherein s ═ s (t) represents the generalized path variable after introduction of the time scale,
Figure FDA0003056000470000019
denotes the first derivative of s, tfRepresenting a track time;
and (3) expressing the redefined robot joint space track by q (s (t)), introducing a time variable s, substituting q (s (t)) into a formula (2), and performing chain derivation to obtain a kinematic and dynamic model of the robot as follows:
Figure FDA0003056000470000021
wherein M is the derived form of M,
Figure FDA0003056000470000022
the second derivative of s is represented by,
Figure FDA0003056000470000023
representing the square of the first derivative of s, C being the form after C derivation, G being the form after G derivation, the friction moment F being multiplied by a sign function sign, the derivation being 0;
movement time t for trajectory control of a robotfThe minimum can be reached, representing the robot constraints as follows:
Figure FDA0003056000470000024
Figure FDA0003056000470000029
representing a kinematic and dynamic constraint set of the robot end effector, wherein gamma represents a joint space; after the generalized path coordinate transformation, the basic form of the time optimal trajectory optimization is expanded, and head and tail end constraints are added to obtain a new constraint form:
Figure FDA0003056000470000025
wherein the content of the first and second substances,τ(s (t)) and
Figure FDA0003056000470000026
the upper and lower limits of the moment of the mechanical arm;
in step S22, a new variable is constructed to replace the generalized path variable S (t), and the differential component is as follows:
Figure FDA0003056000470000027
Figure FDA0003056000470000028
taking a(s) and b(s) as optimization variables to obtain a reconstructed constraint form:
Figure FDA0003056000470000031
wherein b'(s) represents the first derivative of b;
in the step S23, an objective constraint function is constructed
Figure FDA0003056000470000032
The target constraint function defines a real-valued function in a certain real number vector space, the domain of which is defined
Figure FDA0003056000470000033
Two arbitrary points x on1And x2All satisfy:
f((1-α)x1+αx2)≤(1-α)f(x1)+αf(x2) (11)
Figure FDA0003056000470000034
wherein x has the variables b(s), α ∈ R,0 ≦ α ≦ 1.
5. The method for optimizing the time-optimal control trajectory of the industrial robot according to claim 4, further comprising the steps of S3, constructing a second-order cone planning SOCP basic form;
at each point, defined by a normalized unit second order cone, a new variable is introduced
Figure FDA0003056000470000035
For those in the formula (10)
Figure FDA0003056000470000036
Is replaced due to the fact that
Figure FDA0003056000470000037
All have b(s), c(s) > 0, so that there are the following equivalent substitutions:
Figure FDA0003056000470000038
f(b,c)＝c2b is the form of a convex function with respect to the variables b, c, with the inequality c2(s) -b(s) ≦ 0 for the convex cone constraint form, -c2(s) + b(s) ≦ 0 is the corresponding concave conic form constraint, and the concave constraint-c is discarded2(s) + b(s) ≦ 0, transforming the nonlinear equation constraint to a convex constraint as follows:
Figure FDA0003056000470000041
wherein, compare
Figure FDA0003056000470000042
The meaning of c(s) is unchanged, except that c(s) becomes two-dimensional after the second-order cone SOCP planning is introduced;
obtaining cone constraint conforming to the basic form of cone planning through the design of convex relaxation transformation
Figure FDA0003056000470000043
All satisfy:
Figure FDA0003056000470000044
introducing a variable d(s), and performing primary cone constraint transformation of the SOCP:
Figure FDA0003056000470000045
a basic form that conforms to the SOCP optimization is obtained.
6. The method for optimizing the time-optimal control trajectory of the industrial robot according to claim 5, further comprising a step S4 of solving a convex programming algorithm for a second-order cone, and performing discretization;
setting a threshold value by using a discretization method, iteratively calculating to obtain an optimal solution, and setting a discrete point siAnd si+1The track speed related variable b(s) changes in a piecewise linear way, and the first differential of the formula (9) is obtained, and b'(s) is in siAnd si+1The interval is also constant, and is directly obtained according to a basic straight line parameter equation:
Figure FDA0003056000470000046
wherein b isi＝b(si)，bi+1＝b(si+1) Introduction of the variable d(s) ═ b(s)-1/2Substitution of the constraint targets for two particular discretized target points 0+And 1-And (3) introducing an adjusting variable epsilon, wherein epsilon is more than 0 and less than 1, discretizing the corresponding target point as follows:
Figure FDA0003056000470000051
is obtained at siAnd si+1When a(s) is constant within the interval, the formula (10) passes through the variable d(s) ═ b(s)-1/2The constraint objective problem after replacement is transformed into the following form:
Figure FDA0003056000470000052
wherein, the generalized path variable s, the constraint variables a(s), b(s), and tau(s) are discretized into N points, and the boundary condition is s0＝0,sN-11, and for
Figure FDA0003056000470000053
All have si＜si+1Wherein is to
Figure FDA0003056000470000054
The formula of discretization value is as follows:
Figure FDA0003056000470000055
Figure FDA0003056000470000056
Figure FDA0003056000470000057
in the formula (10), the derivative relations q'(s) and q ″(s) between the joint space and the generalized path variable are discretized, and discretized q(s) is obtained at i-0, 1,2,3i) Using discretized intermediate values as
Figure FDA0003056000470000058
Is expressed as follows:
Figure FDA0003056000470000059
Figure FDA00030560004700000510
Figure FDA00030560004700000511
thereby determining all discretization procedures that are compliant with the SOCP optimization.
7. The method for optimizing the time-optimal control trajectory of the industrial robot according to claim 6, further comprising a step S5 of linearizing the SOCP second-order cone program, comprising the steps of:
s51: and (14) simplifying, and judging the optimal solution of the linear programming LP when only the constraint condition of the kinematic level is considered:
Figure FDA0003056000470000061
wherein
Figure FDA0003056000470000062
ΔsiDenotes si+1To siAmount of change, qi maxThe maximum value of the discrete value of the joint angle q is represented, μ and ξ are variables defined in the linearization process, the above constraint conditions are further simplified, and μ ═ 1, ξ ═ 0:
Figure FDA0003056000470000063
get the final feasible solution of
Figure FDA0003056000470000064
Figure FDA0003056000470000065
In the case of only kinematic constraints, the LP form is transformed from the SOCP form, and therefore boptSimultaneously, the method is a feasible solution of two solving forms; LP is compared to SOCP form, except that the functional form in the constraint objective is different;
s52: judgment boptWhether it is the optimal solution, the constraint target problem forms of LP and SOCP are first compared:
Figure FDA0003056000470000066
ySOCP(b) the constraint objective function is monotonically decreasing, and yLP(b) The constraint objective function is monotonically increasing, thus satisfying the following condition:
Figure FDA0003056000470000067
due to ySOCP(b) And yLP(b) Are all monotonic functions within the constraints, so boptFor SOCP and LP problems, it is a locally optimal solution; in the SOCP problem, the objective is to minimize the convexity of the objective function, while in the LP problem, the objective is to maximize the linear objective function under the same convex constraint environment, so the local optimal solution of the two is also the global optimal solution.
8. The method of claim 4, wherein in the new constraint form,
Figure FDA0003056000470000068
and
Figure FDA0003056000470000069
the boundary case takes 0.
9. The method of claim 1, wherein before the step S1, a DH parameter table of the robot model is established.
10. A method for optimizing a time-optimal control trajectory for an industrial robot according to claim 1, characterized in that the method is modelled using a yalmap optimization solver in MATLAB.
CN202110499901.XA 2021-05-08 2021-05-08 Optimization method for time optimal control trajectory of industrial robot Active CN113290555B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110499901.XA CN113290555B (en) 2021-05-08 2021-05-08 Optimization method for time optimal control trajectory of industrial robot

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110499901.XA CN113290555B (en) 2021-05-08 2021-05-08 Optimization method for time optimal control trajectory of industrial robot

Publications (2)

Publication Number Publication Date
CN113290555A true CN113290555A (en) 2021-08-24
CN113290555B CN113290555B (en) 2022-04-15

Family

ID=77321158

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110499901.XA Active CN113290555B (en) 2021-05-08 2021-05-08 Optimization method for time optimal control trajectory of industrial robot

Country Status (1)

Country Link
CN (1) CN113290555B (en)

Cited By (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113787525A (en) * 2021-11-18 2021-12-14 季华实验室 Mechanical arm movement time optimization method based on joint performance limitation
CN114102606A (en) * 2021-12-22 2022-03-01 北京航空航天大学杭州创新研究院 Robot motion information planning method and related device
CN114137982A (en) * 2021-11-29 2022-03-04 深圳市优必选科技股份有限公司 Robot motion control method and device, robot control device and storage medium
CN114654464A (en) * 2022-03-22 2022-06-24 上海景吾酷租科技发展有限公司 Cleaning robot positioning position selection method and system based on time optimization
CN115302520A (en) * 2022-10-12 2022-11-08 深圳市智绘科技有限公司 Robot path optimization method and device and electronic equipment

Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104062902A (en) * 2014-05-15 2014-09-24 江南大学 Delta robot time optimal trajectory planning method
CN108621158A (en) * 2018-04-27 2018-10-09 上海师范大学 A kind of time optimal trajectory planning control method and device about mechanical arm
CN109828600A (en) * 2019-01-09 2019-05-31 北京理工大学 Time optimal quick three-dimensional obstacle-avoiding route planning method
CN110209048A (en) * 2019-05-20 2019-09-06 华南理工大学 Robot time optimal trajectory planning method, equipment based on kinetic model
CN111399439A (en) * 2020-03-02 2020-07-10 之江实验室 Second-order cone programming based trajectory generation method for mobile robot
CN111399514A (en) * 2020-03-30 2020-07-10 浙江钱江机器人有限公司 Robot time optimal trajectory planning method
CN111562797A (en) * 2020-07-06 2020-08-21 北京理工大学 Unmanned aerial vehicle flight time optimal real-time trajectory optimization method capable of ensuring convergence
CN112757306A (en) * 2021-01-25 2021-05-07 北京交通大学 Inverse solution multi-solution selection and time optimal trajectory planning algorithm for mechanical arm

Patent Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104062902A (en) * 2014-05-15 2014-09-24 江南大学 Delta robot time optimal trajectory planning method
CN108621158A (en) * 2018-04-27 2018-10-09 上海师范大学 A kind of time optimal trajectory planning control method and device about mechanical arm
CN109828600A (en) * 2019-01-09 2019-05-31 北京理工大学 Time optimal quick three-dimensional obstacle-avoiding route planning method
CN110209048A (en) * 2019-05-20 2019-09-06 华南理工大学 Robot time optimal trajectory planning method, equipment based on kinetic model
CN111399439A (en) * 2020-03-02 2020-07-10 之江实验室 Second-order cone programming based trajectory generation method for mobile robot
CN111399514A (en) * 2020-03-30 2020-07-10 浙江钱江机器人有限公司 Robot time optimal trajectory planning method
CN111562797A (en) * 2020-07-06 2020-08-21 北京理工大学 Unmanned aerial vehicle flight time optimal real-time trajectory optimization method capable of ensuring convergence
CN112757306A (en) * 2021-01-25 2021-05-07 北京交通大学 Inverse solution multi-solution selection and time optimal trajectory planning algorithm for mechanical arm

Cited By (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113787525A (en) * 2021-11-18 2021-12-14 季华实验室 Mechanical arm movement time optimization method based on joint performance limitation
CN113787525B (en) * 2021-11-18 2022-02-01 季华实验室 Mechanical arm movement time optimization method based on joint performance limitation
CN114137982A (en) * 2021-11-29 2022-03-04 深圳市优必选科技股份有限公司 Robot motion control method and device, robot control device and storage medium
CN114102606A (en) * 2021-12-22 2022-03-01 北京航空航天大学杭州创新研究院 Robot motion information planning method and related device
CN114102606B (en) * 2021-12-22 2022-12-23 北京航空航天大学杭州创新研究院 Robot motion information planning method and related device
WO2023115784A1 (en) * 2021-12-22 2023-06-29 北京航空航天大学杭州创新研究院 Robot motion information planning method and related apparatus
CN114654464A (en) * 2022-03-22 2022-06-24 上海景吾酷租科技发展有限公司 Cleaning robot positioning position selection method and system based on time optimization
CN115302520A (en) * 2022-10-12 2022-11-08 深圳市智绘科技有限公司 Robot path optimization method and device and electronic equipment

Also Published As

Publication number Publication date
CN113290555B (en) 2022-04-15

Similar Documents

Publication Publication Date Title
CN113290555B (en) Optimization method for time optimal control trajectory of industrial robot
Liu et al. Digital twin modeling method based on biomimicry for machining aerospace components
Raza et al. Kinematic analysis and geometrical improvement of an industrial robotic arm
Karbasizadeh et al. Experimental dynamic identification and model feed-forward control of Novint Falcon haptic device
Yang et al. General 7-instant DCZNN model solving future different-level system of nonlinear inequality and linear equation
Li et al. A flexible manufacturing assembly system with deep reinforcement learning
Arian et al. Kinematic and dynamic analysis of the Gantry-Tau, a 3-DoF translational parallel manipulator
Chen et al. Jerk-level synchronous repetitive motion scheme with gradient-type and zeroing-type dynamics algorithms applied to dual-arm redundant robot system control
Du et al. A cognitive joint angle compensation system based on self-feedback fuzzy neural network with incremental learning
Yang et al. Robot kinematics and dynamics modeling
Jangid et al. Trajectory tracking optimization and control of a three link robotic manipulator for application in casting
De Sapio et al. Least action principles and their application to constrained and task-level problems in robotics and biomechanics
Kelaiaia et al. Multiobjective optimization of 6-dof UPS parallel manipulators
Pelliccia et al. Task-based motion control of digital humans for industrial applications
Ozakyol et al. Advanced robotics analysis toolbox for kinematic and dynamic design and analysis of high‐DOF redundant serial manipulators
Pikalov et al. Vector model for solving the inverse kinematics problem in the system of external adaptive control of robotic manipulators
Dewandhana et al. Forward Kinematics with Full-Arm Analysis on “T-FLoW” 3.0 Humanoid Robot
Dash et al. Inverse kinematics solution of a 6-DOF industrial robot
Crenganis et al. Inverse kinematics of a 7 DOF manipulator using adaptive neuro-fuzzy inference systems
Yao et al. Virtual data generation for human intention prediction based on digital modeling of human-robot collaboration
Er et al. Real-time hybrid adaptive fuzzy control of a SCARA robot
Chen et al. Applications of an improved Dixon elimination method for the inverse kinematics of 6R manipulators
Varedi-Koulaei et al. Trajectory tracking solution of a robotic arm based on optimized ANN
Abdullah et al. Algebraic models based on trigonometric and Cramer's rules for computing inverse kinematics of robotic arm
Eberhard et al. Nonlinear position control of a very flexible parallel robot 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
GR01 Patent grant
GR01 Patent grant