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

Abstract

The invention discloses a speed control method for inching operation of a robot. According to the method, the speed control process of inching operation is divided into an acceleration and constant speed stage and a stopping stage. The acceleration and constant-speed stage includes a periodically-changing acceleration substage, an acceleration-constant speed transition substage and a constant speed substage, and the acceleration-constant speed transition substage and the constant speed substage can be omitted. A time slice with a fixed duration serves as the unit of each substage, so that the interpolating increment of a track is planned. By the adoption of the speed control method for inching operation of the robot, micro-distance inching can be achieved, the problems that too great mechanical shock is caused by inching operation in a manual teaching process, and high-speed inching cannot be stopped smoothly in time are effectively solved. The speed control method can be used for speed control of inching operation of manual teaching in spaces of all coordinate systems of the robot.

Description

The method for control speed of a kind of robot crawl operation

Technical field

The present invention relates to a kind of method for control speed of crawl operation under robot teaching pattern, belong to Industrial Robot Technology.

Background technology

Under manual teaching pattern, crawl (JOG) operation is that robot teaching personnel driven machine people under the spaces such as all kinds of joint coordinate system space or cartesian coordinate system, tool coordinates system and user coordinate system moves and arrives the operation of taught point.When crawl runs, controller needs the key-press status coordinating teaching process, carries out interpolation calculating and execution in real time and synchronously.Traditional controller adopts one solution intuitively: controller calculates next interpolated point position at each real-time control cycle, and completes in same period and execute the task.But there is following shortcoming in this scheme: 1. crawl speed unsmooth, can produce mechanical shock; 2. setting in motion acceleration is general very large, cannot realize micro-displacement crawl; 3. only consider the range of movement in the single cycle, if when current kinetic speed is very large, sports limiting may be exceeded because stopping in time.

Summary of the invention

Goal of the invention: in order to overcome the deficiencies in the prior art, the present invention is directed to the situation that servomotor is in position control mode, the method for control speed of crawl operation under a kind of robot hand teaching pattern is provided, for the single shaft level and smooth crawl operation realized under joint of robot coordinate system space provides a kind of trajectory planning step and Program Realizing Method, provide a kind of trajectory planning step and Program Realizing Method for realizing mobile crawl under the spaces such as cartesian coordinate system, tool coordinates system and user coordinate system and rotating crawl operation simultaneously.The inventive method can make crawl process rate smoothing, effectively can reduce the impact to frame for movement, wearing and tearing and reduce sports energy consumption, realizing slight distance crawl, and solving the problem that high speed crawl cannot smoothly stop in time.

Technical scheme: for achieving the above object, the technical solution used in the present invention is:

The method for control speed of a kind of robot crawl operation, 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.

Cycle become accelerating sections, the changeover portion of acceleration-at the uniform velocity and at the uniform velocity section state transition relation as shown in Figure 1: when the crawl operating time is longer, crawl operation comprises above-mentioned three subs and stop segment; When the crawl operating time is shorter, accelerate-at the uniform velocity changeover portion and at the uniform velocity section may or scarce.As shown in Figure 2, the interpolation sequence of points of crawl is added by initial position territory interpolation increment to be determined the dynamic indicator of each stage interpolation increment.The method specifically comprise as lower part:

Part1: record initial position or posture information

Controller, after receiving crawl request command, records initial position or posture information.If perform the operation of single shaft crawl under joint space, the initial position of crawl is the initial angle of a moving axis; If perform the operation of single shaft crawl under the spaces such as cartesian coordinate system, tool coordinates system and user coordinate system, the initial pose of crawl is robot tool distal point (ToolCenterPoint, TCP) module and carriage transformation matrix under reference frame.

Part2: the definition of parameter

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 and θ ; 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 ; The interpolation time is t; i=1 time initial;

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

Part3: interpolation Design with Rule

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.

Part4: the calculating of the interpolation sequence of the timeslice of cycle change accelerating sections and execution

