CN113664839A - Non-real-time and real-time mixed interpolation calculation method for industrial robot - Google Patents

Non-real-time and real-time mixed interpolation calculation method for industrial robot Download PDF

Info

Publication number
CN113664839A
CN113664839A CN202111241737.9A CN202111241737A CN113664839A CN 113664839 A CN113664839 A CN 113664839A CN 202111241737 A CN202111241737 A CN 202111241737A CN 113664839 A CN113664839 A CN 113664839A
Authority
CN
China
Prior art keywords
real
interpolation
track
time
discrete
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
CN202111241737.9A
Other languages
Chinese (zh)
Other versions
CN113664839B (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.)
Wuhan Hanmai Technology Co ltd
Original Assignee
Wuhan Hanmai Technology Co ltd
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 Wuhan Hanmai Technology Co ltd filed Critical Wuhan Hanmai Technology Co ltd
Priority to CN202111241737.9A priority Critical patent/CN113664839B/en
Publication of CN113664839A publication Critical patent/CN113664839A/en
Application granted granted Critical
Publication of CN113664839B publication Critical patent/CN113664839B/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

Landscapes

  • Engineering & Computer Science (AREA)
  • Robotics (AREA)
  • Mechanical Engineering (AREA)
  • Manipulator (AREA)
  • Numerical Control (AREA)

Abstract

The invention provides a non-real-time and real-time mixed interpolation calculation method for an industrial robot, which is characterized by comprising the following steps of: the interpolation calculation of the motion control system of the industrial robot is divided into a non-real-time discrete calculation part and a real-time interpolation calculation part, the non-real-time discrete calculation part and the real-time interpolation calculation part are mutually independent, an independent central processing unit is adopted to occupy independent memory resources, and a track instruction is converted into a real-time servo instruction through the mixed calculation of the non-real-time discrete calculation part and the real-time interpolation calculation part, so that the motion control of the robot is realized. The invention removes the limitation of the computer operation speed and the interpolation period beat, puts the complex track planning algorithm in the traditional real-time interpolation calculation into the non-real-time discrete calculation of the invention, leads the track planning to be separated from the time limitation of the real-time interpolation period, and improves the integral performance of the interpolation algorithm. The method reduces the calculated amount of the real-time interpolation calculation, improves the efficiency of the real-time interpolation calculation, and also improves the efficiency of the integral interpolation calculation.

Description

Non-real-time and real-time mixed interpolation calculation method for industrial robot
Technical Field
The invention belongs to the field of motion control of industrial robots, and particularly relates to a non-real-time and real-time mixed interpolation calculation method for an industrial robot.
Background
Industrial robotic machining is one of the basic processes of contemporary machine manufacturing. Industrial robots work by means of multi-joint robots (also called robot arms). The motion control system is one of the core components of an industrial robot, and complex and changeable industrial application puts higher requirements on the motion control system of the industrial robot, so that on one hand, the robot end effector must move according to certain process requirements, and the requirements on an end pose point, speed and acceleration are required. On the other hand, the motion control system directly determines the motion mode, the operation precision and the service life of the industrial robot. A high-speed and high-precision motion control system is a fundamental guarantee for improving the processing efficiency and quality.
Generally, a motion control system is divided into an interpolation algorithm and a servo drive control algorithm, the interpolation algorithm receives a robot track program, completes real-time interpolation calculation, generates a servo motion instruction, and the servo drive control algorithm receives the servo motion instruction and controls a servo motor to realize motion through the servo control algorithm. The invention mainly aims at improving an interpolation algorithm in a motion control system, the traditional robot interpolation algorithm carries out real-time interpolation calculation in sequence according to the sequence of motion instructions, instruction analysis, smooth track, speed planning, inverse kinematics solution and interpolation point calculation are required to be completed in the middle, all the calculation must be completed according to the beat of an interpolation period, the track planning algorithm is limited by the interpolation period, the calculation speed of the track planning algorithm depends on the calculation speed of a computer, a relatively complex track optimization algorithm cannot be carried out, vibration occurs in actual operation, and the precision of the track cannot be ensured.
The robot track program received by the interpolation algorithm is composed of a plurality of rows of robot track instructions, and the robot track instructions are divided into motion instructions (joint motion MoveJ, linear motion MoveL, circular motion MoveC and the like), logic instructions (conditions, circulation, skip and the like), input/output instructions, conventional instructions (comments, time delay and the like) and the like. The motion instruction can be divided into a motion instruction of a joint space and a motion instruction of a Cartesian space, the joint space is an abstract space, and motion instruction information of the joint space comprises an angle, an angular velocity and an angular acceleration of the joint; the cartesian space is a general three-dimensional space, and the motion instruction information includes the position and posture of the tool in the user coordinate system, the translational velocity and rotational speed, the translational acceleration and rotational acceleration, the tool coordinate, and the user coordinate.
Disclosure of Invention
Aiming at the problems in the prior art, the invention provides a non-real-time and real-time mixed interpolation calculation method for an industrial robot, which divides an interpolation algorithm of the industrial robot into two parts: non-real-time discrete calculation and real-time interpolation calculation, and a complex track planning algorithm is put into the non-real-time discrete calculation, so that the limitation of interpolation period beats is removed. The non-real-time discrete calculation and the real-time interpolation calculation are independent, the two parts respectively occupy independent calculation resources and independent memory resources, and simultaneously share one memory area for data transmission interaction. The non-real-time discrete calculation is responsible for track instruction analysis, track smoothness, speed planning, track optimization, track pre-dispersion and output of a discrete track point array. And the real-time interpolation calculation is responsible for acquiring servo feedback information, signals of the controller and discrete track points, calculating interpolation points and outputting servo motion instructions.
The technical scheme adopted for solving the problems in the prior art is as follows:
a non-real-time and real-time mixed interpolation calculation method for an industrial robot divides interpolation calculation of an industrial robot motion control system into a non-real-time discrete calculation part and a real-time interpolation calculation part, wherein the non-real-time discrete calculation part and the real-time interpolation calculation part are mutually independent, an independent central processing unit is adopted to occupy independent memory resources, a track instruction is converted into a real-time servo instruction through the mixed calculation of the non-real-time discrete calculation part and the real-time interpolation calculation part, and the motion control of the robot is realized, and the specific steps are as follows:
s1, robot parameters are set, wherein the robot parameters comprise kinematic parameters of the robot, limit values of each joint of the robot, speed thresholds of each joint of the robot, acceleration thresholds of each joint of the robot, and servo parameters (the number of single-circle pulses and the reduction ratio) of each joint of the robot;
s2, inputting a robot track program, and segmenting the track program according to preset conditions;
s3, allocating independent memory areas and computing resources for non-real-time discrete computation, inputting the track segment in S2 into the non-real-time discrete computation, carrying out track planning on a track instruction, generating a discrete track point array, and storing the discrete track point array in a 'one' shared memory area;
and S4, allocating independent memory areas and computing resources for real-time interpolation computing, acquiring servo feedback data and controller signals in real time, acquiring discrete track points from the first shared memory area, computing real-time interpolation points according to control signals, generating an interpolation point array, and storing the interpolation point array in the second shared memory area.
The first and second numbers are only used for distinguishing two shared memory areas, two shared memory areas are established, one shared memory area is used for storing a discrete track point array, discrete track points are generated through non-real-time discrete calculation and stored in the discrete track point array, and the discrete track points are obtained through real-time interpolation calculation, so that data transmission and interaction between the two are guaranteed, and independent operation of the two is not influenced; and the other block is used for storing an interpolation point array, generating interpolation point data through real-time interpolation calculation and storing the interpolation point data in the interpolation point array, then acquiring interpolation points from the interpolation point data through servo real-time communication and sending the interpolation points to a servo driver to control the motion of the robot.
The discrete track point array in the step S3 is formed by packaging joint angle data, joint angular velocity data, and joint angular acceleration data in a robot joint space, and the discrete track point array is expressed as:
Figure 586514DEST_PATH_IMAGE001
(1)
wherein
Figure 503654DEST_PATH_IMAGE002
The data of the joint angle is represented,
Figure 719610DEST_PATH_IMAGE003
the data of the angular velocity of the joint is represented,
Figure 114819DEST_PATH_IMAGE004
representing the angular acceleration data of the joint, K is a lower corner mark, the value is taken from 0 to M, T is a transposed symbol of the vector,
Figure 866874DEST_PATH_IMAGE005
represents the total number of joint axes of the robot,
Figure 689337DEST_PATH_IMAGE006
representing the total number of trace points.
In step S4, the interpolation point array is formed by encapsulating interpolation point data, and the interpolation point array is expressed as:
Figure 894053DEST_PATH_IMAGE007
（2）
wherein
Figure 92953DEST_PATH_IMAGE008
It is shown that the interpolation point data,
Figure 761832DEST_PATH_IMAGE009
the number of the discrete track point where the interpolation point is positioned is shown,
Figure 692879DEST_PATH_IMAGE010
when indicates the first
Figure 447208DEST_PATH_IMAGE011
A interpolation point is positioned at
Figure 184220DEST_PATH_IMAGE012
A discrete locus point and the first
Figure 143824DEST_PATH_IMAGE013
Between the discrete points of the track, the distance between the discrete points,
Figure 573668DEST_PATH_IMAGE014
indicates the total number of interpolation points.
The non-real-time discrete calculation in step S3 includes: and (3) completing trajectory instruction analysis, trajectory smoothing, trajectory speed planning and trajectory pre-dispersion, generating a discrete trajectory point array, putting a complex trajectory optimization algorithm into non-real-time discrete calculation, enabling the trajectory planning to be separated from real-time control circulation, and removing the limitation of the operation speed and interpolation cycle beat of a computer.
The non-real-time discrete calculation in step S3 specifically includes the following steps:
s31, analyzing the track instructions in sequence, obtaining motion instruction information including keywords, track points, speed, acceleration, user coordinates and tool coordinates of the instructions, setting the default values when partial parameters are default in the instructions, and storing the track information as an original track parameter structure array;
the original track parameter structure body is formed by packaging track types, track points, speeds, accelerations, user coordinates and tool coordinates, and is expressed as follows:
Figure 815293DEST_PATH_IMAGE015
(3)
wherein
Figure 28100DEST_PATH_IMAGE016
The type of the track is represented, and comprises a joint command, a straight line command, a circular arc command and the like,
Figure 671571DEST_PATH_IMAGE017
the data representing the points of the track is,
Figure 944420DEST_PATH_IMAGE018
the speed data representing the trajectory is then calculated,
Figure 673342DEST_PATH_IMAGE019
acceleration data representing the trajectory of the object is,
Figure 752156DEST_PATH_IMAGE020
user coordinate data representing the trajectory of the object,
Figure 187817DEST_PATH_IMAGE021
tool coordinate data representing a trajectory; n represents the number of track segments;
s32, carrying out track smoothing on the original track parameter structure array obtained in S31, calculating the track of the transition section between two sections of linear track sections, and ensuring that G1 is continuous between the linear track and the track of the transition section, wherein the track smoothing method comprises the following steps of arc smoothing, spline smoothing and the like, which are not repeated herein, and the track smoothing process can be represented as follows:
Figure 959464DEST_PATH_IMAGE022
(4)
wherein
Figure 910102DEST_PATH_IMAGE023
To represent
Figure 252264DEST_PATH_IMAGE024
The linear trajectory expression in between is used to,
Figure 604747DEST_PATH_IMAGE025
to represent
Figure 547296DEST_PATH_IMAGE026
The corresponding transition track expression is expressed by the transition track expression,
Figure 922913DEST_PATH_IMAGE027
represents a smoothing coefficient;
Figure 343530DEST_PATH_IMAGE024
all represent the number of track points and N track segments;
and S33, performing speed planning on the track array obtained in the S32, enabling a function of the track with respect to time to reach first-order differential continuity under the conditions of known speed parameters and acceleration parameters, ensuring that the first-order differential of the connection part of the linear track and the transition track is equal, and storing a track smooth result and a speed planning result as a smooth track parameter structure array.
There are various speed planning methods, including trapezoidal speed planning, polynomial interpolation, etc., which are not described herein again.
The smooth track parameter structure body is formed by packaging a geometric expression and a speed function expression of a track, and is expressed as follows:
Figure 816100DEST_PATH_IMAGE028
(5)
wherein
Figure 867233DEST_PATH_IMAGE029
The geometric expression representing the trajectory includes two forms:
Figure 792463DEST_PATH_IMAGE030
represents a linear trajectory;
Figure 16771DEST_PATH_IMAGE031
the transition trajectory is represented by a graph of the transition trajectory,
Figure 15951DEST_PATH_IMAGE032
representing the velocity function expression, i represents the subscript, N represents the number of track segments, and t represents time.
Two speed planning processes for a trajectory can be represented as:
Figure 300302DEST_PATH_IMAGE033
(6)
Figure 712829DEST_PATH_IMAGE034
(7)
and S34, pre-dispersing the smooth track parameter structure array obtained in the S33, calculating discrete track points and performing inverse kinematics solution, and outputting a discrete track point array.
The discrete track point calculation is not based on a fixed time interval, but based on pose precision and speed variation, the chord height difference between the discrete track and the original track is ensured to be within a threshold range, and the speed variation between the front discrete point and the rear discrete point is ensured to be within the threshold range. The chord height difference includes a position chord height difference and an attitude chord height difference, D in the diagram shown in fig. 7 represents the position chord height difference, and the threshold value, i.e. the maximum critical value that can be reached, is determined by the trajectory precision and is generally set by the user according to the robot trajectory precision.
The inverse kinematics solution is only directed to the trajectory in the cartesian space, and the process is as follows: and establishing a robot kinematics model through the set robot kinematics parameters, establishing a kinematics equation according to the pose data in the Cartesian space, and reversely solving each joint angle corresponding to the locus point in the Cartesian space.
The pre-dispersion of the trajectory can be expressed as:
Figure 177046DEST_PATH_IMAGE035
（8）
the non-real-time discrete calculation is carried out in a non-real-time interpolation central processing unit, is independent of a real-time interpolation period, is not limited by the real-time interpolation period, can freely control the time of circular calculation, and can be added with a complex track planning algorithm.
The non-real-time discrete computation in S3 may implement parallel computation, that is, multiple non-real-time computations are performed in parallel. The method comprises the steps of calculating an interception point according to preset conditions, dividing the whole track into a plurality of sections of tracks, enabling non-real-time discrete calculation of each section of track to be mutually independent, inputting the non-real-time discrete calculation into different non-real-time interpolation threads to realize parallel calculation, and greatly improving the calculation efficiency of non-real-time interpolation.
In step S4, the real-time interpolation calculation is responsible for generating interpolation point data, and the interpolation point data is stored in the "second" shared memory area where the interpolation point array is located. The real-time interpolation calculation is set to be an independent thread which is independent of a main thread of servo communication, the main thread of servo communication is responsible for acquiring data from the interpolation point array and sending the data to the servo driver, and feedback data is acquired from the servo driver and used for real-time interpolation calculation. Both are performed in real time at the same cycle.
The real-time interpolation calculation is to calculate interpolation points based on the discrete track point array, and select different interpolation methods according to different control signals. The interpolation method comprises conventional interpolation, drop-stop interpolation and restart interpolation, wherein the conventional interpolation is interpolation point calculation according to discrete track points, the drop-stop interpolation is interpolation point calculation for controlling the robot to stop moving in the moving process, and the restart interpolation is interpolation point calculation which is started again at the stop position and is sequentially performed according to discrete track point arrays without performing non-real-time discrete calculation again.
The real-time interpolation calculation substep is as follows:
s41, inputting discrete track point array (joint angle)
Figure 358629DEST_PATH_IMAGE036
Angular velocity of joint
Figure 548302DEST_PATH_IMAGE037
) Number of discrete track points
Figure 385808DEST_PATH_IMAGE013
(in the initial state
Figure 889601DEST_PATH_IMAGE038
) Current joint angle fed back by servo communication thread
Figure 191270DEST_PATH_IMAGE039
And current joint angular velocity
Figure 551844DEST_PATH_IMAGE040
；
S42, acquiring a control signal of the controller;
s43, when the control signal is stop and the operation state is normal or restart, obtaining the interpolation point corresponding to the current joint angle
Figure 876646DEST_PATH_IMAGE041
Value, number current discrete trace points
Figure 246447DEST_PATH_IMAGE013
Is arranged as
Figure 573261DEST_PATH_IMAGE042
Then clear all of
Figure 370316DEST_PATH_IMAGE043
The interpolation point data of (1). Otherwise, the cleaning is not needed;
s44, updating the operation state: when the control signal is stop, the running state is set to be down. When the control signal is running and the current running state is stopped, the running state is set to restart. When the control signal is running and the current running state is restarting or normal, no change is needed;
s45, judging the current operation state: when the operation state is normal, the routine proceeds to S46. Entering S47 when the running state is the drop stop, and entering S48 when the running state is the restart;
s46, performing conventional interpolation calculation, and then entering S410;
s47, judging whether the endpoint speed of the interpolation point array is 0: if yes, the real-time interpolation calculation is ended. If not, performing descending stop interpolation calculation, and then entering S410;
s48, carrying out restart interpolation calculation, and then entering S49;
s49, judging the end point joint angular velocity
Figure 979152DEST_PATH_IMAGE044
: if the angular velocity of the joint is at the end
Figure 90327DEST_PATH_IMAGE044
Is equal to
Figure 101009DEST_PATH_IMAGE013
Angular velocity of joint at discrete track points
Figure 68965DEST_PATH_IMAGE037
Then modify the fortuneThe row state is normal;
s410, updating the number of the discrete track points:
Figure 102780DEST_PATH_IMAGE045
；
and S411, adding the calculated interpolation data to the interpolation point array.
The conventional interpolation calculation method in step S46 is as follows:
s461, input first
Figure 814384DEST_PATH_IMAGE012
Joint angle of discrete track point
Figure 617255DEST_PATH_IMAGE046
And joint angular velocity
Figure 756112DEST_PATH_IMAGE047
As the starting joint angle
Figure 339540DEST_PATH_IMAGE048
And joint angular velocity
Figure 25474DEST_PATH_IMAGE049
Input the first
Figure 10748DEST_PATH_IMAGE013
Joint angle of discrete track point
Figure 320506DEST_PATH_IMAGE036
And joint angular velocity
Figure 328914DEST_PATH_IMAGE037
As end point joint angle
Figure 382320DEST_PATH_IMAGE050
And joint angular velocity
Figure 222100DEST_PATH_IMAGE044
：
Figure 640443DEST_PATH_IMAGE051
（9）；
S462, establishing a linear parameterized trajectory equation as follows, and calculating a parametric velocity,
Figure 932884DEST_PATH_IMAGE052
which represents the speed of the parameter of origin,
Figure 727665DEST_PATH_IMAGE053
represents the endpoint parameter speed:
Figure 421951DEST_PATH_IMAGE054
(10)；
s463, pair parameters
Figure 73513DEST_PATH_IMAGE055
And (3) performing trapezoidal speed planning:
Figure 555047DEST_PATH_IMAGE056
(11)；
s464 according to interpolation period
Figure 950257DEST_PATH_IMAGE057
Calculating interpolation point data
Figure 436733DEST_PATH_IMAGE007
：
Figure 524774DEST_PATH_IMAGE058
(12)。
The method of the drop-stop interpolation calculation in step S47 is as follows:
s471, input
Figure 729491DEST_PATH_IMAGE012
Joint angle of discrete track point
Figure 928391DEST_PATH_IMAGE046
And joint angular velocity
Figure 331690DEST_PATH_IMAGE047
Of 1 at
Figure 528317DEST_PATH_IMAGE013
Discrete track point joint angle
Figure 282646DEST_PATH_IMAGE036
And joint angular velocity
Figure 455876DEST_PATH_IMAGE037
Threshold value of acceleration of joint
Figure 979261DEST_PATH_IMAGE059
. To be provided with
Figure 409106DEST_PATH_IMAGE046
Is the initial joint angle
Figure 322835DEST_PATH_IMAGE048
To do so by
Figure 863538DEST_PATH_IMAGE036
Is the end point of the joint angle
Figure 444692DEST_PATH_IMAGE050
. To be provided with
Figure 779858DEST_PATH_IMAGE047
To start the angular velocity of the joint
Figure 446463DEST_PATH_IMAGE049
：
Figure 525277DEST_PATH_IMAGE060
(13)；
S472, calculating
Figure 23255DEST_PATH_IMAGE013
Joint angular velocity of discrete track points
Figure 965541DEST_PATH_IMAGE061
I.e. the end point joint angular velocity
Figure 181758DEST_PATH_IMAGE044
And storing the data into a discrete track point array; suppose that
Figure 1947DEST_PATH_IMAGE062
，
Figure 354431DEST_PATH_IMAGE063
For the scale factor, then for the 1 st joint:
Figure 234662DEST_PATH_IMAGE064
(14)
Figure 610280DEST_PATH_IMAGE065
(15)；
for a multi-axis robot, the following are available:
Figure 30897DEST_PATH_IMAGE066
(16)
Figure 939685DEST_PATH_IMAGE067
(17)
Figure 787555DEST_PATH_IMAGE068
(18)；
s473, establishing a linear parameterized trajectory equation and calculating a parametric speed, wherein
Figure 712786DEST_PATH_IMAGE052
Which is indicative of the speed of the starting parameter,
Figure 874777DEST_PATH_IMAGE053
represents the endpoint parameter speed:
Figure 201853DEST_PATH_IMAGE069
(19)；
s474, parameter pair
Figure 423887DEST_PATH_IMAGE055
And (3) performing trapezoidal speed planning:
Figure 836414DEST_PATH_IMAGE070
(20)；
s475 according to interpolation period
Figure 536516DEST_PATH_IMAGE057
Calculating interpolation point data
Figure 718099DEST_PATH_IMAGE007
The calculation formula is as follows:
Figure 609569DEST_PATH_IMAGE071
(21)。
the restart interpolation calculation method in step S48 is as follows:
s481, inputting
Figure 509392DEST_PATH_IMAGE012
Joint angle of discrete track point
Figure 75503DEST_PATH_IMAGE046
And joint angular velocity
Figure 49275DEST_PATH_IMAGE047
Of 1 at
Figure 675429DEST_PATH_IMAGE013
Discrete track point joint angle
Figure 231DEST_PATH_IMAGE036
Angular velocity of joint
Figure 104453DEST_PATH_IMAGE037
Threshold value of acceleration of joint
Figure 260628DEST_PATH_IMAGE059
To do so by
Figure 995366DEST_PATH_IMAGE046
Is the initial joint angle
Figure 604201DEST_PATH_IMAGE048
To do so by
Figure 213912DEST_PATH_IMAGE036
Is the end point of the joint angle
Figure 224593DEST_PATH_IMAGE050
To do so by
Figure 192549DEST_PATH_IMAGE047
Is the initial joint angle
Figure 226364DEST_PATH_IMAGE049
：
Figure 672389DEST_PATH_IMAGE060
(22)；
S482, calculating
Figure 740839DEST_PATH_IMAGE013
Joint angular velocity of discrete track points
Figure 879697DEST_PATH_IMAGE061
I.e. the end point joint angular velocity
Figure 463125DEST_PATH_IMAGE044
And storing the data into a discrete track point array; suppose that
Figure 650524DEST_PATH_IMAGE062
Then, for the 1 st joint:
Figure 635797DEST_PATH_IMAGE072
(23)
Figure 381774DEST_PATH_IMAGE073
(24)；
for a multi-axis robot, the following are available:
Figure 452498DEST_PATH_IMAGE074
(25)
Figure 443588DEST_PATH_IMAGE075
(26)
Figure 283368DEST_PATH_IMAGE068
(27)；
s483, establishing a linear parameterized trajectory equation, calculating parameter speed,
Figure 764028DEST_PATH_IMAGE052
in order to start the speed of the parameter,
Figure 994152DEST_PATH_IMAGE076
setting a preset endpoint parameter speed;
Figure 851250DEST_PATH_IMAGE069
(28)；
s484, parameter pair
Figure 545536DEST_PATH_IMAGE055
And (3) performing trapezoidal speed planning:
Figure 134781DEST_PATH_IMAGE070
(29)；
s485, according to the interpolation period
Figure 180097DEST_PATH_IMAGE077
Calculating interpolation point data
Figure 11524DEST_PATH_IMAGE007
：
Figure 560317DEST_PATH_IMAGE078
(30)。
The invention has the following advantages:
1. the invention removes the limitation of the computer operation speed and the interpolation period beat, puts the complex track planning algorithm in the traditional real-time interpolation calculation into the non-real-time discrete calculation of the invention, leads the track planning to be separated from the time limitation of the real-time interpolation period, and improves the integral performance of the interpolation algorithm.
2. The invention improves the overall interpolation calculation efficiency. The non-real-time discrete computation and the real-time interpolation computation are realized in a parallel computation mode, and after the track program is segmented, the non-real-time discrete computation of different track segments can also realize the parallel computation, so that the interpolation computation efficiency of the whole track is improved.
3. The invention reduces the calculation amount of the real-time interpolation calculation and improves the efficiency of the real-time interpolation calculation. The real-time interpolation calculation is based on interpolation calculation performed by discrete track points, the discrete track points are data obtained by discretization after track planning, and the discrete track points contain track points and speed information, so that the real-time interpolation calculation can calculate and implement interpolation points only by performing simple speed planning between the discrete track points, the calculated amount is reduced, and the calculation efficiency is improved.
Drawings
Fig. 1 is a general flowchart of non-real-time and real-time hybrid interpolation calculation according to the present invention.
FIG. 2 is a schematic flow chart of non-real-time discrete computation.
Fig. 3 is a schematic flow chart of the real-time interpolation calculation.
Fig. 4 is a flowchart of a conventional interpolation calculation.
Fig. 5 is a flowchart of the descent interpolation calculation.
Fig. 6 is a flowchart of restart interpolation calculation.
Fig. 7 is a schematic diagram of pre-discretization of a trajectory.
Detailed Description
The technical scheme of the invention is further specifically described by the following embodiments and the accompanying drawings. Using the TURIN robot as an example:
Figure 648359DEST_PATH_IMAGE079
the above is a track program file of the TURIN robot, based on the interpolation calculation method of the robot of the present invention, and the specific implementation steps are explained by combining with specific cases:
the invention divides the interpolation algorithm of the industrial robot into two parts for carrying out: the non-real-time discrete calculation and the real-time interpolation calculation are independent from each other, and independent calculation resources are adopted, so that independent memory resources are occupied. As shown in fig. 1, the method specifically comprises the following steps:
s1 setting robot parameters including kinematic parameters (DH parameters) and robot joint speed threshold
Figure 853076DEST_PATH_IMAGE080
Threshold value of acceleration of joint
Figure 786397DEST_PATH_IMAGE081
The preset conditions of track segmentation, the servo parameters (the number of single-circle pulses and the reduction ratio) of the robot joint and the like. The step can be set for robots of different models once.
And S2, inputting a robot track program file, and segmenting the track program according to preset conditions. The trace program instruction in the case of fig. 7 is a TURIN-format instruction, the present invention includes but is not limited to a TURIN robot instruction, and the formats of other robot instructions are similar to this, which is not repeated herein. ML in the above-described trajectory program file indicates an abbreviation of MoveLine, i.e., a linear motion instruction.
S3 allocates separate memory regions and computing resources for non-real-time discrete computations. And sequentially acquiring the track instructions of the robot, inputting the track instructions into non-real-time discrete calculation, performing complex track planning calculation on the track instructions, generating a discrete track array, and storing the discrete track array in a shared memory area.
S4 independent memory areas and computing resources are distributed for real-time interpolation computing, servo feedback data and controller signals are obtained in real time, discrete track points are obtained from a shared memory, real-time interpolation points are computed according to control signals, servo motion instructions are generated and sent to a robot servo control system, and the robot is controlled to run according to a target track.
The invention establishes two shared memory areas: one block is used for saving the discrete track point array, the discrete track points are generated through non-real-time discrete calculation and are saved in the discrete track point array, and the discrete track points are obtained through real-time interpolation calculation, so that data transmission and interaction between the discrete track points are guaranteed, and independent operation of the discrete track points and the discrete track points is not influenced. And the other block is used for storing an interpolation point array, generating interpolation point data through real-time interpolation calculation, storing the interpolation point data in the interpolation point array, acquiring interpolation points from the interpolation point data through real-time communication, and sending the interpolation points to the servo drive to control the robot to move. As shown in fig. 1.
The discrete track point array is formed by packaging joint angle data, joint angular velocity data and joint angular acceleration data in a robot joint space. The discrete trace point array may be represented as:
Figure 392958DEST_PATH_IMAGE082
wherein
Figure 651901DEST_PATH_IMAGE002
Indicating jointThe angle data is used to determine the angle of the object,
Figure 140652DEST_PATH_IMAGE003
the data of the angular velocity of the joint is represented,
Figure 80926DEST_PATH_IMAGE004
represents the angular acceleration data of the joint,
Figure 604311DEST_PATH_IMAGE005
represents the total number of joint axes of the robot,
Figure 204794DEST_PATH_IMAGE006
representing the total number of trace points.
The interpolation point array is formed by data encapsulation of interpolation points, and can be expressed as:
Figure 446420DEST_PATH_IMAGE083
wherein
Figure 987123DEST_PATH_IMAGE008
It is shown that the interpolation point data,
Figure 302697DEST_PATH_IMAGE009
the number of the discrete track point where the interpolation point is positioned is shown,
Figure 903443DEST_PATH_IMAGE010
when indicates the first
Figure 304468DEST_PATH_IMAGE011
A interpolation point is positioned at
Figure 648862DEST_PATH_IMAGE012
A discrete locus point and the first
Figure 146840DEST_PATH_IMAGE013
Between the discrete points of the track, the distance between the discrete points,
Figure 590590DEST_PATH_IMAGE014
indicates the total number of interpolation points.
In step S3, the non-real-time discrete computation mainly completes the trajectory instruction analysis, the trajectory smoothing, the trajectory speed planning, and the trajectory pre-dispersion, and generates a discrete trajectory point array. A complex track optimization algorithm is put into non-real-time discrete calculation, so that track planning is separated from real-time control circulation, and the limitation of the calculation speed and the interpolation period beat of a computer is removed. As shown in fig. 2, the non-real-time discrete computation performs complex trajectory planning computation, and its input is a continuous trajectory instruction and its output is a discrete trajectory point array.
The non-real-time discrete computation substep in step S3 is as follows:
s31 sequentially analyzes the track instruction, obtains motion instruction information including the keyword, track point, speed, acceleration, smoothness, user coordinate and tool coordinate of the instruction, sets the default value when the default partial parameters are included in the instruction, and stores the track information as the original track parameter structure array.
The array of original trajectory parameter structures of the TURIN robot can be represented as follows:
Figure 806808DEST_PATH_IMAGE084
ML is represented as a straight-line trajectory instruction,
Figure 125532DEST_PATH_IMAGE017
is the data of the track points, and the data of the track points,
Figure 478016DEST_PATH_IMAGE018
the speed data representing the trajectory is then calculated,
Figure 92668DEST_PATH_IMAGE019
acceleration data representing the trajectory of the object is,
Figure 530602DEST_PATH_IMAGE020
user coordinate data representing the trajectory of the object,
Figure 216799DEST_PATH_IMAGE021
tool coordinate data representing a trajectory, e.g. data of the first row
Figure 361472DEST_PATH_IMAGE085
，
Figure 474922DEST_PATH_IMAGE086
(representing the percentage of maximum velocity of the joint),
Figure 337835DEST_PATH_IMAGE087
(representing the percentage of maximum acceleration of the joint),
Figure 562143DEST_PATH_IMAGE088
，
Figure 889219DEST_PATH_IMAGE089
。
and S32, performing track smoothing on the first input parameter structure array obtained in S31, calculating a transition section track between two sections of linear track sections, and ensuring that G1 continuity is achieved between the linear track and the transition section track, wherein multiple track smoothing methods are provided and are not repeated herein.
And S33, performing speed planning on the track array obtained in the S32, and enabling the function of the track with respect to time to reach first order differential continuity under the condition of known speed parameters and acceleration parameters, and simultaneously ensuring that the first order differential of the connection part of the linear track and the transition track is equal. And storing the smooth track result and the speed planning result together into a smooth track parameter structure array. There are various speed planning methods, including trapezoidal speed planning, polynomial interpolation, etc., which are not described herein in detail.
S34 pre-dispersing the optimized array obtained in S33 in sequence, including calculation of discrete track points and inverse kinematics, and outputting the discrete track array
Figure 609789DEST_PATH_IMAGE001
. As shown in FIG. 7, the discrete trace point calculation of this step is not a fixed timeThe interval is taken as the basis, but the pose precision and the speed variation are taken as the calculation basis, so that the chord height difference between the discrete track and the original track is ensured to be at the threshold value
Figure 756736DEST_PATH_IMAGE090
Within the range, the variation of the speed between the front discrete point and the rear discrete point is ensured to be within the threshold range.
The inverse kinematics solution process in step S34 is: and establishing a robot kinematics model through the robot kinematics parameters set in the S1, establishing a kinematics equation according to the pose data in the Cartesian space, and reversely solving each joint angle.
And establishing a plurality of non-real-time discrete computing threads, and inputting the track program in the embodiment into different non-real-time discrete threads for computing after segmentation processing.
As shown in fig. 3, the discrete trace point array output in S3 is input to a real-time interpolation calculation, and different interpolation methods are selected according to different control signals. The interpolation method comprises conventional interpolation, drop-stop interpolation and restart interpolation.
The real-time interpolation calculation sub-step is as follows:
s41, input the first
Figure 722418DEST_PATH_IMAGE013
Discrete track points (joint angle)
Figure 904001DEST_PATH_IMAGE036
Angular velocity of joint
Figure 359253DEST_PATH_IMAGE037
At the initial state
Figure 196759DEST_PATH_IMAGE038
) Current joint angle fed back by servo communication thread
Figure 762869DEST_PATH_IMAGE039
And current joint angular velocity
Figure 736642DEST_PATH_IMAGE040
。
And S42, acquiring a control signal of the controller.
S43, when the control signal is stop and the operation state is normal or restart, obtaining the interpolation point corresponding to the current joint angle
Figure 362795DEST_PATH_IMAGE041
Value, number current discrete trace points
Figure 920553DEST_PATH_IMAGE013
Is arranged as
Figure 290354DEST_PATH_IMAGE042
Then clear all of
Figure 384212DEST_PATH_IMAGE043
The interpolation point data of (1). Otherwise, no purge is used.
S44, updating the operation state: when the control signal is stop, the running state is set to be down. When the control signal is running and the current running state is stopped, the running state is set to restart. When the control signal is active and the current operating state is reset or normal, no change is required.
S45, judging the current operation state: when the operation state is normal, the routine proceeds to S46. When the operation state is the descent control, the routine proceeds to S47. When the operation state is restart, S48 is entered.
S46, a normal interpolation calculation is performed, and the process proceeds to S410.
S47, judging whether the initial joint angular velocity is 0: if yes, the real-time interpolation calculation is ended. If not, the process proceeds to step S410 after performing the descent interpolation calculation.
S48, restart interpolation calculation is performed, and the process then proceeds to S49.
S49, judging the end point joint angular velocity
Figure 181267DEST_PATH_IMAGE044
: if the angular velocity of the joint is at the end
Figure 790103DEST_PATH_IMAGE044
Is equal to
Figure 635699DEST_PATH_IMAGE013
Angular velocity of joint at discrete track points
Figure 646381DEST_PATH_IMAGE037
Then the modified operating state is normal.
S410, updating the number of the discrete track points:
Figure 552020DEST_PATH_IMAGE045
。
and S411, adding the calculated interpolation data to the interpolation point array.
The protective scope of the present invention is not limited to the above-described embodiments, and it is apparent that various modifications and variations can be made to the present invention by those skilled in the art without departing from the scope and spirit of the present invention. It is intended that the present invention cover the modifications and variations of this invention provided they come within the scope of the appended claims and their equivalents.

Claims (10)

1. A non-real-time and real-time mixed interpolation calculation method for an industrial robot is characterized by comprising the following steps: the interpolation calculation of an industrial robot motion control system is divided into a non-real-time discrete calculation part and a real-time interpolation calculation part, the non-real-time discrete calculation part and the real-time interpolation calculation part are mutually independent, an independent central processing unit is adopted to occupy independent memory resources, a track instruction is converted into a real-time servo instruction through the mixed calculation of the two parts, and the motion control of the robot is realized, and the method specifically comprises the following steps:
s1, robot parameters are set, wherein the robot parameters comprise kinematic parameters of the robot, limit values of each joint of the robot, speed thresholds of each joint of the robot, acceleration thresholds of each joint of the robot and servo parameters of each joint of the robot;
s2, inputting a robot track program, and segmenting the track program according to preset conditions;
s3, allocating independent memory areas and computing resources for non-real-time discrete computation, inputting the track segment in S2 into the non-real-time discrete computation, carrying out track planning on a track instruction, generating a discrete track point array, and storing the discrete track point array in a 'one' shared memory area;
and S4, allocating independent memory areas and computing resources for real-time interpolation computing, acquiring servo feedback data and controller signals in real time, acquiring discrete track points from the first shared memory area, computing real-time interpolation points according to control signals, generating an interpolation point array, and storing the interpolation point array in the second shared memory area.
2. The interpolation calculation method for the non-real-time and real-time mixed industrial robot according to claim 1, wherein: the discrete track point array in the step S3 is formed by packaging joint angle data, joint angular velocity data, and joint angular acceleration data in a robot joint space, and the discrete track point array is expressed as:
Figure 192906DEST_PATH_IMAGE001
(1)
wherein
Figure 904510DEST_PATH_IMAGE002
The data of the joint angle is represented,
Figure 596129DEST_PATH_IMAGE003
the data of the angular velocity of the joint is represented,
Figure 734986DEST_PATH_IMAGE004
representing the angular acceleration data of the joint, K is a lower corner mark, the value is taken from 0 to M, T is a transposed symbol of the vector,
Figure 115152DEST_PATH_IMAGE005
represents the total number of joint axes of the robot,
Figure 630447DEST_PATH_IMAGE006
representing the total number of trace points.
3. The interpolation calculation method for the non-real-time and real-time mixed industrial robot according to claim 1, wherein: the non-real-time discrete calculation in step S3 includes: completing track instruction analysis, track smoothing, track speed planning and track pre-dispersion, and generating a discrete track point array;
the non-real-time discrete calculation in step S3 specifically includes the following steps:
s31, analyzing the track instructions in sequence, obtaining motion instruction information including keywords, track points, speed, acceleration, user coordinates and tool coordinates of the instructions, setting the default values when partial parameters are default in the instructions, and storing the track information as an original track parameter structure array;
the original track parameter structure body is formed by packaging track types, track points, speeds, accelerations, user coordinates and tool coordinates, and is expressed as follows:
Figure 615720DEST_PATH_IMAGE007
(3)
wherein
Figure 987796DEST_PATH_IMAGE008
The type of the track is represented, and comprises a joint command, a straight line command, a circular arc command and the like,
Figure 792941DEST_PATH_IMAGE009
the data representing the points of the track is,
Figure 908664DEST_PATH_IMAGE010
the speed data representing the trajectory is then calculated,
Figure 436860DEST_PATH_IMAGE011
acceleration data representing the trajectory of the object is,
Figure 917520DEST_PATH_IMAGE012
user coordinate data representing the trajectory of the object,
Figure 537857DEST_PATH_IMAGE013
tool coordinate data representing a trajectory; n represents the number of track segments;
s32, performing track smoothing on the original track parameter structure array obtained in the S31, calculating a transition section track between two sections of linear track sections, and ensuring that G1 continuity is achieved between the linear track and the transition section track, wherein the track smoothing process is represented as follows:
Figure 129375DEST_PATH_IMAGE014
(4)
wherein
Figure 89241DEST_PATH_IMAGE015
To represent
Figure 325092DEST_PATH_IMAGE016
The linear trajectory expression in between is used to,
Figure 104829DEST_PATH_IMAGE017
to represent
Figure 500038DEST_PATH_IMAGE018
The corresponding transition track expression is expressed by the transition track expression,
Figure 376728DEST_PATH_IMAGE019
represents a smoothing coefficient;
Figure 199190DEST_PATH_IMAGE016
all represent the number of track points and N track segments;
s33, carrying out speed planning on the track array obtained in the S32, and storing the result of smooth track and the result of speed planning as a smooth track parameter structure array;
and S34, pre-dispersing the smooth track parameter structure array obtained in the S33, calculating discrete track points and performing inverse kinematics solution, and outputting a discrete track point array.
4. A non-real time and real time hybrid industrial robot interpolation calculation method according to claim 3, wherein: in the step S34, the calculation of the discrete trajectory point uses pose accuracy and speed variation as calculation basis, the chord height difference between the discrete trajectory and the original trajectory is within a threshold range, and the variation of the speed between the front discrete point and the rear discrete point is within the threshold range, wherein the chord height difference includes position chord height difference and posture chord height difference; the inverse kinematics solution is only directed to the trajectory in the cartesian space, and the process is as follows: and establishing a robot kinematics model through the set robot kinematics parameters, establishing a kinematics equation according to the pose data in the Cartesian space, and reversely solving each joint angle corresponding to the locus point in the Cartesian space.
5. The interpolation calculation method for the non-real-time and real-time mixed industrial robot according to claim 1, wherein: in the step S3, the non-real-time discrete computation may implement parallel computation, that is, multiple non-real-time computations are performed in parallel, the truncation point is computed according to a preset condition, the entire trajectory is divided into multiple sections of trajectories, the non-real-time discrete computations of each section of trajectory are independent of each other, and the trajectories are input into different non-real-time interpolation threads to implement parallel computation.
6. The interpolation calculation method for the non-real-time and real-time mixed industrial robot according to claim 1, wherein: in step S4, the interpolation point array is formed by encapsulating interpolation point data, and the interpolation point array is expressed as:
Figure 466223DEST_PATH_IMAGE020
（2）
wherein
Figure 727440DEST_PATH_IMAGE021
It is shown that the interpolation point data,
Figure 396319DEST_PATH_IMAGE022
the number of the discrete track point where the interpolation point is positioned is shown,
Figure 452000DEST_PATH_IMAGE023
when indicates the first
Figure 206329DEST_PATH_IMAGE024
A interpolation point is positioned at
Figure 208920DEST_PATH_IMAGE025
A discrete locus point and the first
Figure 296087DEST_PATH_IMAGE026
Between the discrete points of the track, the distance between the discrete points,
Figure 460352DEST_PATH_IMAGE027
indicates the total number of interpolation points.
7. The interpolation calculation method for the non-real-time and real-time mixed industrial robot according to claim 1, wherein: in the step S4, the real-time interpolation calculation is responsible for generating interpolation point data and storing the interpolation point data in the shared memory region "two", the real-time interpolation calculation is set as an independent thread and is independent of the main thread of the servo communication, the main thread of the servo communication is responsible for acquiring data from the interpolation point array and sending the data to the servo drive, and the feedback data is acquired from the servo drive and used for the real-time interpolation calculation, and the two are executed in real time in the same period;
the real-time interpolation calculation is to calculate interpolation points based on a discrete track point array, and select different interpolation methods according to different control signals; the interpolation method comprises conventional interpolation, drop-stop interpolation and restart interpolation, wherein the conventional interpolation is interpolation point calculation according to discrete track points, the drop-stop interpolation is interpolation point calculation for controlling the robot to stop moving in the moving process, and the restart interpolation is to restart the interpolation point calculation in sequence according to discrete track point arrays at the stop without performing non-real-time discrete calculation again;
the real-time interpolation calculation substep is as follows:
s41, inputting a discrete track point array: joint angle
Figure 764295DEST_PATH_IMAGE028
Angular velocity of joint
Figure 304997DEST_PATH_IMAGE029
Number of discrete track points
Figure 682889DEST_PATH_IMAGE026
At the initial state
Figure 345952DEST_PATH_IMAGE030
Current joint angle fed back by servo communication thread
Figure 809294DEST_PATH_IMAGE031
And current joint angular velocity
Figure 153688DEST_PATH_IMAGE032
；
S42, acquiring a control signal of the controller;
s43, when the control signal is stop and the operation state is normal or restart, obtaining the interpolation point corresponding to the current joint angle
Figure 448403DEST_PATH_IMAGE033
Value, number current discrete trace points
Figure 220050DEST_PATH_IMAGE026
Is arranged as
Figure 731540DEST_PATH_IMAGE034
Then clear all of
Figure 614046DEST_PATH_IMAGE035
The interpolation point data of (2) is not cleared in other cases;
s44, updating the operation state: when the control signal is stopped, the running state is set to be stopped, when the control signal is running and the current running state is stopped, the running state is set to be restarted, and when the control signal is running and the current running state is restarted or conventional, the running state does not need to be changed;
s45, judging the current operation state: the method comprises the following steps of entering S46 when the operation state is normal, entering S47 when the operation state is descent stop, and entering S48 when the operation state is restart;
s46, performing conventional interpolation calculation, and then entering S410;
s47, judging whether the endpoint speed of the interpolation point array is 0: if yes, ending the real-time interpolation calculation, if no, performing descending stop interpolation calculation, and then entering S410;
s48, carrying out restart interpolation calculation, and then entering S49;
s49, judging the end point joint angular velocity
Figure 966530DEST_PATH_IMAGE036
: if the angular velocity of the joint is at the end
Figure 971395DEST_PATH_IMAGE036
Is equal to
Figure 409329DEST_PATH_IMAGE026
Angular velocity of joint at discrete track points
Figure 95526DEST_PATH_IMAGE029
If so, modifying the running state to be conventional;
s410, updating the number of the discrete track points:
Figure 364833DEST_PATH_IMAGE037
；
and S411, adding the calculated interpolation data to the interpolation point array.
8. The non-real-time and real-time hybrid industrial robot interpolation calculation method according to claim 7, wherein: the conventional interpolation calculation method in step S46 is as follows:
s461, input first
Figure 478282DEST_PATH_IMAGE025
Joint angle of discrete track point
Figure 465830DEST_PATH_IMAGE038
And joint angular velocity
Figure 690138DEST_PATH_IMAGE039
As the starting joint angle
Figure 17214DEST_PATH_IMAGE040
And joint angular velocity
Figure 599767DEST_PATH_IMAGE041
Input the first
Figure 12294DEST_PATH_IMAGE026
Joint angle of discrete track point
Figure 40293DEST_PATH_IMAGE028
And joint angular velocity
Figure 284193DEST_PATH_IMAGE029
As end point joint angle
Figure 739445DEST_PATH_IMAGE042
And joint angular velocity
Figure 373688DEST_PATH_IMAGE036
：
Figure 2116DEST_PATH_IMAGE043
（9）；
S462, establishing a linear parameterized trajectory equation as follows, and calculating a parametric velocity,
Figure 303784DEST_PATH_IMAGE044
which represents the speed of the parameter of origin,
Figure 929937DEST_PATH_IMAGE045
represents the endpoint parameter speed:
Figure 113794DEST_PATH_IMAGE046
(10)；
s463, pair parameters
Figure 483596DEST_PATH_IMAGE047
And (3) performing trapezoidal speed planning:
Figure 200622DEST_PATH_IMAGE048
(11)；
s464 according to interpolation period
Figure 997677DEST_PATH_IMAGE049
Calculating interpolation point data
Figure 606513DEST_PATH_IMAGE020
：
Figure 576743DEST_PATH_IMAGE050
(12)。
9. The non-real-time and real-time hybrid industrial robot interpolation calculation method according to claim 7, wherein: the method of the drop-stop interpolation calculation in step S47 is as follows:
s471, input
Figure 587424DEST_PATH_IMAGE025
Joint angle of discrete track point
Figure 555380DEST_PATH_IMAGE038
And joint angular velocity
Figure 713829DEST_PATH_IMAGE039
Of 1 at
Figure 425433DEST_PATH_IMAGE026
Discrete track point joint angle
Figure 352938DEST_PATH_IMAGE028
And joint angular velocity
Figure 491795DEST_PATH_IMAGE029
Threshold value of acceleration of joint
Figure 639005DEST_PATH_IMAGE051
To do so by
Figure 154300DEST_PATH_IMAGE038
Is the initial joint angle
Figure 873995DEST_PATH_IMAGE040
To do so by
Figure 246070DEST_PATH_IMAGE028
Is the end point of the joint angle
Figure 316794DEST_PATH_IMAGE042
To do so by
Figure 370201DEST_PATH_IMAGE039
To start the angular velocity of the joint
Figure 272298DEST_PATH_IMAGE041
：
Figure 815275DEST_PATH_IMAGE052
(13)；
S472, calculating
Figure 373295DEST_PATH_IMAGE026
Joint angular velocity of discrete track points
Figure 525665DEST_PATH_IMAGE053
I.e. the end point joint angular velocity
Figure 219952DEST_PATH_IMAGE036
And storing the data into a discrete track point array; suppose that
Figure 137092DEST_PATH_IMAGE054
，
Figure 979146DEST_PATH_IMAGE055
For the scale factor, then for the 1 st joint:
Figure 374356DEST_PATH_IMAGE056
(14)
Figure 251045DEST_PATH_IMAGE057
(15)；
for a multi-axis robot, the following are available:
Figure 73507DEST_PATH_IMAGE058
(16)
Figure 340540DEST_PATH_IMAGE059
(17)
Figure 601758DEST_PATH_IMAGE060
(18)；
s473, establishing a linear parameterized trajectory equation and calculating a parametric speed, wherein
Figure 270636DEST_PATH_IMAGE044
Which is indicative of the speed of the starting parameter,
Figure 827782DEST_PATH_IMAGE045
represents the endpoint parameter speed:
Figure 582111DEST_PATH_IMAGE061
(19)；
s474, parameter pair
Figure 381440DEST_PATH_IMAGE047
And (3) performing trapezoidal speed planning:
Figure 904825DEST_PATH_IMAGE062
(20)；
s475 according to interpolation period
Figure 396986DEST_PATH_IMAGE049
Calculating interpolation point data
Figure 638612DEST_PATH_IMAGE020
The calculation formula is as follows:
Figure 976052DEST_PATH_IMAGE063
(21)。
10. the non-real-time and real-time hybrid industrial robot interpolation calculation method according to claim 7, wherein: the restart interpolation calculation method in step S48 is as follows:
s481, inputting
Figure 619523DEST_PATH_IMAGE025
Joint angle of discrete track point
Figure 515542DEST_PATH_IMAGE038
And joint angular velocity
Figure 244463DEST_PATH_IMAGE039
Of 1 at
Figure 385595DEST_PATH_IMAGE026
Discrete track point joint angle
Figure 883572DEST_PATH_IMAGE028
Angular velocity of joint
Figure 717536DEST_PATH_IMAGE029
Threshold value of acceleration of joint
Figure 668174DEST_PATH_IMAGE051
To do so by
Figure 612997DEST_PATH_IMAGE038
Is the initial joint angle
Figure 965481DEST_PATH_IMAGE040
To do so by
Figure 471810DEST_PATH_IMAGE028
Is the end point of the joint angle
Figure 909745DEST_PATH_IMAGE042
To do so by
Figure 392679DEST_PATH_IMAGE039
Is the initial joint angle
Figure 865249DEST_PATH_IMAGE041
：
Figure 41015DEST_PATH_IMAGE064
(22)；
S482, calculating
Figure 966246DEST_PATH_IMAGE026
Joint angular velocity of discrete track points
Figure 252871DEST_PATH_IMAGE053
I.e. the end point joint angular velocity
Figure 314367DEST_PATH_IMAGE036
And storing the data into a discrete track point array; suppose that
Figure 159570DEST_PATH_IMAGE054
Then, for the 1 st joint:
Figure 572097DEST_PATH_IMAGE065
(23)
Figure 662413DEST_PATH_IMAGE066
(24)；
for a multi-axis robot, the following are available:
Figure 843996DEST_PATH_IMAGE067
(25)
Figure 95985DEST_PATH_IMAGE068
(26)
Figure 995808DEST_PATH_IMAGE060
(27)；
s483, establishing a linear parameterized trajectory equation, calculating parameter speed,
Figure 624236DEST_PATH_IMAGE044
in order to start the speed of the parameter,
Figure 925904DEST_PATH_IMAGE069
setting a preset endpoint parameter speed;
Figure 850260DEST_PATH_IMAGE061
(28)；
s484, parameter pair
Figure 237379DEST_PATH_IMAGE047
And (3) performing trapezoidal speed planning:
Figure 669497DEST_PATH_IMAGE062
(29)；
s485, according to the interpolation period
Figure 560093DEST_PATH_IMAGE070
Calculating interpolation point data
Figure 419464DEST_PATH_IMAGE020
：
Figure 28300DEST_PATH_IMAGE071
(30)。
CN202111241737.9A 2021-10-25 2021-10-25 Non-real-time and real-time mixed interpolation calculation method for industrial robot Active CN113664839B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202111241737.9A CN113664839B (en) 2021-10-25 2021-10-25 Non-real-time and real-time mixed interpolation calculation method for industrial robot

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202111241737.9A CN113664839B (en) 2021-10-25 2021-10-25 Non-real-time and real-time mixed interpolation calculation method for industrial robot

Publications (2)

Publication Number Publication Date
CN113664839A true CN113664839A (en) 2021-11-19
CN113664839B CN113664839B (en) 2022-01-18

Family

ID=78551042

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202111241737.9A Active CN113664839B (en) 2021-10-25 2021-10-25 Non-real-time and real-time mixed interpolation calculation method for industrial robot

Country Status (1)

Country Link
CN (1) CN113664839B (en)

Citations (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US5396160A (en) * 1991-03-11 1995-03-07 General Motors Corporation Method of real-time machine path planning from a math model
CN101807065A (en) * 2010-03-26 2010-08-18 华南理工大学 Real-time task scheduling method of soft numerical control system
CN106826829A (en) * 2017-02-22 2017-06-13 武汉工程大学 A kind of industrial robot fairing trace generator method of Controllable Error
CN107775637A (en) * 2016-08-29 2018-03-09 许亚夫 A kind of knuckle formula manipulator control system
CN109933008A (en) * 2019-03-28 2019-06-25 佛山智能装备技术研究院 A kind of double interpolating methods and device of non real-time system and robot controller
CN110842920A (en) * 2019-11-12 2020-02-28 佛山智能装备技术研究院 Control and drive integrated system architecture of single processing chip
CN111015669A (en) * 2019-12-27 2020-04-17 南京埃斯顿机器人工程有限公司 Industrial robot motion stopping trajectory planning method
CN111185908A (en) * 2020-01-14 2020-05-22 深圳众为兴技术股份有限公司 Robot control method and device for recognizing friction force, robot and storage medium
CN111745651A (en) * 2020-06-15 2020-10-09 哈工大机器人（合肥）国际创新研究院 Intelligent robot operating system structure and operating method thereof
CN113253677A (en) * 2021-07-05 2021-08-13 武汉瀚迈科技有限公司 Robot motion control method combining speed optimization and feedforward compensation

Patent Citations (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US5396160A (en) * 1991-03-11 1995-03-07 General Motors Corporation Method of real-time machine path planning from a math model
CN101807065A (en) * 2010-03-26 2010-08-18 华南理工大学 Real-time task scheduling method of soft numerical control system
CN107775637A (en) * 2016-08-29 2018-03-09 许亚夫 A kind of knuckle formula manipulator control system
CN106826829A (en) * 2017-02-22 2017-06-13 武汉工程大学 A kind of industrial robot fairing trace generator method of Controllable Error
CN109933008A (en) * 2019-03-28 2019-06-25 佛山智能装备技术研究院 A kind of double interpolating methods and device of non real-time system and robot controller
CN110842920A (en) * 2019-11-12 2020-02-28 佛山智能装备技术研究院 Control and drive integrated system architecture of single processing chip
CN111015669A (en) * 2019-12-27 2020-04-17 南京埃斯顿机器人工程有限公司 Industrial robot motion stopping trajectory planning method
CN111185908A (en) * 2020-01-14 2020-05-22 深圳众为兴技术股份有限公司 Robot control method and device for recognizing friction force, robot and storage medium
CN111745651A (en) * 2020-06-15 2020-10-09 哈工大机器人（合肥）国际创新研究院 Intelligent robot operating system structure and operating method thereof
CN113253677A (en) * 2021-07-05 2021-08-13 武汉瀚迈科技有限公司 Robot motion control method combining speed optimization and feedforward compensation

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
MARTIN SˇVEJDA 等: "Interpolation method for robot trajectory planning", 《INTERNATIONAL CONFERENCE ON PROCESS CONTROL (PC)》 *

Also Published As

Publication number Publication date
CN113664839B (en) 2022-01-18

Similar Documents

Publication Publication Date Title
CN107263484B (en) Robot joint space point-to-point motion trajectory planning method
CN111791236B (en) Industrial robot Cartesian space trajectory transition method
EP3195990B1 (en) Robot trajectory generation method, apparatus, computer program and computer readable storage medium
US5396160A (en) Method of real-time machine path planning from a math model
Lavernhe et al. Optimization of 5-axis high-speed machining using a surface based approach
CN107030702B (en) A kind of method for planning track of mechanical arm
CN114952868B (en) 7-degree-of-freedom SRS (sounding reference Signal) type mechanical arm control method and device and piano playing robot
CN112975992B (en) Error-controllable robot track synchronous optimization method
CN113021356B (en) Robot track planning method and system for ingot trimming process
JP2016055404A (en) Locus generation method, locus generation device, robot device, program, and recording medium
JPS6020209A (en) Method for interpolating control of robot
CN111283683B (en) Servo tracking accelerated convergence method for robot visual feature planning track
CN113189938A (en) Speed curve planning method for continuous machining path
CN115741695A (en) Attitude track transition planning method for six-axis series industrial robot tail end
CN113084792A (en) Method for determining joint singular area, robot and storage device
CN113103240B (en) Method, device and system for realizing C2 continuous robot trajectory planning
JPH05250023A (en) Automatic route generating method for robot manipulator
CN113664839B (en) Non-real-time and real-time mixed interpolation calculation method for industrial robot
CN112207833B (en) Method and system for planning movement path, host and storage medium
CN112720472B (en) Robot trajectory planning method and device, storage medium and robot
CN111515954A (en) Method for generating high-quality motion path of mechanical arm
CN116117796A (en) Industrial robot gesture track transition and speed planning method and system
CN114952852A (en) NURBS curve speed planning method and equipment for robot and storage medium
JPH05228860A (en) Control method of robot manipulator
JP7351672B2 (en) Travel route generation 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
GR01 Patent grant
GR01 Patent grant