CN111015669B - Industrial robot motion stopping trajectory planning method - Google Patents

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

Industrial robot motion stopping trajectory planning method

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:

t_i＝t_i-1+kT_c，

Jpos(t_i)＝y(t_i)

wherein, t_iIs the current time, t_i-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 t_iThe joint position corresponding to the time; y (t)_i) Is t_iCubic 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σ⁵-15σ⁴+10σ³)

wherein:

sigma is a proportional coefficient, and the value range is 0-1; t is_stopTotal time required for stopping; t is t_0iTo interpolate time, t_0(i-1)At the interpolation time of the previous interpolation period, t_0i≤T_stop；J_velo、J_accThe joint velocity and the joint acceleration at the stop time are respectively.

And step 3, stopping interpolation output:

at a stop time T_stopIn step 2, each interpolation cycleThe period output Jpos (t), when k is 0, i.e. t_0i＝T_stopAnd 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 is_stop＝0.2s；

② stop interpolation time t_0iThe 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 t_i＝t_i-1+kT_cObtaining the time of each stop period; according to the formula Jpos (t)_i)＝y(t_i) 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. t_0iWhen 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:

t_i＝t_i-1+kT_c，

Jpos(t_i)＝y(t_i)

wherein, t_iIs the current time, t_i-1The time corresponding to the previous interpolation period is 0 when i is 1,2, 3 …, and i is 1, Jpos (t)_i) Is t_iThe joint position corresponding to the time; y (t)_i) Is t_iCubic 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σ⁵-15σ⁴+10σ³)

wherein:

sigma is a proportional coefficient, and the value range is 0-1; t is_stopTotal time required for stopping; t is t_0iTo interpolate time, t_0(i-1)At the interpolation time of the previous interpolation period, t_0i≤T_stop；J_velo、J_accThe 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 stopping_stopJpos (t) is output for each interpolation period, and when k is 0, t is output_0i＝T_stopAnd stopping ending.

Images