Control program opens the computational process of interpolation sequence, and first computing cycle becomes the interpolation sequence of i-th timeslice of accelerating sections, and the interpolation incremental rate curve function defining method that the cycle becomes accelerating sections is as follows:

Cycle becomes accelerating sections and can comprise 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.

The θ of each timeslice of accelerating sections is become due to the cycle _i0, with can be regarded as determined amounts, T ₂₀for fixed value, therefore can consider to use fourth order polynomial to become the interpolation amount θ of i-th timeslice in accelerating sections to the cycle _it () carries out interpolation, that is:

θ _i(t)＝ω _i0+ω _i1t+ω _i2t ²+ω _i3t ³+ω _i4t ⁴

Wherein ω _i0, ω _i1, ω _i2, ω _i3and ω _i4for multinomial coefficient, its constraints is:

$\left\{\begin{array}{c}{\mathrm{\θ}}_{i0}={\mathrm{\ω}}_{i0}\\ {\stackrel{\·}{\mathrm{\θ}}}_{i0}={\mathrm{\ω}}_{i1}\\ {\stackrel{\·\·}{\mathrm{\θ}}}_{i0}=2{\mathrm{\ω}}_{i2}\\ {\stackrel{\·\·}{\mathrm{\θ}}}_{ih}=2{\mathrm{\ω}}_{i2}+6{\mathrm{\ω}}_{i3}\left(\frac{T_{20}}{2}\right)+12{\mathrm{\ω}}_{i4}{\left(\frac{T_{20}}{2}\right)}^2\\ {\stackrel{\·\·}{\mathrm{\θ}}}_{if}=2{\mathrm{\ω}}_{i2}+6{\mathrm{\ω}}_{i3}{T}_{20}+12{\mathrm{\ω}}_{i4}{T}_{20}^2\end{array}\right.$

Solve the interpolation amount θ obtained i-th timeslice in cycle change accelerating sections _it function that () carries out interpolation is:

${\mathrm{\θ}}_i\left(t\right)={\mathrm{\θ}}_{i0}+{\stackrel{\·}{\mathrm{\θ}}}_{i0}t+\frac{2{\stackrel{\·\·}{\mathrm{\θ}}}_{ih}}{3T_{20}}{t}^3-\frac{{\stackrel{\·\·}{\mathrm{\θ}}}_{ih}}{3T_{20}^2}{t}^4$

Part5 the: accelerate-at the uniform velocity calculating of the interpolation sequence of changeover portion timeslice and execution

Owing to accelerating-θ of the at the uniform velocity timeslice of changeover portion _i0, with can be regarded as determined amounts, therefore can consider equally to use fourth order polynomial to accelerating-the interpolation amount θ of at the uniform velocity i-th timeslice of changeover portion _it () carries out interpolation, that is:

θ _i(t)＝ω _i0+ω _i1t+ω _i2t ²+ω _i3t ³+ω _i4t ⁴

Wherein ω _i0, ω _i1, ω _i2, ω _i3and ω _i4for multinomial coefficient, its constraints is:

$\left\{\begin{array}{c}{\mathrm{\θ}}_{i0}={\mathrm{\ω}}_{i0}\\ {\stackrel{\·}{\mathrm{\θ}}}_{i0}={\mathrm{\ω}}_{i1}\\ {\stackrel{\·\·}{\mathrm{\θ}}}_{i0}=2{\mathrm{\ω}}_{i2}\\ {\stackrel{\·\·}{\mathrm{\θ}}}_{if}={\mathrm{\ω}}_{i0}+2{\mathrm{\ω}}_{i1}{T}_{20}+3{\mathrm{\ω}}_{i2}{T}_{20}^2+4{\mathrm{\ω}}_{i3}{T}_{20}^3\\ {\stackrel{\·\·}{\mathrm{\θ}}}_{if}=2{\mathrm{\ω}}_{i2}+6{\mathrm{\ω}}_{i3}{T}_{20}+12{\mathrm{\ω}}_{i4}{T}_{20}^2\end{array}\right.$

Solve and obtain to accelerating-interpolation amount the θ of at the uniform velocity i-th timeslice of changeover portion _it function that () carries out interpolation is:

${\mathrm{\θ}}_i\left(t\right)={\mathrm{\θ}}_{i0}+{\stackrel{\·}{\mathrm{\θ}}}_{i0}t+\frac{{\stackrel{\·}{\mathrm{\θ}}}_{if}-{\stackrel{\·}{\mathrm{\θ}}}_{i0}}{T_{20}^2}{t}^3-\frac{{\stackrel{\·}{\mathrm{\θ}}}_{if}-{\stackrel{\·}{\mathrm{\θ}}}_{i0}}{2T_{20}^3}{t}^4$

Part6: the at the uniform velocity calculating of the interpolation sequence of section timeslice and execution

Due to the θ of the at the uniform velocity timeslice of section _i0with can be regarded as determined amounts, therefore can consider to use once linear multinomial to the interpolation amount θ of i-th timeslice at the uniform velocity section equally _it () carries out interpolation, that is:

θ _i(t)＝ω _i0+ω _i1t

Wherein ω _i0and ω _i1for multinomial coefficient, its constraints is:

$\left\{\begin{array}{c}{\mathrm{\θ}}_{i0}={\mathrm{\ω}}_{i0}\\ {\stackrel{\·}{\mathrm{\θ}}}_{i0}={\mathrm{\ω}}_{i1}\end{array}\right.$

Solve the interpolation amount θ obtained i-th timeslice at the uniform velocity section _it function that () carries out interpolation is:

${\mathrm{\θ}}_i\left(t\right)={\mathrm{\θ}}_{i0}+{\stackrel{\·}{\mathrm{\θ}}}_{i0}t$

Part7: the calculating of the interpolation sequence of stop segment and execution

If crawl operation terminates, then nullify calculation procedure; If open execution thread, after waiting for that stop segment sequence is finished, nullify execution thread, wait for crawl request next time.

Once find that crawl operation terminates, exceeds robot coverage or collide, then 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, and at stop segment, no longer service time, sheet was split, and uses the maximum deceleration-a running and allow _{decel, max}(-a _{decel, max}for on the occasion of) plan as deceleration peak value.

Due to the θ of stop segment ₀₀, ${\stackrel{\·}{\mathrm{\θ}}}_{00},{\stackrel{\·\·}{\mathrm{\θ}}}_{00}(=0),{\stackrel{\·\·}{\mathrm{\θ}}}_{0h}(=-a_{Decel,max}),{\stackrel{\·}{\mathrm{\θ}}}_{0f}(=0)$ With can regard determined amounts as, but △ t running time is a known variables, can considers equally to use fourth order polynomial to the interpolation amount θ of stop segment ₀t () carries out interpolation, that is:

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

Wherein ω ₀₀, ω ₀₁, ω ₀₂, ω ₀₃and ω ₀₄for multinomial coefficient, its constraints is:

$\left\{\begin{array}{c}{\mathrm{\θ}}_{00}={\mathrm{\ω}}_{00}\\ {\stackrel{\·}{\mathrm{\θ}}}_{00}={\mathrm{\ω}}_{01}\\ {\stackrel{\·\·}{\mathrm{\θ}}}_{00}=2{\mathrm{\ω}}_{02}\\ {\stackrel{\·\·}{\mathrm{\θ}}}_{0h}=2{\mathrm{\ω}}_{02}+6{\mathrm{\ω}}_{03}\left(\frac{\mathrm{\Δ}t}{2}\right)+12{\mathrm{\ω}}_{04}{\left(\frac{\mathrm{\Δ}t}{2}\right)}^2\\ {\stackrel{\·}{\mathrm{\θ}}}_{0f}={\mathrm{\ω}}_{00}+2{\mathrm{\ω}}_{01}\mathrm{\Δ}t+3{\mathrm{\ω}}_{02}{\mathrm{\Δt}}^2+4{\mathrm{\ω}}_{03}{\mathrm{\Δt}}^3\\ {\stackrel{\·\·}{\mathrm{\θ}}}_f=2{\mathrm{\ω}}_2+6{\mathrm{\ω}}_3\mathrm{\Δ}t+12{\mathrm{\ω}}_4{\mathrm{\Δt}}^2\end{array}\right.$

Solve the interpolation amount θ obtained stop segment ₀t function that () carries out interpolation is:

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

The running time of stop segment is:

Beneficial effect: the method for control speed of robot provided by the invention crawl operation, relative to prior art, has following advantage: the exercise performance 1, improving crawl operation.Compared to the crawl solution of traditional controller, the program results accelerating curve of crawl of the present invention is continuous, and rate smoothing, effectively can reduce the mechanical shock in crawl process and wearing and tearing, the extension device life-span; Crawl starting stage acceleration is less and strengthen gradually, and can realize micro-displacement crawl, the user that is more convenient for carries out teaching operation to robot; In units of timeslice, planning process takes into full account that can crawl stop in coverage, avoids robot high speed crawl easily run off the soft spacing problem of motion in algorithm aspect.2, there is versatility.The present invention does not relate to the particular type of robot, has versatility to the crawl operation realizing method of robot teaching.

Accompanying drawing explanation

Fig. 1 is the state transition schematic diagram of crawl trajectory planning.Program is at first waits for crawl solicited status, and when push button signalling being detected, the cycle that jumps to becomes accelerating sections state; If key press time is longer, will by accelerate-at the uniform velocity changeover portion enter at the uniform velocity section state.In process, once detect that button unclamps or has unreachable interpolated point, stop segment state will be entered.In figure: crawl request signal 1. detected; 2. future time sheet acceleration increases progressively (but must not exceed peak acceleration), and the termination speed of planning gained is still less than given at the uniform velocity section speed; If 3. future time sheet acceleration increases progressively (but must not exceed peak acceleration), the termination speed of planning gained will exceed given at the uniform velocity section speed; 4. do not receive crawl ending request, and the motion of future time sheet can reach; 5. receive crawl ending request, or future time sheet there is unreachable point.

Fig. 2 is the planning schematic diagram of four stages interpolation increment of the present invention and pace of change, change acceleration and change acceleration.

Fig. 3 is under user of the present invention initiatively stops situation, the synchronous citing of computational threads and execution thread.

Fig. 4 be of the present invention have unreachable some situation under, the synchronous citing of computational threads and execution thread.

Fig. 5 is calculating and the implementation status of each timeslice interpolation sequence in crawl process of the present invention.In figure: 1. initialize: receive crawl request, start the two sections of interpolated point sequences calculating first timeslice; If calculate successfully, start to perform sequence.2. transition between timeslice: " continuing running mark position " effectively, namely detects that two sections of interpolated point sequences of future time sheet have calculated, illustrate that these path point all can reach; So abandon the stop segment sequence that a timeslice calculates, and perform new acceleration (at the uniform velocity) section.3. stop signal: " continuing campaign signs position " is cleared.4. stop: " continuing campaign signs position " lost efficacy, and no longer calculated two sections of new sequences, and the next stage performs stop segment sequence.

Fig. 6 is calculating and the execution result of interpolation sequence under crawl stopping (user initiatively stops/have unreachable point) condition that the present invention two kinds is different.

Fig. 7 is artificially routine with vertical six axle joint type industrial machines, the schematic diagram of single shaft crawl operation under displaying joint coordinate system space.

Fig. 8 is under cartesian coordinate system space, and robot tool distal point TCP moves along X, Y of cartesian coordinate system or Z axis, or rotates around X, Y of cartesian coordinate system or Z axis, carries out the schematic diagram of crawl operation.

Fig. 9 is under tool coordinates system space, and robot tool distal point TCP moves along X, Y of tool coordinates system or Z axis, or rotates around X, Y of tool coordinates system or Z axis, carries out the schematic diagram of crawl operation.

Detailed description of the invention

Below in conjunction with accompanying drawing, the present invention is further described.

Key of the invention process is the computational threads of control program to robot crawl interpolation sequence and the management by synchronization of execution thread.When controller receives crawl request signal, program opens the computational threads of interpolation sequence and execution stroke in succession; Cycle calculations with perform after several accelerate the interpolation sequence of & at the uniform velocity section timeslice, when detecting that user initiatively stops crawl (unclamping button) or robot motion to have unreachable point (exceeding robot coverage maybe will collide), perform stop segment sequence, nullify calculation procedure and execution thread successively, wait for crawl request next time.The length of timeslice is fixing for program, needs application developer to determine according to controller actual conditions, citing: it is 8 milliseconds that certain controller scheduling real-time periodic controls clock, can consider that with 20 cycles be a timeslice, i.e. each timeslice T ₂₀it is 160 milliseconds.

Fig. 3 and Fig. 4 be under user initiatively stops situation respectively, have unreachable some situation under, adopt the implementation step of technical solution of the present invention, calculate the stroke typical case synchronous with execution thread and illustrate.Operationally in sheet, receive button release message or calculate failed message when execution thread, " continuing campaign signs position " can be reset, next timeslice decision-making is for performing stop segment interpolation sequence.Future time sheet is perform new acceleration (at the uniform velocity) section sequence on earth, or performing current time sheet stop segment sequence immediately, is the Determines that control program passes through to read " continuing campaign signs position " at the end of each timeslice interpolation sequence performs.In crawl process, the calculating of each timeslice interpolation sequence and execution result are as shown in Figure 5 and Figure 6.

Implementation step of the present invention is introduced further below with the single shaft crawl (Fig. 7) under joint coordinate system space, the movement under cartesian coordinate system space and the movement of rotating under crawl (Fig. 8) and tool coordinates system space and rotation crawl (Fig. 9).

N is used to represent the periodicity that each timeslice comprises, citing: N=20; The periodicity that uses m to represent that stop segment takies ( △ t is the running time of stop segment; T is the control cycle time, citing: T=8ms).

(1) the single shaft crawl (Fig. 7) under joint coordinate system space

Step1: the initial position of measuring point moving axis or joint angles P ₀;

Step2: calculate interpolation sequence

Become to the interpolation amount θ of i-th timeslice in accelerating sections the cycle _it function that () carries out interpolation is to accelerating-the interpolation amount θ of at the uniform velocity i-th timeslice of changeover portion _it function that () carries out interpolation is to the interpolation amount θ of i-th timeslice at the uniform velocity section _it function that () carries out interpolation is function interpolation amount θ (t) of stop segment being carried out to interpolation is

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

If there is unreachable point (exceeding the spacing robot that maybe will cause of a moving axis to collide) i-th timeslice two ends interpolation sequence, or detect that now button unclamps, directly jumps to Step4, enter stop segment, some moving axis is failure to actuate; Otherwise control program will open the performing a programme of interpolation sequence, and proceeds the calculating of interpolation sequence.

To acceleration & at the uniform velocity section increment function get N number of interpolation moment t=0, T, 2T ... (N-1) T, obtains the N number of interpolation increment △ accelerating & at the uniform velocity section _k(k=1,2 ..., N); Record the increment size △ of last interpolated point _nand incremental velocity m+1 interpolated point moment t=0 is got to stop segment increment function, T, 2T ... (m-1) T, △ t, obtains m+1 interpolation increment △ of stop segment _k(k=N+1, N+2 ..., N+m, N+m+1), initial increment and the initial incremental velocity of first timeslice are 0, become accelerating sections: the mid point increment acceleration of timeslice for the cycle the peak acceleration a that motion allows can not be exceeded _max; The mid point increment acceleration of first timeslice can get a smaller value, citing: 0.03 × a _max; Adjacent two timeslice acceleration incremental changes can get a desired value, citing: 0.05 × a _max.

For the single shaft crawl under joint coordinate system space, the mid point increment acceleration of cycle change accelerating sections timeslice maximum angular acceleration or a linear acceleration that the motion of a some moving axis allows can not be exceeded, accelerate-termination the incremental velocity of at the uniform velocity changeover portion timeslice correspond to the some moving axis desired motion speed that instruction is given, the mid point incremental retard degree of stop segment the maximum angular deceleration that the motion of corresponding points moving axis allows or line deceleration.By P ₀+ △ _kthe axle joint set-point of N number of acceleration & at the uniform velocity section interpolation sequence of points of first timeslice can be calculated, and with the axle joint set-point of this timeslice m+1 stop segment interpolation sequence of points immediately.

Step3: perform the timeslice interpolation sequence calculated, synchronously return Step2 and calculate next timeslice interpolation sequence.

Execution thread reads and performs the current time sheet calculated and accelerates (at the uniform velocity) section sequence; Meanwhile, computational threads starts the two sections of interpolation sequences calculating future time sheet, the increment size of the last interpolated point of acceleration (at the uniform velocity) section in recording two sections and incremental velocity, and upgrades the motion stage state of crawl process.

The motion stage state upgrading crawl process needs to lead according to the state flow chart of Fig. 1, in interpolation sequence computational process, the initial increment size of future time sheet and angular velocity/linear velocity are respectively increment size and the angular velocity/linear velocity that current time sheet accelerates the last interpolated point of (at the uniform velocity) section.

Step4: crawl terminates

Nullify calculation procedure; If open execution thread, after waiting for that stop segment sequence is finished, nullify execution thread.Wait for crawl request next time.

(2) movement under cartesian coordinate system space and rotation crawl (Fig. 8)

Crawl under cartesian coordinate system space is reference frame with cartesian coordinate, robot tool distal point TCP can be the mobile crawl carrying out+X/-X/+Y/-Y/+Z/-Z direction along reference axis X/Y/Z, also can be the rotation crawl carrying out+Rx/-Rx/+Ry/-Ry/+Rz/-Rz direction around reference axis X/Y/Z.Concrete steps are as follows:

Step1: the initial pose of recorder people TCP

After receiving the request of user's crawl, first record each joint position, by solving the forward kinematics solution of robot, 4 × 4 transformation matrixs obtaining robot current tool distal point TCP are T ₀, r ₀for representing 3 × 3 coordinate spin matrixs of attitude, p ₀for locative 3 × 1 position offset vectors.

Step2: the interpolation sequence calculating first timeslice

First need, according to being mobile crawl or rotating crawl, to specify the plan objects of crawl.

If mobile crawl, plan objects is the position of TCP on reference axis X/Y/Z direction.To move along X-axis positive direction, if the positional increment in edge+X-direction is △ _j(j=1,2 ..., N), then moving the path position offset increment that dimension produces is rotate not change in dimension, corresponding spin matrix increment is unit matrix: △ R _j=I _{3 × 3}.Therefore, increment transformation matrix is the reference coordinate of motion is cartesian coordinate system, and the correspondent transform matrix of interpolated point is former transformation matrix T ₀premultiplication increment transformation matrix △ T _j, that is:

$T_j={\mathrm{\ΔT}}_j\·T_0=\left[\begin{array}{cc}{I}_{3\×3}& {\mathrm{\Δp}}_j\\ 0& 1\end{array}\right]\left[\begin{array}{cc}{R}_0& p_0\\ 0& 1\end{array}\right]=\left[\begin{array}{cc}{R}_0& p_0+{\mathrm{\Δp}}_j\\ 0& 1\end{array}\right]$

If rotation crawl, plan objects is the angular dimension that just or in the other direction rotating of TCP around reference axis X/Y/Z.To rotate around X-axis positive direction, if around the rotation angle increment of+X-direction be then rotate the spin matrix increment that dimension produces not change in mobile dimension, corresponding position offset increment is null vector: therefore, increment transformation matrix is the reference coordinate of motion is cartesian coordinate system, and the correspondent transform matrix of corresponding interpolated point is former transformation matrix T ₀premultiplication increment transformation matrix that is:

${\tilde{T}}_j=\mathrm{\Δ}{\tilde{T}}_j\·T_0=\left[\begin{array}{cc}\mathrm{\Δ}{\tilde{R}}_j& {\mathrm{\Θ}}_{3\×1}\\ 0& 1\end{array}\right]\left[\begin{array}{cc}{R}_0& p_0\\ 0& 1\end{array}\right]=\left[\begin{array}{cc}\mathrm{\Δ}{\tilde{R}}_j{R}_0& \mathrm{\Δ}{\tilde{R}}_j{p}_0\\ 0& 1\end{array}\right]$

After specifying plan objects, the particular content of this step can be described as: open computational threads, calculate two sections of interpolation sequences of first timeslice, and record in two sections of sequences and accelerate the & at the uniform velocity increment size of the last interpolated point of section (i.e. cycle change accelerating sections) and incremental velocity.If there is unreachable point (exceeding robot coverage maybe will collide) in two sections of interpolation sequences, or detect that now button unclamps, jumps to Step5, robot is failure to actuate.Otherwise control program, by opening the execution thread of interpolation sequence, enters Step3.

The method that computational methods and the first situation of two sections of interpolation sequence interpolation increments calculate two sections of interpolation sequence interpolation increments is identical.Utilizing mobile crawl calculating formula or rotate the transformation matrix that crawl calculating formula can calculate each interpolated point, by solving the Inverse Kinematics Solution of robot, each axle joint set-point of robot can be obtained, as one group of interpolation sequence of certain interpolated point; Thus, the N group that can calculate first timeslice accelerates the axle joint set-point of (at the uniform velocity) section interpolation sequence of points, and with the axle joint set-point of this timeslice m+1 group stop segment interpolation sequence of points immediately.

Step3: perform the timeslice interpolation sequence calculated, the next timeslice interpolation sequence of synchronous calculating.

Execution thread reads and performs the current time sheet calculated and accelerates & at the uniform velocity section sequence; Meanwhile, computational threads starts the two sections of interpolation sequences calculating future time sheet, and the acceleration & in recording two sections is the increment size of the last interpolated point of section and incremental velocity at the uniform velocity, and upgrades the motion stage state of crawl process.

The motion stage state upgrading crawl process needs to lead according to the state flow chart of Fig. 1, in interpolation sequence computational process, the increment size of future time sheet and incremental velocity are respectively current time sheet and accelerate the & at the uniform velocity increment size of the last interpolated point of section and incremental velocity.

Step4: repeat Step3, circulation performs and sheet interpolation computing time sequence, until meet stop condition.

After next timeslice two sections of interpolation sequences calculate, if machine is in coverage per capita under each group sequence of points, computational threads wait for execution thread by the acceleration & of current time sheet at the uniform velocity section sequence interpolated point be finished; After being finished, if button is still in down state, returns and repeat synchronous calculating and the execution that Step3 carries out subsequent time slice interpolation sequence.If two of next timeslice sections of interpolation sequences exist unreachable point, or detect after being finished that button unclamps, nullify computational threads, future time sheet starts to perform current time sheet stop segment sequence immediately.

Step5: crawl terminates.

Nullify calculation procedure; If open execution thread, after waiting for that stop segment sequence is finished, nullify execution thread.Wait for crawl request next time.

(3) movement under tool coordinates system space and rotation crawl (Fig. 9)

Crawl under instrument system space is reference frame with tool coordinates, robot tool distal point TCP can be the mobile crawl carrying out+X/-X/+Y/-Y/+Z/-Z direction along tool coordinates system reference axis X/Y/Z, also can be the rotation crawl carrying out+Rx/-Rx/+Ry/-Ry/+Rz/-Rz direction around tool coordinates axle X/Y/Z.

Concrete steps and (2) moved and the rotated crawl step Step1 to Step5 that realizes under planting situation cartesian coordinate system space is very similar, and unique difference is in Step2 and Step3 interpolated point transformation matrix T _jor computational methods.

If mobile crawl, plan objects is the position of TCP on tool coordinates system reference axis X/Y/Z direction.To move along tool coordinates system X-axis positive direction, if be △ along the positional increment in tool coordinates system+X-direction _j(j=1,2 ..., N), then moving the path position offset increment that dimension produces is rotate not change in dimension, corresponding spin matrix increment is unit matrix: △ R _j=I _{3 × 3}.Therefore, increment transformation matrix is the reference coordinate of motion is tool coordinates system, and describe under being transformed into cartesian coordinate space, the correspondent transform matrix of interpolated point is former transformation matrix T ₀increment transformation matrix △ T is taken advantage of on the right side _j, that is:

$T_j=T_0\·{\mathrm{\ΔT}}_j=\left[\begin{array}{cc}{R}_0& p_0\\ 0& 1\end{array}\right]\left[\begin{array}{cc}{I}_{3\×3}& {\mathrm{\Δp}}_j\\ 0& 1\end{array}\right]=\left[\begin{array}{cc}{R}_0& R_0{\mathrm{\Δp}}_j+p_0\\ 0& 1\end{array}\right]$

If rotation crawl, plan objects is the angular dimension that just or in the other direction rotating of TCP around tool coordinates system reference axis X/Y/Z.To rotate, if around the rotation angle increment of tool coordinates system+X-direction be around joint coordinate system X-axis positive direction then rotate the spin matrix increment that dimension produces not change in mobile dimension, corresponding position offset increment is null vector: therefore, increment transformation matrix is the reference coordinate of motion is tool coordinates system, and describe under being transformed into cartesian coordinate space, the correspondent transform matrix of interpolated point is former transformation matrix T ₀increment transformation matrix is taken advantage of on the right side that is:

${\tilde{T}}_j=T_0\·\mathrm{\Δ}{\tilde{T}}_j=\left[\begin{array}{cc}{R}_0& p_0\\ 0& 1\end{array}\right]\left[\begin{array}{cc}\mathrm{\Δ}{\tilde{R}}_j& {\mathrm{\Θ}}_{3\×1}\\ 0& 1\end{array}\right]=\left[\begin{array}{cc}{R}_0\mathrm{\Δ}{\tilde{R}}_j& p_0\\ 0& 1\end{array}\right]$

Move and rotate the calculating formula realized in step Step2 and Step3 of crawl under replacing (2) to plant situation cartesian coordinate system space respectively with above-mentioned two formulas, similar implementation step can be adopted, the movement under corresponding implementation tool coordinate system space and rotation crawl.

The above is only the preferred embodiment of the present invention; be noted that for those skilled in the art; under the premise without departing from the principles of the invention, can also make some improvements and modifications, these improvements and modifications also should be considered as 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{\θ}}_i\left(t\right)={\mathrm{\θ}}_{i0}+{\stackrel{\·}{\mathrm{\θ}}}_{i0}t+\frac{2{\stackrel{\·\·}{\mathrm{\θ}}}_{ih}}{3T_{20}}{t}^3-\frac{{\stackrel{\·\·}{\mathrm{\θ}}}_{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{\θ}}_i\left(t\right)={\mathrm{\θ}}_{i0}+{\stackrel{\·}{\mathrm{\θ}}}_{i0}t+\frac{{\stackrel{\·}{\mathrm{\θ}}}_{if}-{\stackrel{\·}{\mathrm{\θ}}}_{i0}}{T_{20}^2}{t}^3-\frac{{\stackrel{\·}{\mathrm{\θ}}}_{if}-{\stackrel{\·}{\mathrm{\θ}}}_{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{\θ}}_i\left(t\right)={\mathrm{\θ}}_{i0}+{\stackrel{\·}{\mathrm{\θ}}}_{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{\θ}}_0\left(t\right)={\mathrm{\θ}}_{00}+{\stackrel{\·}{\mathrm{\θ}}}_{00}t-\frac{4a_{Decel,max}^2}{9{\stackrel{\·}{\mathrm{\θ}}}_{00}}{t}^3+\frac{4a_{Decel,max}^3}{27{\stackrel{\·}{\mathrm{\θ}}}_{00}^2}{t}^4;$

The running time of stop segment is: