CN111015669B - Industrial robot motion stopping trajectory planning method - Google Patents
Industrial robot motion stopping trajectory planning method Download PDFInfo
- Publication number
- CN111015669B CN111015669B CN201911381022.6A CN201911381022A CN111015669B CN 111015669 B CN111015669 B CN 111015669B CN 201911381022 A CN201911381022 A CN 201911381022A CN 111015669 B CN111015669 B CN 111015669B
- Authority
- CN
- China
- Prior art keywords
- time
- interpolation
- output
- stop
- track
- 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.)
- Active
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
Abstract
The invention discloses a method for planning a stop motion track of an industrial robot, which directly carries out stop planning on a joint position finally output in each interpolation period, effectively avoids the delay from a track interpolation layer to a joint position output layer, and improves the rapidity and timeliness of stop response. The method stores the discrete interpolation output joint position, adds time information, converts the discrete joint position information into continuous information in a time domain by using a cubic spline model, and performs stop planning by using a time scaling method, thereby ensuring that data output in each period in the stop process still comes from the joint position output by track interpolation, and further ensuring that no track deviation exists in the stop process. Meanwhile, the time is planned by using a simplified fifth-order polynomial, the model is simple, the calculated amount is small, and the high-order trajectory planning method ensures continuous and smooth operation and no impact in the whole stopping process.
Description
Technical Field
The invention relates to a method for planning a motion trail of an industrial robot, in particular to a method for planning a stop motion trail of an industrial robot.
Background
In the application of industrial robots, stopping motion is a common action in the motion process of the robots, and how to stop the robot rapidly and smoothly is an important issue in the motion control of the robots. For the stop command, the robot needs to respond quickly and stop the current motion smoothly without deviating from the original teaching trajectory, so as to avoid accidents such as collision and impact.
The control system of the industrial robot is mainly structured as shown in the attached figure 1 and mainly comprises a track teaching module, an instruction analyzing module, a motion control module and a servo control module. The user carries out track teaching through the demonstrator, and the track teaching module carries out recording and processing and sends the recording and processing to the instruction analysis module; the instruction analysis module analyzes the taught track and sends the analyzed track to the motion control module; the motion control processing module firstly plans a track taught by a user, then interpolates and outputs the planned track in real time according to a certain interpolation period, then corrects interpolation output information through a control layer to obtain an interpolation signal which is more beneficial to the motion of the robot, converts the interpolation signal into a joint position interpolation signal through the kinematics module, and finally converts the joint position into a pulse through the pulse output module and sends the pulse to the servo control module to drive the robot to move.
During the operation of the robot, the robot must operate according to the track taught by the user and cannot deviate from the track. Therefore, the common command processing is placed after the trajectory planning and the response is made when the trajectory is interpolated. For example, in the "a real-time robot operation speed adjusting method" (CN201611196749) disclosed in the chinese patent document, a reference interpolation command is modified based on a planned trajectory to respond to a speed adjusting command. The stop command is one of the speed adjustment commands, namely: the speed is adjusted from the current operating speed to zero speed. However, due to the existence of the control layer, some control algorithms are implemented, which modify the track interpolation output information and delay the interpolation output information, such as the most basic filtering process. Then, if the stop command is processed at the time of interpolation output, a delay in response is inevitably caused. As shown in fig. 2, the solid line is the track interpolation output speed command, and the dotted line is the final speed command after passing through the control layer, it can be seen that after the stop command is issued, the interpolation output speed command immediately responds and decelerates to 0, but the final speed command after passing through the control layer is accelerated and then decelerated, and the final stop time lags behind the stop time of the track interpolation speed.
Disclosure of Invention
The invention aims to solve the technical problem of overcoming the defects in the prior art and provides a method for planning the stopping motion track of an industrial robot.
The invention provides a method for planning a stopping motion track of an industrial robot, aiming at quickly responding to stopping motion and ensuring that the motion in the stopping process is smooth and does not deviate from the taught track. The invention puts the stop command into the control layer for processing, and performs stop planning on the joint position finally output by the control layer so as to eliminate the delay of the control layer relative to the track interpolation layer.
The invention provides a method for planning a stop motion track of an industrial robot, which comprises the following steps:
step 1, establishing a piece of cache, and storing track interpolation output information:
the size of the buffer is set to be N +1, and N is the delay period number of the control layer output relative to the track interpolation layer. And the cached data stored in the positions 1 to N +1 are joint positions corresponding to the track interpolation output. If the current track is a Cartesian space track, kinematic inverse solution is required to be carried out on track interpolation output to obtain a joint position; if the space track of the joint is obtained, the track interpolation output is the joint position. And in normal operation, the joint position output in each interpolation period is joint data recorded at the head position of the cache, the joint data corresponding to the current track interpolation is recorded at the (N + 1) th position, and each position data in the cache moves forward by one bit every time the joint data is output.
And 2, when a stop command is received, performing stop planning on the cache segment data:
and 2.1, adding time information to the data in the cache, wherein the time difference between adjacent cache positions is an interpolation period Tc, and the time corresponding to the 1 st, 2 nd and 3 … th N +1 th positions of the cache is 0, Tc and 2Tc … NxTc.
Step 2.2, constructing a cubic spline model for time and joint positions:
y＝f(t,Jpos)
wherein t is time, Jpos is joint position, and y is cubic spline output;
and (3) establishing discrete joint position points as a continuous model on a time domain by adopting a cubic spline method, and serving as a basis for stopping planning.
And 2.3, stopping planning:
planning the time, adopting a time scaling method to encrypt the data in the cache, and subdividing the data in the adjacent caches (the time difference during planning is one interpolation period) by using the model in the step 2.2 so as to achieve the aim of deceleration and stop. The method comprises the following steps:
ti＝ti-1+kTc，
Jpos(ti)＝y(ti)
wherein, tiIs the current time, ti-1The time corresponding to the previous interpolation period (i is 1,2, 3 …, and the time corresponding to the previous interpolation period when i is 1 is 0), Jpos (t)i) Is tiThe joint position corresponding to the time; y (t)i) Is tiCubic spline output of time; k is a time scaling factor and is programmed during the stopping motion such that k decreases from 1 to 0 within a certain time to complete the stopping motion.
In order to ensure that the stop motion is smooth, and the speed and the acceleration of the joint of the robot are continuous and have no impact, the invention adopts a simplified fifth-order polynomial programming method, and the model is as follows:
k＝1.0-(6σ5-15σ4+10σ3)
wherein:
sigma is a proportional coefficient, and the value range is 0-1; t isstopTotal time required for stopping; t is t0iTo interpolate time, t0(i-1)At the interpolation time of the previous interpolation period, t0i≤Tstop；Jvelo、JaccThe joint velocity and the joint acceleration at the stop time are respectively.
And step 3, stopping interpolation output:
at a stop time TstopIn step 2, each interpolation cycleThe period output Jpos (t), when k is 0, i.e. t0i＝TstopAnd stopping ending.
The method for planning the motion stopping track of the industrial robot directly performs the stop planning on the joint position finally output in each interpolation period, effectively avoids the delay from a track interpolation layer to a joint position output layer, and improves the rapidity and timeliness of stop response.
The method stores the discrete interpolation output joint position, adds time information, converts the discrete joint position information into continuous information in a time domain by using a cubic spline model, and performs stop planning by using a time scaling method, thereby ensuring that data output in each period in the stop process still comes from the joint position output by track interpolation, and further ensuring that the stop process is on the original teaching track without any track deviation.
The method adopts the simplified fifth-order polynomial to plan time to complete the stop motion, has simple model and small calculated amount, ensures the continuity of position, speed and acceleration by using the high-order trajectory planning method, further ensures the continuous and smooth operation in the whole stopping process without impact and does not deviate from the original trajectory.
Drawings
Fig. 1 is a block diagram of a control system of a conventional industrial robot.
Fig. 2 is a schematic view of a prior art industrial robot stop planning speed curve.
Fig. 3 is a diagram of the industrial robot stop planning system architecture of the present invention.
FIG. 4 is a schematic diagram of a stop-and-program speed curve according to the present invention.
Detailed Description
The present invention will be described in further detail with reference to the following specific embodiments and the accompanying drawings.
Examples
Taking the joint motion of an industrial robot as an example, the method disclosed by the invention is adopted to plan the stop motion track of the industrial robot, and the method comprises the following specific steps:
step 1, establishing a piece of cache, and storing track interpolation output information:
the size of the cache is 25, the joint position output in each interpolation period in normal operation is the joint data recorded at the head position of the cache, the 25 th position records the joint data corresponding to the current track interpolation, and each position data in the cache moves forward by one bit every time the joint data is output.
And 2, when a stop command is received, performing stop planning on the cache segment data:
and 2.1, adding time information to the data in the cache, wherein the time difference between adjacent cache positions is that the interpolation period Tc is 0.004s, and the time corresponding to the 1 st, 2 nd and 3 … th N +1 th positions of the cache is 0,0.004 and 0.008 … N0.004.
Step 2.2, discrete joint position points are established as a continuous model on a time domain by adopting a cubic spline method,
constructing cubic spline models for time and joint positions:
y＝f(t,Jpos)
wherein t is time, Jpos is joint position, and y is cubic spline output;
and 2.3, stopping planning:
when receiving a stop command, calculating stop time according to the current speed and the current acceleration of the joint: t isstop＝0.2s；
② stop interpolation time t0iThe value of each interpolation period is 0,0.004,0.008 and … 0.2.2 in sequence, the value of the parameter sigma is 0, 0.02, 0.04 and … 1 in sequence, and the time scaling coefficient k is attenuated to 0 from 1 in sequence in the time of 0.2 s;
thirdly, the result of the second calculation is calculated according to the formula ti＝ti-1+kTcObtaining the time of each stop period; according to the formula Jpos (t)i)＝y(ti) Obtaining the joint position of each stop interpolation period;
and step 3, stopping interpolation output:
within 0.2s of the stop time, Jpos (t) is output according to step 2 every interpolation period, when k is 0, i.e. t0iWhen the value is 0.2, the stop is finished.
As shown in fig. 4, the solid line is a trajectory interpolation output curve, the dotted line is a final joint position output curve, when a stop command is received, the trajectory interpolation continues to perform interpolation output according to the trajectory plan, the deceleration processing is not performed, but the final joint output immediately performs deceleration stop, and the section from the issuance of the stop command to the completion of the stop in the figure is a planned stop motion trajectory.
Claims (1)
1. A method for planning a stop motion trail of an industrial robot comprises the following steps:
step 1, establishing a piece of cache, and storing track interpolation output information:
setting the size of a cache as N +1, wherein N is the delay period number of the control layer output relative to a track interpolation layer; the cached data stored at the 1-N +1 positions are joint positions corresponding to the track interpolation output; if the current track is a Cartesian space track, kinematic inverse solution is required to be carried out on track interpolation output to obtain a joint position; if the joint space track is obtained, the track interpolation output is the joint position; in normal operation, the joint position output in each interpolation period is joint data recorded at the head position of the cache, the (N + 1) th position records joint data corresponding to the current track interpolation, and each position data in the cache moves forward by one bit every time the joint data is output;
and 2, when a stop command is received, performing stop planning on the cache segment data:
step 2.1, adding time information to the data in the cache, wherein the time difference of adjacent cache positions is an interpolation period Tc, and the time corresponding to the 1 st, 2 nd, 3 … st +1 st position of the cache is 0, Tc, 2Tc … NxTc;
step 2.2, constructing a cubic spline model for time and joint positions:
y＝f(t,Jpos)
wherein t is time, Jpos is joint position, and y is cubic spline output;
and 2.3, stopping planning:
planning time, encrypting the data in the caches, and subdividing the data in the adjacent caches according to the model in the step 2.2:
ti＝ti-1+kTc，
Jpos(ti)＝y(ti)
wherein, tiIs the current time, ti-1The time corresponding to the previous interpolation period is 0 when i is 1,2, 3 …, and i is 1, Jpos (t)i) Is tiThe joint position corresponding to the time; y (t)i) Is tiCubic spline output of time; k is a time scaling factor, and k is planned during the motion stopping so that k is reduced from 1 to 0 within a certain time to complete the motion stopping;
a simplified quintic polynomial programming method is adopted, and the model is as follows:
k＝1.0-(6σ5-15σ4+10σ3)
wherein:
sigma is a proportional coefficient, and the value range is 0-1; t isstopTotal time required for stopping; t is t0iTo interpolate time, t0(i-1)At the interpolation time of the previous interpolation period, t0i≤Tstop；Jvelo、JaccThe joint speed and the joint acceleration at the stopping moment are respectively;
and step 3, stopping interpolation output:
at the total time T required for stoppingstopJpos (t) is output for each interpolation period, and when k is 0, t is output0i＝TstopAnd stopping ending.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201911381022.6A CN111015669B (en) | 2019-12-27 | 2019-12-27 | Industrial robot motion stopping trajectory planning method |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201911381022.6A CN111015669B (en) | 2019-12-27 | 2019-12-27 | Industrial robot motion stopping trajectory planning method |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN111015669A CN111015669A (en) | 2020-04-17 |
| CN111015669B true CN111015669B (en) | 2022-03-11 |
Family
ID=70194756
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201911381022.6A Active CN111015669B (en) | 2019-12-27 | 2019-12-27 | Industrial robot motion stopping trajectory planning method |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN111015669B (en) |
Families Citing this family (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN112486034B (en) * | 2020-11-09 | 2023-11-10 | 北京配天技术有限公司 | Method for verifying track planning, electronic equipment and storage medium |
| CN113664839B (en) * | 2021-10-25 | 2022-01-18 | 武汉瀚迈科技有限公司 | Non-real-time and real-time mixed interpolation calculation method for industrial robot |
| CN117687417A (en) * | 2024-01-29 | 2024-03-12 | 安徽大学 | Industrial robot external motion trail planning method and system |
Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| WO2002033815A1 (en) * | 2000-10-17 | 2002-04-25 | Technowave, Ltd. | Method for controlling acceleration/deceleration of motor |
| CN1743148A (en) * | 2004-09-02 | 2006-03-08 | 发那科株式会社 | Robot interference prevention control device |
| CN102581850A (en) * | 2012-02-10 | 2012-07-18 | 广州数控设备有限公司 | GSK-Link bus based modular robot control device and control method |
| CN105549534A (en) * | 2014-10-22 | 2016-05-04 | 发那科株式会社 | Numerical control device |
| CN105922265A (en) * | 2016-06-20 | 2016-09-07 | 广州视源电子科技股份有限公司 | Motion trail planning method and device for mechanical arm and robot |
| CN107390634A (en) * | 2017-08-31 | 2017-11-24 | 南京埃斯顿机器人工程有限公司 | A kind of industrial robot track quintic algebra curve planing method |
| CN107561933A (en) * | 2017-08-21 | 2018-01-09 | 深圳市同川科技有限公司 | Speed adjusting method, device, computer equipment and storage medium |
-
2019
- 2019-12-27 CN CN201911381022.6A patent/CN111015669B/en active Active
Patent Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| WO2002033815A1 (en) * | 2000-10-17 | 2002-04-25 | Technowave, Ltd. | Method for controlling acceleration/deceleration of motor |
| CN1743148A (en) * | 2004-09-02 | 2006-03-08 | 发那科株式会社 | Robot interference prevention control device |
| CN102581850A (en) * | 2012-02-10 | 2012-07-18 | 广州数控设备有限公司 | GSK-Link bus based modular robot control device and control method |
| CN105549534A (en) * | 2014-10-22 | 2016-05-04 | 发那科株式会社 | Numerical control device |
| CN105922265A (en) * | 2016-06-20 | 2016-09-07 | 广州视源电子科技股份有限公司 | Motion trail planning method and device for mechanical arm and robot |
| CN107561933A (en) * | 2017-08-21 | 2018-01-09 | 深圳市同川科技有限公司 | Speed adjusting method, device, computer equipment and storage medium |
| CN107390634A (en) * | 2017-08-31 | 2017-11-24 | 南京埃斯顿机器人工程有限公司 | A kind of industrial robot track quintic algebra curve planing method |
Also Published As
| Publication number | Publication date |
|---|---|
| CN111015669A (en) | 2020-04-17 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN111015669B (en) | Industrial robot motion stopping trajectory planning method | |
| CN101898358B (en) | Method and device for controlling a manipulator | |
| CN108340351B (en) | Robot teaching device and method and teaching robot | |
| JP5139230B2 (en) | Collision prevention device in numerical control device | |
| CN106647282B (en) | Six-degree-of-freedom robot trajectory planning method considering tail end motion error | |
| CN107562048B (en) | Dynamic obstacle avoidance control method based on laser radar | |
| US5426722A (en) | Method for optimizing the motion of a multi-axis robot | |
| CN102039596B (en) | Control the method and apparatus of executor | |
| CN110928290B (en) | Quintic curve path planning method and lane center line maintenance method | |
| CN101402199A (en) | Hand-eye type robot movable target extracting method with low servo accuracy based on visual sensation | |
| CN105700530A (en) | Track planning method for robot joint space conveyor belt following movement | |
| CN105965512A (en) | Method for motion reappearance of industrial multi-axial mechanical arm | |
| WO2018119745A1 (en) | Robot motion control method and related apparatus | |
| JPH0778684B2 (en) | Iterative Spline Function Control Positioner | |
| CN111805547B (en) | Method for realizing dynamic tracking of track | |
| CN112965501A (en) | Automatic driving speed planning method and device | |
| CN112965443B (en) | High-precision interpolation control method for corner trajectory tracking of cutting bed | |
| CN110780670B (en) | Robot obstacle avoidance control method based on fuzzy control algorithm | |
| CN109683615A (en) | The speed look-ahead approach and robot controller in the path that robot continuously moves | |
| CN113021356A (en) | Robot track planning method and system for ingot trimming process | |
| CN114721275B (en) | Visual servo robot self-adaptive tracking control method based on preset performance | |
| CN107553484B (en) | Robot operation motion path planning method and system | |
| KR101981641B1 (en) | Method and system for formation control of multiple mobile robots | |
| CN112847373B (en) | Robot track synchronous control method and computer readable storage medium | |
| CN112082555A (en) | Curvature consistency path planning algorithm based on self-adaptive dynamic window method under narrow channel environment |
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 |