CN105082135A - Speed control method for inching operation of robot - Google Patents

Abstract

The invention discloses a speed control method of a robot jog operation. The method divides the speed control process of the jog operation into two stages: an acceleration & constant speed section and a stop section. Among them, the acceleration & constant speed section includes three sub-stages: periodic variable acceleration section, acceleration-constant velocity transition section, and constant velocity section. The interpolation increments for the trajectory are planned. The present invention can realize micro-distance jogging, and effectively avoid excessive mechanical impact caused by jogging operation in the manual teaching process, and problems such as high-speed jogging cannot be stopped smoothly in time; it can be applied to manual teaching in each coordinate system space of the robot Speed control for jog operations.

Description

A speed control method for robot jog operation

technical field

The invention relates to a speed control method for inching operation in a robot teaching mode, which belongs to the technology of industrial robots.

Background technique

Jog (JOG) operation in manual teaching mode is the operation in which the robot teaching personnel drives the robot to reach the teaching point in the space of various joint coordinate systems or Cartesian coordinate system, tool coordinate system and user coordinate system. During jogging operation, the controller needs to cooperate with the button status of the teaching process to perform interpolation calculation and execution in real time and synchronously. The traditional controller adopts an intuitive solution: the controller calculates the next interpolation point position every real-time control cycle, and completes the execution task in the same cycle. However, this solution has the following disadvantages: ①The jogging speed is not smooth, which will cause mechanical shock; When the movement speed is very high, it may exceed the movement limit because it cannot be stopped in time.

Contents of the invention

Purpose of the invention: In order to overcome the deficiencies in the prior art, the present invention aims at the situation that the servo motor is in the position control mode, and provides a speed control method for the jog operation in the manual teaching mode of the robot, in order to realize the joint coordinate system space of the robot. The single-axis smooth jog operation provides a trajectory planning step and program implementation method, and at the same time provides a trajectory planning method for moving and rotating jog operations in Cartesian coordinate system, tool coordinate system and user coordinate system. Steps and procedures to realize the method. The method of the invention can smooth the speed of the jogging process, effectively reduce the impact and wear on the mechanical structure and reduce the energy consumption of the movement, realize jogging at a small distance, and solve the problem that the jogging at high speed cannot be stopped smoothly in time.

Technical scheme: in order to achieve the above object, the technical scheme adopted in the present invention is:

A speed control method for robot jog operation, which divides the speed control process of jog operation into an acceleration & constant speed segment and a stop segment, wherein the acceleration & constant speed segment includes three sub-sections: a periodical variable acceleration segment, an acceleration-constant speed transition segment and a constant speed segment stage, the acceleration-constant-speed transition section and the constant-speed section are two indispensable sub-stages, and each sub-stage uses a fixed-length time slice as a unit for trajectory interpolation planning, and the acceleration of interpolation incremental changes in each time slice is continuous , and the initial incremental acceleration and the final incremental acceleration of each time slice are both zero; each time slice is composed of a number of consecutive control cycles, after each time slice is completed, it is determined to use acceleration at the next moment according to the judgment conditions &The interpolation sequence in the constant speed segment or the interpolation sequence in the stop segment, where the judgment conditions include: ①Which sub-stage is the current control process in?

The state jump relationship of period variable acceleration section, acceleration-constant speed transition section and constant speed section is shown in Figure 1: when the jog operation time is long, the jog operation includes the above three sub-stages and the stop section; when the jog operation time When it is short, the acceleration-constant velocity transition section and the constant velocity section may be missing. The dynamic index of interpolation increment at each stage is shown in Fig. 2, and the interpolation sequence point of jogging is determined by adding the interpolation increment in the initial position domain. The method specifically includes the following parts:

Part1: Record initial position or pose information

After the controller receives the jog request command, it records the initial position or pose information. If the single-axis jog operation is performed in the joint space, the initial position of the jog is the initial angle of the jog axis; if the single-axis jog operation is performed in the Cartesian coordinate system, tool coordinate system and user coordinate system, the point The initial pose of the movement is the pose transformation matrix of the end point of the robot tool (ToolCenterPoint, TCP) in the reference coordinate system.

Part2: Definition of parameters

Let i=1,2,...,X represent all the time slices of the acceleration & constant speed segment, the time length of the time slice is T ₂₀ : the initial increment of the i-th time slice is θ _i0 , and the corresponding initial incremental speed and acceleration are and θ ; The midpoint increment of the i-th time slice is θ _ih , and the corresponding midpoint incremental velocity and acceleration are respectively and , At the same time, it is the acceleration peak value of the i-th time slice; the termination increment of the i-th time slice is θ _if , and the corresponding termination increment velocity and acceleration are respectively and The given speed in the constant speed section is ;The interpolation time is t; Initially i=1;

The initial increment of the stop segment is θ ₀₀ , and the corresponding initial incremental velocity and acceleration are respectively and The terminal increment of the stop segment is θ _0f , and the corresponding terminal incremental velocity and acceleration are respectively and

Part3: Design of imputation rules

When the interpolation sequence of the i-th time slice is executed, the interpolation sequence of the (i+1)-th time slice is calculated at the same time, the purpose is to be able to execute the (i+ 1) The interpolation sequence of the first time slice ensures the continuity of the motion; the interpolation sequence of the (i+1)th time slice is divided into two categories: ① the interpolation sequence Normal of the acceleration & constant speed segment and the stop segment Interpolation sequence Ending;

The interpolation sequence Normal is divided into three situations: ① If the acceleration peak value of the (i+1)th time slice The maximum acceleration a _max has not been reached, and the speed of the (i+1)th time slice has not reached the peak speed Then the (i+1)th time slice is the periodic variable acceleration segment, and the interpolation sequence of the (i+1)th time slice is calculated by using the periodic variable acceleration segment interpolation method; ②If the (i+1)th time slice peak acceleration The maximum acceleration a _max is reached, but the speed of the (i+1)th time slice does not reach the peak speed Then the (i+1)th time slice is the acceleration-constant speed transition section, and the interpolation sequence of the (i+1)th time slice is calculated by using the acceleration-constant speed transition section interpolation method; ③ if the (i+1)th time slice The speed of the time slice reaches the speed peak Then the (i+1)th time slice is a constant speed segment, and the interpolation sequence of the (i+1)th time slice is calculated using the constant speed segment interpolation method;

Since it is not known when the jog operation will stop, it is necessary to calculate the interpolation sequence Ending while executing the interpolation sequence of each time slice. The starting point of the interpolation sequence Ending is the continuation of the i-th interpolation sequence Normal.

Part4: Calculation and execution of the interpolation sequence of the time slice of the period variable acceleration segment

The control program starts the calculation process of the interpolation sequence, and first calculates the interpolation sequence of the i-th time slice of the period variable acceleration segment, and the method for determining the interpolation increment curve function of the cycle variable acceleration segment is as follows:

Periodic variable acceleration period can include several time slices; in the periodical variable acceleration period, the acceleration peak values of two adjacent time slices are incremented by a fixed value △a, namely The acceleration peak value obtained after planning the (i+1)th time slice is required The maximum acceleration a _max is not exceeded.

Due to the θ _i0 of each time slice of the periodical variable acceleration segment, and It can be regarded as a definite quantity, and T ₂₀ is a fixed value, so it can be considered to use a fourth-order polynomial to interpolate the interpolation quantity θ _i (t) of the i-th time slice in the periodic variable acceleration segment, namely:

θ _i (t) = ω _i0 + ω _i1 t + ω _i2 t ² + ω _i3 t ³ + ω _i4 t ⁴

Among them, ω _i0 , ω _i1 , ω _i2 , ω _i3 and ω _i4 are polynomial coefficients, and their constraints are:

$\left\{\begin{array}{c}{\mathrm{<span\; class="notranslate">\; \&theta;</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}\mathrm{<span\; class="notranslate">\; 0</span>}}<span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; \&omega;</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}\mathrm{<span\; class="notranslate">\; 0</span>}}\\ {\stackrel{<span\; class="notranslate">\; \&CenterDot;</span>}{\mathrm{<span\; class="notranslate">\; \&theta;</span>}}}_{\mathrm{<span\; class="notranslate">\; i</span>}\mathrm{<span\; class="notranslate">\; 0</span>}}<span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; \&omega;</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}\mathrm{<span\; class="notranslate">\; 1</span>}}\\ {\stackrel{<span\; class="notranslate">\; \&CenterDot;\&CenterDot;</span>}{\mathrm{<span\; class="notranslate">\; \&theta;</span>}}}_{\mathrm{<span\; class="notranslate">\; i</span>}\mathrm{<span\; class="notranslate">\; 0</span>}}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; 2</span>}{\mathrm{<span\; class="notranslate">\; \&omega;</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}\mathrm{<span\; class="notranslate">\; 2</span>}}\\ {\stackrel{<span\; class="notranslate">\; \&CenterDot;\&CenterDot;</span>}{\mathrm{<span\; class="notranslate">\; \&theta;</span>}}}_{\mathrm{<span\; class="notranslate">\; i</span>}\mathrm{<span\; class="notranslate">\; h</span>}}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; 2</span>}{\mathrm{<span\; class="notranslate">\; \&omega;</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 6</span>}{\mathrm{<span\; class="notranslate">\; \&omega;</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}\mathrm{<span\; class="notranslate">\; 3</span>}}<span\; class="notranslate">\; (</span>\frac{{\mathrm{<span\; class="notranslate">\; T</span>}}_{\mathrm{<span\; class="notranslate">\; 20</span>}}}{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 12</span>}{\mathrm{<span\; class="notranslate">\; \&omega;</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}\mathrm{<span\; class="notranslate">\; 4</span>}}{<span\; class="notranslate">\; (</span>\frac{{\mathrm{<span\; class="notranslate">\; T</span>}}_{\mathrm{<span\; class="notranslate">\; 20</span>}}}{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; )</span>}^{\mathrm{<span\; class="notranslate">\; 2</span>}}\\ {\stackrel{<span\; class="notranslate">\; \&CenterDot;\&CenterDot;</span>}{\mathrm{<span\; class="notranslate">\; \&theta;</span>}}}_{\mathrm{<span\; class="notranslate">\; i</span>}\mathrm{<span\; class="notranslate">\; f</span>}}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; 2</span>}{\mathrm{<span\; class="notranslate">\; \&omega;</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 6</span>}{\mathrm{<span\; class="notranslate">\; \&omega;</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}\mathrm{<span\; class="notranslate">\; 3</span>}}{\mathrm{<span\; class="notranslate">\; T</span>}}_{\mathrm{<span\; class="notranslate">\; 20</span>}}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 12</span>}{\mathrm{<span\; class="notranslate">\; \&omega;</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}\mathrm{<span\; class="notranslate">\; 4</span>}}{\mathrm{<span\; class="notranslate">\; T</span>}}_{\mathrm{<span\; class="notranslate">\; 20</span>}}^{\mathrm{<span\; class="notranslate">\; 2</span>}}\end{array}\right.$

The function to interpolate the interpolation amount θ _i (t) of the i-th time slice in the periodic variable acceleration segment obtained by solving is:

${\mathrm{<span\; class="notranslate">\; \&theta;</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; \&theta;</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}\mathrm{<span\; class="notranslate">\; 0</span>}}<span\; class="notranslate">\; +</span>{\stackrel{<span\; class="notranslate">\; \&CenterDot;</span>}{\mathrm{<span\; class="notranslate">\; \&theta;</span>}}}_{\mathrm{<span\; class="notranslate">\; i</span>}\mathrm{<span\; class="notranslate">\; 0</span>}}\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; +</span>\frac{\mathrm{<span\; class="notranslate">\; 2</span>}{\stackrel{<span\; class="notranslate">\; \&CenterDot;\&CenterDot;</span>}{\mathrm{<span\; class="notranslate">\; \&theta;</span>}}}_{\mathrm{<span\; class="notranslate">\; i</span>}\mathrm{<span\; class="notranslate">\; h</span>}}}{\mathrm{<span\; class="notranslate">\; 3</span>}{\mathrm{<span\; class="notranslate">\; T</span>}}_{\mathrm{<span\; class="notranslate">\; 20</span>}}}{\mathrm{<span\; class="notranslate">\; t</span>}}^{\mathrm{<span\; class="notranslate">\; 3</span>}}<span\; class="notranslate">\; -</span>\frac{{\stackrel{<span\; class="notranslate">\; \&CenterDot;\&CenterDot;</span>}{\mathrm{<span\; class="notranslate">\; \&theta;</span>}}}_{\mathrm{<span\; class="notranslate">\; i</span>}\mathrm{<span\; class="notranslate">\; h</span>}}}{\mathrm{<span\; class="notranslate">\; 3</span>}{\mathrm{<span\; class="notranslate">\; T</span>}}_{\mathrm{<span\; class="notranslate">\; 20</span>}}^{\mathrm{<span\; class="notranslate">\; 2</span>}}}{\mathrm{<span\; class="notranslate">\; t</span>}}^{\mathrm{<span\; class="notranslate">\; 4</span>}}$

Part5: Calculation and execution of the interpolation sequence of the acceleration-constant transition period time slice

Due to the time slice θ _i0 of the acceleration-constant velocity transition period, and It can be regarded as a definite quantity, so it can also be considered to use a fourth-order polynomial to interpolate the interpolation quantity θ _i (t) of the i-th time slice of the acceleration-constant speed transition section, namely:

θ _i (t) = ω _i0 + ω _i1 t + ω _i2 t ² + ω _i3 t ³ + ω _i4 t ⁴

Among them, ω _i0 , ω _i1 , ω _i2 , ω _i3 and ω _i4 are polynomial coefficients, and their constraints are:

$\left\{\begin{array}{c}{\mathrm{<span\; class="notranslate">\; \&theta;</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}\mathrm{<span\; class="notranslate">\; 0</span>}}<span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; \&omega;</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}\mathrm{<span\; class="notranslate">\; 0</span>}}\\ {\stackrel{<span\; class="notranslate">\; \&CenterDot;</span>}{\mathrm{<span\; class="notranslate">\; \&theta;</span>}}}_{\mathrm{<span\; class="notranslate">\; i</span>}\mathrm{<span\; class="notranslate">\; 0</span>}}<span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; \&omega;</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}\mathrm{<span\; class="notranslate">\; 1</span>}}\\ {\stackrel{<span\; class="notranslate">\; \&CenterDot;\&CenterDot;</span>}{\mathrm{<span\; class="notranslate">\; \&theta;</span>}}}_{\mathrm{<span\; class="notranslate">\; i</span>}\mathrm{<span\; class="notranslate">\; 0</span>}}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; 2</span>}{\mathrm{<span\; class="notranslate">\; \&omega;</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}\mathrm{<span\; class="notranslate">\; 2</span>}}\\ {\stackrel{<span\; class="notranslate">\; \&CenterDot;\&CenterDot;</span>}{\mathrm{<span\; class="notranslate">\; \&theta;</span>}}}_{\mathrm{<span\; class="notranslate">\; i</span>}\mathrm{<span\; class="notranslate">\; f</span>}}<span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; \&omega;</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}\mathrm{<span\; class="notranslate">\; 0</span>}}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 2</span>}{\mathrm{<span\; class="notranslate">\; \&omega;</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}\mathrm{<span\; class="notranslate">\; 1</span>}}{\mathrm{<span\; class="notranslate">\; T</span>}}_{\mathrm{<span\; class="notranslate">\; 20</span>}}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 3</span>}{\mathrm{<span\; class="notranslate">\; \&omega;</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}\mathrm{<span\; class="notranslate">\; 2</span>}}{\mathrm{<span\; class="notranslate">\; T</span>}}_{\mathrm{<span\; class="notranslate">\; 20</span>}}^{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 4</span>}{\mathrm{<span\; class="notranslate">\; \&omega;</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}\mathrm{<span\; class="notranslate">\; 3</span>}}{\mathrm{<span\; class="notranslate">\; T</span>}}_{\mathrm{<span\; class="notranslate">\; 20</span>}}^{\mathrm{<span\; class="notranslate">\; 3</span>}}\\ {\stackrel{<span\; class="notranslate">\; \&CenterDot;\&CenterDot;</span>}{\mathrm{<span\; class="notranslate">\; \&theta;</span>}}}_{\mathrm{<span\; class="notranslate">\; i</span>}\mathrm{<span\; class="notranslate">\; f</span>}}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; 2</span>}{\mathrm{<span\; class="notranslate">\; \&omega;</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 6</span>}{\mathrm{<span\; class="notranslate">\; \&omega;</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}\mathrm{<span\; class="notranslate">\; 3</span>}}{\mathrm{<span\; class="notranslate">\; T</span>}}_{\mathrm{<span\; class="notranslate">\; 20</span>}}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 12</span>}{\mathrm{<span\; class="notranslate">\; \&omega;</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}\mathrm{<span\; class="notranslate">\; 4</span>}}{\mathrm{<span\; class="notranslate">\; T</span>}}_{\mathrm{<span\; class="notranslate">\; 20</span>}}^{\mathrm{<span\; class="notranslate">\; 2</span>}}\end{array}\right.$

The function to interpolate the interpolation amount θ _i (t) of the ith time slice of the acceleration-constant speed transition section obtained by solving is:

${\mathrm{<span\; class="notranslate">\; \&theta;</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; \&theta;</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}\mathrm{<span\; class="notranslate">\; 0</span>}}<span\; class="notranslate">\; +</span>{\stackrel{<span\; class="notranslate">\; \&CenterDot;</span>}{\mathrm{<span\; class="notranslate">\; \&theta;</span>}}}_{\mathrm{<span\; class="notranslate">\; i</span>}\mathrm{<span\; class="notranslate">\; 0</span>}}\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; +</span>\frac{{\stackrel{<span\; class="notranslate">\; \&CenterDot;</span>}{\mathrm{<span\; class="notranslate">\; \&theta;</span>}}}_{\mathrm{<span\; class="notranslate">\; i</span>}\mathrm{<span\; class="notranslate">\; f</span>}}<span\; class="notranslate">\; -</span>{\stackrel{<span\; class="notranslate">\; \&CenterDot;</span>}{\mathrm{<span\; class="notranslate">\; \&theta;</span>}}}_{\mathrm{<span\; class="notranslate">\; i</span>}\mathrm{<span\; class="notranslate">\; 0</span>}}}{{\mathrm{<span\; class="notranslate">\; T</span>}}_{\mathrm{<span\; class="notranslate">\; 20</span>}}^{\mathrm{<span\; class="notranslate">\; 2</span>}}}{\mathrm{<span\; class="notranslate">\; t</span>}}^{\mathrm{<span\; class="notranslate">\; 3</span>}}<span\; class="notranslate">\; -</span>\frac{{\stackrel{<span\; class="notranslate">\; \&CenterDot;</span>}{\mathrm{<span\; class="notranslate">\; \&theta;</span>}}}_{\mathrm{<span\; class="notranslate">\; i</span>}\mathrm{<span\; class="notranslate">\; f</span>}}<span\; class="notranslate">\; -</span>{\stackrel{<span\; class="notranslate">\; \&CenterDot;</span>}{\mathrm{<span\; class="notranslate">\; \&theta;</span>}}}_{\mathrm{<span\; class="notranslate">\; i</span>}\mathrm{<span\; class="notranslate">\; 0</span>}}}{\mathrm{<span\; class="notranslate">\; 2</span>}{\mathrm{<span\; class="notranslate">\; T</span>}}_{\mathrm{<span\; class="notranslate">\; 20</span>}}^{\mathrm{<span\; class="notranslate">\; 3</span>}}}{\mathrm{<span\; class="notranslate">\; t</span>}}^{\mathrm{<span\; class="notranslate">\; 4</span>}}$

Part6: Calculation and execution of the interpolation sequence of time slices at a constant speed

Since the θ _i0 of the time slice of the constant velocity segment and It can be regarded as a definite quantity, so it can also be considered to use a linear polynomial to interpolate the interpolation quantity θ _i (t) of the i-th time slice in the constant speed segment, namely:

θ _i (t) = ω _i0 + ω _i1 t

Among them, ω _i0 and ω _i1 are polynomial coefficients, and their constraints are:

$\left\{\begin{array}{c}{\mathrm{<span\; class="notranslate">\; \&theta;</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}\mathrm{<span\; class="notranslate">\; 0</span>}}<span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; \&omega;</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}\mathrm{<span\; class="notranslate">\; 0</span>}}\\ {\stackrel{<span\; class="notranslate">\; \&Center\; Dot;</span>}{\mathrm{<span\; class="notranslate">\; \&theta;</span>}}}_{\mathrm{<span\; class="notranslate">\; i</span>}\mathrm{<span\; class="notranslate">\; 0</span>}}<span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; \&omega;</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}\mathrm{<span\; class="notranslate">\; 1</span>}}\end{array}\right.$

The function of interpolating the interpolation amount θ _i (t) of the i-th time slice in the constant velocity segment obtained by solving is:

${\mathrm{<span\; class="notranslate">\; \&theta;</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; \&theta;</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}\mathrm{<span\; class="notranslate">\; 0</span>}}<span\; class="notranslate">\; +</span>{\stackrel{<span\; class="notranslate">\; \&CenterDot;</span>}{\mathrm{<span\; class="notranslate">\; \&theta;</span>}}}_{\mathrm{<span\; class="notranslate">\; i</span>}\mathrm{<span\; class="notranslate">\; 0</span>}}\mathrm{<span\; class="notranslate">\; t</span>}$

Part7: Calculation and execution of the imputation sequence of the stop segment

If the jog operation ends, log off the calculation process; if the execution thread is enabled, wait for the execution of the stop sequence to complete, log off the execution thread, and wait for the next jog request.

Once it is found that the jog operation is over, beyond the reach of the robot, or a collision occurs, it will directly jump to the stop section; in the stop section, the jog operation will reduce the speed to 0 in the shortest and smoothest way, and no longer in the stop section. Use time slices for division, and use the maximum deceleration allowed by the operation -a _Decel,max (-a _Decel,max is a positive value) as the peak deceleration for planning.

Due to the θ ₀₀ of the stop section, ${\stackrel{\&CenterDot;}{\mathrm{\&theta;}}}_{00},{\stackrel{\&CenterDot;\&CenterDot;}{\mathrm{\&theta;}}}_{00}(=0),{\stackrel{\&CenterDot;\&CenterDot;}{\mathrm{\&theta;}}}_{0h}(=-a_{\mathrm{D.}ecel,max}),{\stackrel{\&Center\; Dot;}{\mathrm{\&theta;}}}_{0f}(=0)$ and It can be regarded as a definite quantity, but the running time △t is an unknown variable. It can also be considered to use a fourth-order polynomial to interpolate the interpolation quantity θ ₀ (t) of the stop segment, namely:

θ ₀ (t)＝ω ₀₀ +ω ₀₁ t+ω ₀₂ t ² +ω ₀₃ t ³ +ω ₀₄ t ⁴

Among them, ω ₀₀ , ω ₀₁ , ω ₀₂ , ω ₀₃ and ω ₀₄ are polynomial coefficients, and their constraints are:

$\left\{\begin{array}{c}{\mathrm{<span\; class="notranslate">\; \&theta;</span>}}_{\mathrm{<span\; class="notranslate">\; 00</span>}}<span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; \&omega;</span>}}_{\mathrm{<span\; class="notranslate">\; 00</span>}}\\ {\stackrel{<span\; class="notranslate">\; \&CenterDot;</span>}{\mathrm{<span\; class="notranslate">\; \&theta;</span>}}}_{\mathrm{<span\; class="notranslate">\; 00</span>}}<span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; \&omega;</span>}}_{\mathrm{<span\; class="notranslate">\; 01</span>}}\\ {\stackrel{<span\; class="notranslate">\; \&CenterDot;\&CenterDot;</span>}{\mathrm{<span\; class="notranslate">\; \&theta;</span>}}}_{\mathrm{<span\; class="notranslate">\; 00</span>}}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; 2</span>}{\mathrm{<span\; class="notranslate">\; \&omega;</span>}}_{\mathrm{<span\; class="notranslate">\; 02</span>}}\\ {\stackrel{<span\; class="notranslate">\; \&CenterDot;\&CenterDot;</span>}{\mathrm{<span\; class="notranslate">\; \&theta;</span>}}}_{\mathrm{<span\; class="notranslate">\; 0</span>}\mathrm{<span\; class="notranslate">\; h</span>}}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; 2</span>}{\mathrm{<span\; class="notranslate">\; \&omega;</span>}}_{\mathrm{<span\; class="notranslate">\; 02</span>}}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 6</span>}{\mathrm{<span\; class="notranslate">\; \&omega;</span>}}_{\mathrm{<span\; class="notranslate">\; 03</span>}}<span\; class="notranslate">\; (</span>\frac{\mathrm{<span\; class="notranslate">\; \&Delta;</span>}\mathrm{<span\; class="notranslate">\; t</span>}}{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 12</span>}{\mathrm{<span\; class="notranslate">\; \&omega;</span>}}_{\mathrm{<span\; class="notranslate">\; 04</span>}}{<span\; class="notranslate">\; (</span>\frac{\mathrm{<span\; class="notranslate">\; \&Delta;</span>}\mathrm{<span\; class="notranslate">\; t</span>}}{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; )</span>}^{\mathrm{<span\; class="notranslate">\; 2</span>}}\\ {\stackrel{<span\; class="notranslate">\; \&Center\; Dot;</span>}{\mathrm{<span\; class="notranslate">\; \&theta;</span>}}}_{\mathrm{<span\; class="notranslate">\; 0</span>}\mathrm{<span\; class="notranslate">\; f</span>}}<span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; \&omega;</span>}}_{\mathrm{<span\; class="notranslate">\; 00</span>}}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 2</span>}{\mathrm{<span\; class="notranslate">\; \&omega;</span>}}_{\mathrm{<span\; class="notranslate">\; 01</span>}}\mathrm{<span\; class="notranslate">\; \&Delta;</span>}\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 3</span>}{\mathrm{<span\; class="notranslate">\; \&omega;</span>}}_{\mathrm{<span\; class="notranslate">\; 02</span>}}{\mathrm{<span\; class="notranslate">\; \&Delta;t</span>}}^{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 4</span>}{\mathrm{<span\; class="notranslate">\; \&omega;</span>}}_{\mathrm{<span\; class="notranslate">\; 03</span>}}{\mathrm{<span\; class="notranslate">\; \&Delta;t</span>}}^{\mathrm{<span\; class="notranslate">\; 3</span>}}\\ {\stackrel{<span\; class="notranslate">\; \&CenterDot;\&CenterDot;</span>}{\mathrm{<span\; class="notranslate">\; \&theta;</span>}}}_{\mathrm{<span\; class="notranslate">\; f</span>}}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; 2</span>}{\mathrm{<span\; class="notranslate">\; \&omega;</span>}}_{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 6</span>}{\mathrm{<span\; class="notranslate">\; \&omega;</span>}}_{\mathrm{<span\; class="notranslate">\; 3</span>}}\mathrm{<span\; class="notranslate">\; \&Delta;</span>}\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 12</span>}{\mathrm{<span\; class="notranslate">\; \&omega;</span>}}_{\mathrm{<span\; class="notranslate">\; 4</span>}}{\mathrm{<span\; class="notranslate">\; \&Delta;t</span>}}^{\mathrm{<span\; class="notranslate">\; 2</span>}}\end{array}\right.$

The interpolation function obtained by solving the interpolation amount θ ₀ (t) of the stop segment is:

${\mathrm{<span\; class="notranslate">\; \&theta;</span>}}_{\mathrm{<span\; class="notranslate">\; 0</span>}}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; \&theta;</span>}}_{\mathrm{<span\; class="notranslate">\; 00</span>}}<span\; class="notranslate">\; +</span>{\stackrel{<span\; class="notranslate">\; \&Center\; Dot;</span>}{\mathrm{<span\; class="notranslate">\; \&theta;</span>}}}_{\mathrm{<span\; class="notranslate">\; 00</span>}}\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; -</span>\frac{\mathrm{<span\; class="notranslate">\; 4</span>}{\mathrm{<span\; class="notranslate">\; a</span>}}_{\mathrm{<span\; class="notranslate">\; D.</span>}\mathrm{<span\; class="notranslate">\; e</span>}\mathrm{<span\; class="notranslate">\; c</span>}\mathrm{<span\; class="notranslate">\; e</span>}\mathrm{<span\; class="notranslate">\; l</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; m</span>}\mathrm{<span\; class="notranslate">\; a</span>}\mathrm{<span\; class="notranslate">\; x</span>}}^{\mathrm{<span\; class="notranslate">\; 2</span>}}}{\mathrm{<span\; class="notranslate">\; 9</span>}{\stackrel{<span\; class="notranslate">\; \&CenterDot;</span>}{\mathrm{<span\; class="notranslate">\; \&theta;</span>}}}_{\mathrm{<span\; class="notranslate">\; 00</span>}}}{\mathrm{<span\; class="notranslate">\; t</span>}}^{\mathrm{<span\; class="notranslate">\; 3</span>}}<span\; class="notranslate">\; +</span>\frac{\mathrm{<span\; class="notranslate">\; 4</span>}{\mathrm{<span\; class="notranslate">\; a</span>}}_{\mathrm{<span\; class="notranslate">\; D.</span>}\mathrm{<span\; class="notranslate">\; e</span>}\mathrm{<span\; class="notranslate">\; c</span>}\mathrm{<span\; class="notranslate">\; e</span>}\mathrm{<span\; class="notranslate">\; l</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; m</span>}\mathrm{<span\; class="notranslate">\; a</span>}\mathrm{<span\; class="notranslate">\; x</span>}}^{\mathrm{<span\; class="notranslate">\; 3</span>}}}{\mathrm{<span\; class="notranslate">\; 27</span>}{\stackrel{<span\; class="notranslate">\; \&Center\; Dot;</span>}{\mathrm{<span\; class="notranslate">\; \&theta;</span>}}}_{\mathrm{<span\; class="notranslate">\; 00</span>}}^{\mathrm{<span\; class="notranslate">\; 2</span>}}}{\mathrm{<span\; class="notranslate">\; t</span>}}^{\mathrm{<span\; class="notranslate">\; 4</span>}}<span\; class="notranslate">\; ;</span>$

The running time of the stop segment is:

Beneficial effects: the speed control method for robot inching operation provided by the present invention has the following advantages compared with the prior art: 1. Improve the motion performance of inching operation. Compared with the jog solution of the traditional controller, the acceleration curve of the jog planning result of the present invention is continuous and the speed is smooth, which can effectively reduce the mechanical impact and wear during the jog process and prolong the life of the equipment; the acceleration in the initial stage of jog is small And gradually increase, it can realize small displacement jogging, which is more convenient for users to teach the robot; taking the time slice as the unit, the planning process fully considers whether the jogging can stop within the reachable range, and avoids the high-speed point of the robot at the algorithm level. The problem that the movement is easy to exceed the soft limit of movement. 2. It is universal. The present invention does not relate to the specific type of the robot, and has universality to the realization method of the inching operation taught by the robot.

Description of drawings

Figure 1 is a schematic diagram of the state jump of jogging trajectory planning. The program is initially in the state of waiting for the jog request. When the key signal is detected, it will jump to the state of the periodic variable acceleration stage; if the key is pressed for a long time, it will enter the state of the constant speed stage through the acceleration-constant speed transition stage. During the process, once it is detected that the key is released or there is an unreachable interpolation point, it will enter the stop segment state. In the figure: ①The jog request signal is detected; ②The acceleration in the next time slice increases (but must not exceed the maximum acceleration), and the termination speed obtained by planning is still less than the given constant speed segment speed; Exceeds the maximum acceleration), the planned end speed will exceed the speed of the given constant speed segment; ④ The jog end request is not received, and the next time slice movement is reachable; ⑤ The jog end request is received, or the next time slice has unreachable point.

Fig. 2 is a schematic diagram of planning of the four-stage interpolation increment and its changing speed, changing acceleration and changing jerk according to the present invention.

FIG. 3 is an example of the synchronization between the calculation thread and the execution thread under the situation that the user actively stops in the present invention.

FIG. 4 is an example of the synchronization between calculation threads and execution threads in the case of unreachable points in the present invention.

Fig. 5 shows the calculation and execution of each time slice interpolation sequence in the jogging process of the present invention. In the figure: ①Initialization: After receiving the jog request, start to calculate the two interpolation point sequences of the first time slice; if the calculation is successful, start to execute the sequence. ② Transition between time slices: "Continue to run flag" is valid, that is, it is detected that the calculation of the two interpolation point sequences of the next time slice is completed, indicating that these path points are reachable; so the stop segment calculated in the previous time slice is discarded sequence, and execute a new acceleration (constant velocity) segment. ③Stop signal: "Continue to move flag" is cleared. ④Stop: The "continuing motion flag" is invalid, no longer calculate the new two-segment sequence, and execute the stop segment sequence in the next stage.

Fig. 6 shows the calculation and execution results of the interpolation sequence under two different conditions of jogging stop (active stop by the user/unreachable point) in the present invention.

Figure 7 takes a vertical six-axis articulated industrial robot as an example to show a schematic diagram of a single-axis jog operation in the joint coordinate system space.

Figure 8 shows that in the space of the Cartesian coordinate system, the end point TCP of the robot tool moves along the X, Y or Z axis of the Cartesian coordinate system, or rotates around the X, Y or Z axis of the Cartesian coordinate system for inching operation schematic diagram.

FIG. 9 is a schematic diagram of the robot tool end point TCP moving along the X, Y or Z axis of the tool coordinate system, or rotating around the X, Y or Z axis of the tool coordinate system in the space of the tool coordinate system for jog operation.

Detailed ways

The present invention will be further described below in conjunction with the accompanying drawings.

The key to the implementation of the invention lies in the synchronous management of the calculation thread and the execution thread of the robot jogging interpolation sequence by the control program. When the controller receives the jog request signal, the program successively starts the calculation thread and execution stroke of the interpolation sequence; after cyclically calculating and executing several interpolation sequences of acceleration & constant speed time slices, when it is detected that the user actively stops the jog (release the button) or there is an unreachable point in the robot movement (beyond the reachable range of the robot or a collision will occur), execute the stop segment sequence, log off the calculation process and execution thread in turn, and wait for the next jog request. The length of the time slice is fixed for the program, and needs to be determined by the program developer according to the actual situation of the controller. For example, if a controller has a real-time periodic control clock of 8 milliseconds, 20 cycles can be considered as a time slice, that is, every A time slice T ₂₀ is 160 milliseconds.

Fig. 3 and Fig. 4 are typical examples of synchronizing the calculation stroke and the execution thread by adopting the implementation steps of the technical solution of the present invention under the situation of the user actively stopping and the situation of unreachable points respectively. In the running time slice, when the execution thread receives a button release message or a calculation failure message, it will clear the "continue motion flag", and the next time slice decision is to execute the interpolation sequence of the stop segment. Whether the next time slice is to execute a new acceleration (constant speed) segment sequence, or to execute the stop segment sequence following the current time slice, is the control program reading the "continuing motion flag bit" at the end of each time slice interpolation sequence execution? ” is determined by the state. The calculation and execution results of each time slice interpolation sequence during the jogging process are shown in Figure 5 and Figure 6.

The following is single-axis jogging in the joint coordinate system space (Fig. 7), moving and rotating jogging in the Cartesian coordinate system space (Fig. 8), and moving and rotating jogging in the tool coordinate system space (Fig. 9) To further introduce the implementation steps of the present invention.

Use N to represent the number of cycles contained in each time slice, for example: N=20; use m to represent the number of cycles occupied by the stop segment ( Δt is the running time of the stop section; T is the control cycle time, for example: T=8ms).

(1) Single-axis jogging in joint coordinate system space (Figure 7)

Step1: Record the initial position or joint angle P ₀ of the jog axis;

Step2: Calculate the imputation sequence

The function for interpolating the interpolation amount θ _i (t) of the i-th time slice in the period variable acceleration segment is The function to interpolate the interpolation amount θ _i (t) of the i-th time slice in the acceleration-constant speed transition section is The function for interpolating the interpolation amount θ _i (t) of the i-th time slice in the constant velocity segment is The function for interpolating the interpolation amount θ(t) of the stop segment is

${\mathrm{<span\; class="notranslate">\; \&theta;</span>}}_{\mathrm{<span\; class="notranslate">\; 0</span>}}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; \&theta;</span>}}_{\mathrm{<span\; class="notranslate">\; 00</span>}}<span\; class="notranslate">\; +</span>{\stackrel{<span\; class="notranslate">\; \&CenterDot;</span>}{\mathrm{<span\; class="notranslate">\; \&theta;</span>}}}_{\mathrm{<span\; class="notranslate">\; 00</span>}}\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; -</span>\frac{\mathrm{<span\; class="notranslate">\; 4</span>}{\mathrm{<span\; class="notranslate">\; a</span>}}_{\mathrm{<span\; class="notranslate">\; D.</span>}\mathrm{<span\; class="notranslate">\; e</span>}\mathrm{<span\; class="notranslate">\; c</span>}\mathrm{<span\; class="notranslate">\; e</span>}\mathrm{<span\; class="notranslate">\; l</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; m</span>}\mathrm{<span\; class="notranslate">\; a</span>}\mathrm{<span\; class="notranslate">\; x</span>}}^{\mathrm{<span\; class="notranslate">\; 2</span>}}}{\mathrm{<span\; class="notranslate">\; 9</span>}{\stackrel{<span\; class="notranslate">\; \&CenterDot;</span>}{\mathrm{<span\; class="notranslate">\; \&theta;</span>}}}_{\mathrm{<span\; class="notranslate">\; 00</span>}}}{\mathrm{<span\; class="notranslate">\; t</span>}}^{\mathrm{<span\; class="notranslate">\; 3</span>}}<span\; class="notranslate">\; +</span>\frac{\mathrm{<span\; class="notranslate">\; 4</span>}{\mathrm{<span\; class="notranslate">\; a</span>}}_{\mathrm{<span\; class="notranslate">\; D.</span>}\mathrm{<span\; class="notranslate">\; e</span>}\mathrm{<span\; class="notranslate">\; c</span>}\mathrm{<span\; class="notranslate">\; e</span>}\mathrm{<span\; class="notranslate">\; l</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; m</span>}\mathrm{<span\; class="notranslate">\; a</span>}\mathrm{<span\; class="notranslate">\; x</span>}}^{\mathrm{<span\; class="notranslate">\; 3</span>}}}{\mathrm{<span\; class="notranslate">\; 27</span>}{\stackrel{<span\; class="notranslate">\; \&Center\; Dot;</span>}{\mathrm{<span\; class="notranslate">\; \&theta;</span>}}}_{\mathrm{<span\; class="notranslate">\; 00</span>}}^{\mathrm{<span\; class="notranslate">\; 2</span>}}}{\mathrm{<span\; class="notranslate">\; t</span>}}^{\mathrm{<span\; class="notranslate">\; 4</span>}}<span\; class="notranslate">\; .</span>$

If there is an unreachable point in the interpolation sequence at both ends of the i-th time slice (exceeding the limit of the jog axis may cause the robot to collide), or it is detected that the button has been released at this time, directly jump to Step4 and enter the stop section. The jog axis does not move; otherwise, the control program will start the execution program of the interpolation sequence and continue the calculation of the interpolation sequence.

Take N interpolation moments t=0, T, 2T,...(N-1)T for the incremental function of the acceleration & constant speed section, and obtain N interpolation increments △ _k (k=1,2,... ,N); record the incremental size △ _N and incremental speed of the last interpolation point Take m+1 interpolation point time t=0, T, 2T,...(m-1)T, △t for the incremental function of the stop segment, and obtain m+1 interpolation increments △ _k (k=N +1,N+2,…,N+m,N+m+1), the initial increment and initial increment speed of the first time slice are both 0, for the period variable acceleration section: the middle of the time slice point incremental acceleration It cannot exceed the maximum acceleration a _max allowed by the movement; the incremental acceleration at the midpoint of the first time slice can take a small value, for example: 0.03×a _max ; the acceleration increments of two adjacent time slices can take an appropriate value, Example: 0.05×a _max .

For single-axis jogging in the joint coordinate system space, the midpoint incremental acceleration of the time slice of the period variable acceleration segment It cannot exceed the maximum angular acceleration or linear acceleration allowed by the jog axis movement, and the end incremental velocity of the time slice of the acceleration-constant velocity transition period Corresponding to the expected movement speed of the jog axis given by the command, the incremental deceleration at the midpoint of the stop segment Corresponding to the maximum angular deceleration or linear deceleration allowed by the jog axis. From P ₀ +△ _k , the given value of axis joints of the N acceleration & constant speed segment interpolation sequence points in the first time slice can be calculated, as well as the m+1 stop segment interpolation sequence points immediately following this time slice The given value of the axis joint.

Step3: Execute the calculated time slice interpolation sequence, and return to Step2 to calculate the next time slice interpolation sequence synchronously.

The execution thread reads and executes the current time slice acceleration (constant speed) segment sequence that has been calculated; at the same time, the calculation thread starts to calculate the two interpolation sequences of the next time slice, and records the acceleration (constant speed) segment sequence in the two segments. Interpolate the incremental size and incremental velocity of the points, and update the state of the motion phase of the jog process.

Updating the state of the motion stage in the jog process needs to be guided by the state flow chart in Figure 1. During the calculation of the interpolation sequence, the initial increment size and angular velocity/linear velocity of the next time slice are the current time slice acceleration (constant speed) segment Increment size and angular/linear velocity of the last interpolated point.

Step4: End of jog

Log off the calculation process; if the execution thread is enabled, wait for the execution of the stop segment sequence to complete, and then log off the execution thread. Waiting for the next jog request.

(2) Movement and rotation in the Cartesian coordinate system space (Figure 8)

Jog in the Cartesian coordinate system space takes the Cartesian coordinate system as the reference coordinate system, and the end point TCP of the robot tool can be +X/-X/+Y/-Y/+Z/ along the coordinate axis X/Y/Z The movement jog in the -Z direction can also be the rotation jog in the +Rx/-Rx/+Ry/-Ry/+Rz/-Rz direction around the coordinate axis X/Y/Z. Specific steps are as follows:

Step1: Record the initial pose of the robot TCP

After receiving the jog request from the user, first record the positions of each joint, and obtain the 4×4 transformation matrix of the robot’s current tool end point TCP as T ₀ by solving the kinematics positive solution of the robot. R ₀ is a 3×3 coordinate rotation matrix representing attitude, and p ₀ is a 3×1 position offset vector representing position.

Step2: Calculate the imputation sequence of the first time slice

First of all, it is necessary to specify the planning object of jogging according to whether it is moving jogging or rotating jogging.

If it is jogging, the planning object is the position of the TCP in the X/Y/Z direction of the coordinate axis. Taking moving along the positive direction of the X-axis as an example, if the position increment along the +X direction is △ _j (j=1,2,…,N), the path position offset increment generated in the moving dimension is There is no change in the rotation dimension, and the corresponding rotation matrix increment is the identity matrix: △R _j =I _3×3 . Therefore, the incremental transformation matrix is The reference coordinate system of the movement is the Cartesian coordinate system, and the corresponding transformation matrix of the interpolation point is the original transformation matrix T ₀ multiplied by the incremental transformation matrix △T _j , namely:

${\mathrm{<span\; class="notranslate">\; T</span>}}_{\mathrm{<span\; class="notranslate">\; j</span>}}<span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; \&Delta;T</span>}}_{\mathrm{<span\; class="notranslate">\; j</span>}}<span\; class="notranslate">\; \&Center\; Dot;</span>{\mathrm{<span\; class="notranslate">\; T</span>}}_{\mathrm{<span\; class="notranslate">\; 0</span>}}<span\; class="notranslate">\; =</span>\left[\begin{array}{cc}{\mathrm{<span\; class="notranslate">\; I</span>}}_{\mathrm{<span\; class="notranslate">\; 3</span>}<span\; class="notranslate">\; \&times;</span>\mathrm{<span\; class="notranslate">\; 3</span>}}& {\mathrm{<span\; class="notranslate">\; \&Delta;p</span>}}_{\mathrm{<span\; class="notranslate">\; j</span>}}\\ \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 1</span>}\end{array}\right]\left[\begin{array}{cc}{\mathrm{<span\; class="notranslate">\; R</span>}}_{\mathrm{<span\; class="notranslate">\; 0</span>}}& {\mathrm{<span\; class="notranslate">\; p</span>}}_{\mathrm{<span\; class="notranslate">\; 0</span>}}\\ \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 1</span>}\end{array}\right]<span\; class="notranslate">\; =</span>\left[\begin{array}{cc}{\mathrm{<span\; class="notranslate">\; R</span>}}_{\mathrm{<span\; class="notranslate">\; 0</span>}}& {\mathrm{<span\; class="notranslate">\; p</span>}}_{\mathrm{<span\; class="notranslate">\; 0</span>}}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; \&Delta;p</span>}}_{\mathrm{<span\; class="notranslate">\; j</span>}}\\ \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 1</span>}\end{array}\right]$

If it is rotating and jogging, the planning object is the angle of the positive or negative rotation of the TCP around the coordinate axis X/Y/Z. Taking the rotation around the positive direction of the X axis as an example, if the rotation angle increment around the +X direction is Then the rotation matrix increment generated in the rotation dimension is There is no change in the movement dimension, and the corresponding position offset increment is a zero vector: Therefore, the incremental transformation matrix is The reference coordinate system of the movement is the Cartesian coordinate system, and the corresponding transformation matrix corresponding to the interpolation point is the original transformation matrix T ₀ multiplied by the incremental transformation matrix Right now:

${\stackrel{<span\; class="notranslate">\; ~</span>}{\mathrm{<span\; class="notranslate">\; T</span>}}}_{\mathrm{<span\; class="notranslate">\; j</span>}}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; \&Delta;</span>}{\stackrel{<span\; class="notranslate">\; ~</span>}{\mathrm{<span\; class="notranslate">\; T</span>}}}_{\mathrm{<span\; class="notranslate">\; j</span>}}<span\; class="notranslate">\; \&Center\; Dot;</span>{\mathrm{<span\; class="notranslate">\; T</span>}}_{\mathrm{<span\; class="notranslate">\; 0</span>}}<span\; class="notranslate">\; =</span>\left[\begin{array}{cc}\mathrm{<span\; class="notranslate">\; \&Delta;</span>}{\stackrel{<span\; class="notranslate">\; ~</span>}{\mathrm{<span\; class="notranslate">\; R</span>}}}_{\mathrm{<span\; class="notranslate">\; j</span>}}& {\mathrm{<span\; class="notranslate">\; \&Theta;</span>}}_{\mathrm{<span\; class="notranslate">\; 3</span>}<span\; class="notranslate">\; \&times;</span>\mathrm{<span\; class="notranslate">\; 1</span>}}\\ \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 1</span>}\end{array}\right]\left[\begin{array}{cc}{\mathrm{<span\; class="notranslate">\; R</span>}}_{\mathrm{<span\; class="notranslate">\; 0</span>}}& {\mathrm{<span\; class="notranslate">\; p</span>}}_{\mathrm{<span\; class="notranslate">\; 0</span>}}\\ \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 1</span>}\end{array}\right]<span\; class="notranslate">\; =</span>\left[\begin{array}{cc}\mathrm{<span\; class="notranslate">\; \&Delta;</span>}{\stackrel{<span\; class="notranslate">\; ~</span>}{\mathrm{<span\; class="notranslate">\; R</span>}}}_{\mathrm{<span\; class="notranslate">\; j</span>}}{\mathrm{<span\; class="notranslate">\; R</span>}}_{\mathrm{<span\; class="notranslate">\; 0</span>}}& \mathrm{<span\; class="notranslate">\; \&Delta;</span>}{\stackrel{<span\; class="notranslate">\; ~</span>}{\mathrm{<span\; class="notranslate">\; R</span>}}}_{\mathrm{<span\; class="notranslate">\; j</span>}}{\mathrm{<span\; class="notranslate">\; p</span>}}_{\mathrm{<span\; class="notranslate">\; 0</span>}}\\ \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 1</span>}\end{array}\right]$

After the planning object is defined, the specific content of this step can be described as: start the calculation thread, calculate the two interpolation sequences of the first time slice, and record the last of the acceleration & constant speed segment (that is, the period variable acceleration segment) in the two sequences. The increment size and speed of the interpolation points. If there are unreachable points in the two interpolation sequences (beyond the reachable range of the robot or will collide), or it is detected that the button has been released at this time, jump to Step5 and the robot will not move. Otherwise, the control program will start the execution thread of the interpolation sequence and enter Step3.

The method of calculating the interpolation increment of two interpolation sequences is the same as the method of calculating the interpolation increment of two interpolation sequences in the first case. The transformation matrix of each interpolation point can be calculated by using the movement jog calculation formula or the rotation jog calculation formula. By solving the kinematic inverse solution of the robot, the given values of the joints of each axis of the robot can be obtained as a part of a certain interpolation point. Group interpolation sequence; therefore, the axis joint given value of N groups of acceleration (constant speed) segment interpolation sequence points of the first time slice can be calculated, as well as m+1 groups of stop segment interpolation immediately following this time slice Axis joint reference for sequence points.

Step3: Execute the calculated time slice interpolation sequence, and calculate the next time slice interpolation sequence synchronously.

The execution thread reads and executes the current time slice acceleration & constant speed segment sequence that has been calculated; at the same time, the calculation thread starts to calculate the two interpolation sequences of the next time slice, and records the final interpolation of the acceleration & constant speed segment in the two segments The incremental size and incremental speed of the point, and update the state of the motion phase of the jog process.

Updating the state of the motion phase of the jog process needs to be guided by the state flow chart in Figure 1. During the calculation of the interpolation sequence, the incremental size and incremental speed of the next time slice are respectively the last interpolation point of the current time slice acceleration & constant speed segment The increment size and increment speed.

Step4: Repeat Step3, execute and calculate the time slice interpolation sequence cyclically, until the stop condition is satisfied.

After the calculation of the two interpolation sequences in the next time slice is completed, if the robot is within the reachable range of each group of sequence points, the calculation thread waits for the execution thread to complete the execution of the acceleration & constant speed sequence interpolation points of the current time slice; when the execution After completion, if the button is still pressed, return and repeat Step3 to perform synchronous calculation and execution of the subsequent time slice interpolation sequence. If there are unreachable points in the two interpolation sequences of the next time slice, or if the key is released after the execution is completed, the calculation thread will be logged off, and the next time slice will start to execute the stop sequence immediately following the current time slice.

Step5: Jog ends.

Log off the calculation process; if the execution thread is enabled, wait for the execution of the stop segment sequence to complete, and then log off the execution thread. Waiting for the next jog request.

(3) Movement and rotation in the tool coordinate system space (Figure 9)

The jogging in the tool space takes the tool coordinate system as the reference coordinate system, and the robot tool end point TCP can be +X/-X/+Y/-Y/+Z/ along the coordinate axis X/Y/Z of the tool coordinate system The movement jog in the -Z direction can also be the rotation jog in the +Rx/-Rx/+Ry/-Ry/+Rz/-Rz direction around the tool coordinate axis X/Y/Z.

The specific steps are very similar to the implementation steps Step1 to Step5 of moving and rotating jog in the Cartesian coordinate system space in case (2), the only difference is that in Step2 and Step3, the transformation matrix T _j or calculation method.

If it is jogging, the planning object is the position of the TCP in the X/Y/Z direction of the coordinate axis of the tool coordinate system. Take moving along the positive direction of the X-axis of the tool coordinate system as an example, if the position increment along the +X direction of the tool coordinate system is △ _j (j=1,2,…,N), then the path position generated in the moving dimension The offset increment is There is no change in the rotation dimension, and the corresponding rotation matrix increment is the identity matrix: △R _j =I _3×3 . Therefore, the incremental transformation matrix is The reference coordinate system of the movement is the tool coordinate system, which is described in the Cartesian coordinate space. The corresponding transformation matrix of the interpolation point is the original transformation matrix T ₀ multiplied by the incremental transformation matrix △T _j , namely:

${\mathrm{<span\; class="notranslate">\; T</span>}}_{\mathrm{<span\; class="notranslate">\; j</span>}}<span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; T</span>}}_{\mathrm{<span\; class="notranslate">\; 0</span>}}<span\; class="notranslate">\; \&Center\; Dot;</span>{\mathrm{<span\; class="notranslate">\; \&Delta;T</span>}}_{\mathrm{<span\; class="notranslate">\; j</span>}}<span\; class="notranslate">\; =</span>\left[\begin{array}{cc}{\mathrm{<span\; class="notranslate">\; R</span>}}_{\mathrm{<span\; class="notranslate">\; 0</span>}}& {\mathrm{<span\; class="notranslate">\; p</span>}}_{\mathrm{<span\; class="notranslate">\; 0</span>}}\\ \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 1</span>}\end{array}\right]\left[\begin{array}{cc}{\mathrm{<span\; class="notranslate">\; I</span>}}_{\mathrm{<span\; class="notranslate">\; 3</span>}<span\; class="notranslate">\; \&times;</span>\mathrm{<span\; class="notranslate">\; 3</span>}}& {\mathrm{<span\; class="notranslate">\; \&Delta;p</span>}}_{\mathrm{<span\; class="notranslate">\; j</span>}}\\ \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 1</span>}\end{array}\right]<span\; class="notranslate">\; =</span>\left[\begin{array}{cc}{\mathrm{<span\; class="notranslate">\; R</span>}}_{\mathrm{<span\; class="notranslate">\; 0</span>}}& {\mathrm{<span\; class="notranslate">\; R</span>}}_{\mathrm{<span\; class="notranslate">\; 0</span>}}{\mathrm{<span\; class="notranslate">\; \&Delta;p</span>}}_{\mathrm{<span\; class="notranslate">\; j</span>}}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; p</span>}}_{\mathrm{<span\; class="notranslate">\; 0</span>}}\\ \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 1</span>}\end{array}\right]$

If it is rotating and jogging, the planning object is the angle of the forward or reverse rotation of the TCP around the coordinate axis X/Y/Z of the tool coordinate system. Taking the rotation around the positive direction of the X-axis of the joint coordinate system as an example, if the rotation angle increment around the +X direction of the tool coordinate system is Then the rotation matrix increment generated in the rotation dimension There is no change in the movement dimension, and the corresponding position offset increment is a zero vector: Therefore, the incremental transformation matrix is The reference coordinate system of the movement is the tool coordinate system, which is described in the Cartesian coordinate space. The corresponding transformation matrix of the interpolation point is the original transformation matrix T ₀ multiplied by the incremental transformation matrix Right now:

${\stackrel{<span\; class="notranslate">\; ~</span>}{\mathrm{<span\; class="notranslate">\; T</span>}}}_{\mathrm{<span\; class="notranslate">\; j</span>}}<span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; T</span>}}_{\mathrm{<span\; class="notranslate">\; 0</span>}}<span\; class="notranslate">\; \&CenterDot;</span>\mathrm{<span\; class="notranslate">\; \&Delta;</span>}{\stackrel{<span\; class="notranslate">\; ~</span>}{\mathrm{<span\; class="notranslate">\; T</span>}}}_{\mathrm{<span\; class="notranslate">\; j</span>}}<span\; class="notranslate">\; =</span>\left[\begin{array}{cc}{\mathrm{<span\; class="notranslate">\; R</span>}}_{\mathrm{<span\; class="notranslate">\; 0</span>}}& {\mathrm{<span\; class="notranslate">\; p</span>}}_{\mathrm{<span\; class="notranslate">\; 0</span>}}\\ \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 1</span>}\end{array}\right]\left[\begin{array}{cc}\mathrm{<span\; class="notranslate">\; \&Delta;</span>}{\stackrel{<span\; class="notranslate">\; ~</span>}{\mathrm{<span\; class="notranslate">\; R</span>}}}_{\mathrm{<span\; class="notranslate">\; j</span>}}& {\mathrm{<span\; class="notranslate">\; \&Theta;</span>}}_{\mathrm{<span\; class="notranslate">\; 3</span>}<span\; class="notranslate">\; \&times;</span>\mathrm{<span\; class="notranslate">\; 1</span>}}\\ \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 1</span>}\end{array}\right]<span\; class="notranslate">\; =</span>\left[\begin{array}{cc}{\mathrm{<span\; class="notranslate">\; R</span>}}_{\mathrm{<span\; class="notranslate">\; 0</span>}}\mathrm{<span\; class="notranslate">\; \&Delta;</span>}{\stackrel{<span\; class="notranslate">\; ~</span>}{\mathrm{<span\; class="notranslate">\; R</span>}}}_{\mathrm{<span\; class="notranslate">\; j</span>}}& {\mathrm{<span\; class="notranslate">\; p</span>}}_{\mathrm{<span\; class="notranslate">\; 0</span>}}\\ \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 1</span>}\end{array}\right]$

Using the above two formulas to replace the calculation formulas in Step 2 and Step 3 of the implementation steps of movement and rotation in the Cartesian coordinate system space in the case (2), similar implementation steps can be used to realize the corresponding movement in the tool coordinate system space and turn jog.

The above is only a preferred embodiment of the present invention, it should be pointed out that for those of ordinary skill in the art, without departing from the principle of the present invention, some improvements and modifications can also be made. It should be regarded as the protection scope of the present invention.

Claims (6)

1. the method for control speed of a robot crawl operation, it is characterized in that: the rate control process that crawl operates is divided into and accelerates & at the uniform velocity section and stop segment, wherein accelerate & at the uniform velocity section comprise the cycle become accelerating sections, accelerate-at the uniform velocity changeover portion and at the uniform velocity section three subs, accelerate-at the uniform velocity changeover portion and at the uniform velocity section be two can or scarce subs, each sub carries out locus interpolation planning in units of the timeslice of regular length, and the acceleration of interpolation increment change is continuous in each timeslice, and the initial increment acceleration of each timeslice and termination increment acceleration are zero, each timeslice is made up of several control cycles of continuous print, after each timeslice completes, adopt according to Rule of judgment determination subsequent time and accelerate & at the uniform velocity section interpolation sequence or stop segment interpolation sequence, wherein Rule of judgment comprises: 1. which sub current control process is in, 2. whether crawl operation terminates, 3. whether robot motion is in coverage, and 4. whether robot produces collision,

With i=1,2 ..., X represents all timeslices accelerating & at the uniform velocity section, and the time span of timeslice is T ₂₀: the initial increment of i-th timeslice is θ _i0, corresponding initial incremental velocity and acceleration are respectively with the mid point increment of i-th timeslice is θ _ih, corresponding mid point incremental velocity and acceleration are respectively with it is the acceleration peak value of i-th timeslice simultaneously; The termination increment of i-th timeslice is θ _if, corresponding termination incremental velocity and acceleration are respectively with at the uniform velocity the given speed of section is it is velocity peak values simultaneously; The interpolation time is t, θ _{(i+1) 0}=θ _if, the initial increment of stop segment is θ ₀₀, corresponding initial incremental velocity and acceleration are respectively with the termination increment of stop segment is θ _0f, corresponding termination incremental velocity and acceleration are respectively with

2. the method for control speed of robot according to claim 1 crawl operation, it is characterized in that: when the interpolation sequence of execution i-th timeslice, calculate the interpolation sequence of (i+1) individual timeslice simultaneously, object is the interpolation sequence in order to can perform (i+1) individual timeslice immediately when i-th timeslice interpolation completes, and ensures the continuity of motion; The interpolation sequence of described (i+1) individual timeslice is divided into two classes: 1. accelerate the & at the uniform velocity interpolation sequence Normal of section and the interpolation sequence Ending of stop segment;

Described interpolation sequence Normal is divided into three kinds of situations: if the acceleration peak value of 1. (i+1) individual timeslice do not reach peak acceleration a _max, the speed of (i+1) individual timeslice does not also reach velocity peak values then (i+1) individual timeslice is the cycle become accelerating sections, adopts the cycle to become the interpolation sequence of accelerating sections interpolating method calculating (i+1) individual timeslice; If the 2. acceleration peak value of (i+1) individual timeslice reach peak acceleration a _maxbut the speed of (i+1) individual timeslice does not reach velocity peak values then (i+1) individual timeslice is acceleration-at the uniform velocity changeover portion, adopt to accelerate-at the uniform velocity changeover portion interpolating method calculate the interpolation sequence of (i+1) individual timeslice; If 3. the speed of (i+1) individual timeslice reaches velocity peak values then (i+1) individual timeslice is at the uniform velocity section, adopts at the uniform velocity section interpolating method to calculate the interpolation sequence of (i+1) individual timeslice;

Owing to not knowing when crawl operation stops, therefore needing to calculate interpolation sequence Ending while the interpolation sequence performing each timeslice, the starting point of interpolation sequence Ending is the continuity of i-th interpolation sequence Normal.

3. the method for control speed of robot according to claim 1 crawl operation, is characterized in that: the described cycle becomes accelerating sections and comprises several timeslices; Become in accelerating sections in the cycle, the acceleration peak value of adjacent two timeslices increases progressively fixed value △ a successively, namely require the acceleration peak value of the rear gained of (i+1) individual timeslice planning be no more than peak acceleration a _max; If i-th timeslice becomes accelerating sections in the cycle, if acceleration peak value continues to increase △ a, the acceleration peak value of gained after (i+1) individual timeslice planning peak acceleration a will be exceeded _max, and after (i+1) individual timeslice planning, the speed of gained does not reach velocity peak values then (i+1) individual timeslice jumps to acceleration-at the uniform velocity changeover portion; Following fourth order polynomial is used to become the interpolation amount θ of i-th timeslice in accelerating sections to the cycle _it () carries out interpolation:

${\mathrm{\&theta;}}_i\left(t\right)={\mathrm{\&theta;}}_{i0}+{\stackrel{\&CenterDot;}{\mathrm{\&theta;}}}_{i0}t+\frac{2{\stackrel{\&CenterDot;\&CenterDot;}{\mathrm{\&theta;}}}_{ih}}{3T_{20}}{t}^3-\frac{{\stackrel{\&CenterDot;\&CenterDot;}{\mathrm{\&theta;}}}_{ih}}{3T_{20}^2}{t}^4.$

4. robot according to claim 1 crawl operation method for control speed, it is characterized in that: if described existences acceleration-at the uniform velocity changeover portion, then acceleration-at the uniform velocity changeover portion only take a timeslice; Use following fourth order polynomial to accelerating-the interpolation amount θ of i-th timeslice at the uniform velocity in changeover portion _it () carries out interpolation:

${\mathrm{\&theta;}}_i\left(t\right)={\mathrm{\&theta;}}_{i0}+{\stackrel{\&CenterDot;}{\mathrm{\&theta;}}}_{i0}t+\frac{{\stackrel{\&CenterDot;}{\mathrm{\&theta;}}}_{if}-{\stackrel{\&CenterDot;}{\mathrm{\&theta;}}}_{i0}}{T_{20}^2}{t}^3-\frac{{\stackrel{\&CenterDot;}{\mathrm{\&theta;}}}_{if}-{\stackrel{\&CenterDot;}{\mathrm{\&theta;}}}_{i0}}{2T_{20}^3}{t}^4.$

5. the method for control speed of robot according to claim 1 crawl operation, is characterized in that: described at the uniform velocity section comprises several timeslices; Use following linear polynomial to the interpolation amount θ of i-th timeslice at the uniform velocity section _it () carries out interpolation:

${\mathrm{\&theta;}}_i\left(t\right)={\mathrm{\&theta;}}_{i0}+{\stackrel{\&CenterDot;}{\mathrm{\&theta;}}}_{i0}t.$

6. the method for control speed of robot according to claim 1 crawl operation, it is characterized in that: once find that crawl operation terminates, robot motion is unreachable or robot produces collision, then by acceleration & at the uniform velocity section directly jump to stop segment, at stop segment, speed is reduced to 0 in the mode that running time is short and the most level and smooth by crawl operation, at stop segment, no longer service time, sheet was split, and two use the maximum deceleration-a running and allow _{decel, max}plan as deceleration peak value, use following fourth order polynomial to the interpolation amount θ of stop segment ₀t () carries out interpolation:

${\mathrm{\&theta;}}_0\left(t\right)={\mathrm{\&theta;}}_{00}+{\stackrel{\&CenterDot;}{\mathrm{\&theta;}}}_{00}t-\frac{4a_{Decel,max}^2}{9{\stackrel{\&CenterDot;}{\mathrm{\&theta;}}}_{00}}{t}^3+\frac{4a_{Decel,max}^3}{27{\stackrel{\&CenterDot;}{\mathrm{\&theta;}}}_{00}^2}{t}^4;$

The running time of stop segment is: