JPS583002A - Control system of robot - Google Patents

Abstract

PURPOSE:To generate an indicating value by a smaller sampling period, and to move and control a body to be controlled, smoothly and with high accuracy, by interpolating a space against plural samples, joint angles and angular speeds. CONSTITUTION:When a start point and an end point have been provided, an operation processing part 1' generates a speed curve by which a movement in the axial direction ends simultaneously, from these data, in accordance with which a point on the moving curve extending from the start point to the end point is calculated at every sampling period. Subsequently, a sampling position Xk+m read in advance is outputted to a coordinate converting part 11, by (m) time point (m>=3). The coordinate converting part 11 converts the sampling position Xk+m outputted from the operation processing part 1', to each jount angle thetak+m, and outputs it to a locus interpolating part 12. The locus interpolating part 12 operates a value of each joint angle in the intermediate part of each sampling time point.

Description

DETAILED DESCRIPTION OF THE INVENTION The present invention relates to a control method for a robot, and in particular, a control method for a robot that enables accurate and smooth rotational control of each joint of a robot's arm and manibu. Regarding.

Conventionally, as control methods for robot arms and manipulators, a position control method in which a joint angle is given as an instruction value, and a speed control method in which an angular velocity is given as an instruction value have been widely adopted. The angular velocity during the joint angle is.

It is usually obtained by converting the position and velocity on spatial coordinates such as Cartesian coordinates or cylindrical coordinates.

For example, for the arithmetic processing device 1 in FIG.

Starting point Xo (Xo r Y@ + i@,
αO9βO9T, ) and the end point XN(xH, yw, zH,
aH, β□γm) (here α.

β, r are Euler angles), the arithmetic processing unit 1 uses the above data to create a speed curve as shown in Fig. 2 (A) K, such that movement in the directions of each “+1+” axis ends at the same time. Based on this, a transition curve from Xo to XN as shown in FIG. 2 (b) K is created.

Based on this, the arithmetic processing unit 1 operates as shown in FIG. 2()→
As shown in the figure, the joint angle θ, which is the arm movement amount for each section, is calculated by 4gK at the sampling time, and the joint angle at each sampling time is transmitted to the speed instruction value generation section 2.

The speed instruction value generation section 2 is as follows.

The joint angle ^ and encoder value calculated from the arithmetic processing unit 1 are sent to the speed function generator 5 (this is K).
i is compared, and when the indicated value θl is large, an output corresponding to this is generated and transmitted to the V/F converter 5. The V/F converter 5 converts the analog signal from the speed function generator 5 into a digital signal.

Output to comparison position counter 5. On the other hand, speed function generator 3
The output analog signal passes through the differentiator 4 and is transmitted to the motor 7 as acceleration. Comparison counter 6 is V/
The digital signals from the F converter 5 are counted and integrated, and the result is output as the joint angle θ. The encoder position counter 8 is connected to a pulse θhos that indicates the current position of the motor 7. The speed instruction value generator 2 and the position controller 10
The output signal is output to the D/A converter 9 connected to the D/A converter 9. Then, the difference between the information on the joint angle θh and the joint angle θi indicating the current position of the encoder position counter 8 is input to the position control unit 1o via the D/A converter 9. The position control unit 10 drives and controls the motor 7 so that the positional deviation between the joint angles θ and θ′ becomes zero.

However, with such a control system, the servo control is insufficient, so the applicant has filed patent application No. 111455-18788.
No. 7 "Robot Control Method", as shown in Figure 3, at the sampling time, the arithmetic processing unit 1 outputs the joint angle θ-11 at +1 time at the next sampling time, and this and the current It was proposed to compare the joint angle θd' of the joint angle θd' and to perform sheeting control based on the position deviation. This is K
According to the method, although the accuracy is greatly improved, there are still the following gaps.

That is, since there is generally a nonlinear relationship between joint angles and positions on spatial coordinates, it takes a long time to convert them. Therefore, when reading the current value and calculating based on 4, it is not possible to specify joint angles and angular velocities at fine sampling intervals, and discontinuities in the indicated values of joint angles and angular velocities at each sampling point occur. It was not possible to move the large 9-arm smoothly. Additionally, the position of the arm deviates from the indicated trajectory between each sampling point, which is a major problem in work where the movement path is important.

Therefore, an object of the present invention is to improve this problem by interpolating several sampled joint angles and angular velocities in order to make the controlled object follow the instructed trajectory without deviation. To provide a control method for a robot that can generate instruction values at a smaller sampling period and that allows a controlled object to smoothly stack each sampling point (movement control 1).

In order to achieve this object, the present invention provides a robot control system having function generating means for generating joint angles and angular velocities by calculating sampling position information on a predetermined instruction trajectory of a moving controlled object. 4. In this method, a trajectory interpolation means is provided to calculate interpolated values from several sampling positions on the indicated trajectory. Make it slight.

A further embodiment of the present invention will be described below with reference to FIGS. 4 to 6. FIG. 4 is a diagram illustrating the configuration of seven embodiments of the present invention, and FIGS. 5 and 6 are diagrams illustrating the operating state thereof.

In the figure, the alternative and the symbol parts indicate the same parts, 1' is an arithmetic processing unit and corresponds to the arithmetic processing unit 1 in Fig. 1, 11 is a coordinate transformation unit, and 12 is a trajectory interpolation unit. .

The arithmetic processing unit 1' calculates the starting point X・(xo+y・, 2@, α・,
β・.

γ0) and the end point XN(x),,yH,g)1. ay,
β, γN) (however, α, β, and γ are angles) are given.

From these data, create a speed 1 helix as shown in Figure 5 (a) so that the movement in each x, Y+''+α, β, and γ axis direction ends at the same time, and based on that, move from the starting point Xo to the ending point formula. Points on the curve are calculated every sampling period ΔT. Then, the sampling position III When, m■
Look ahead at the position at time +4 in 4.

It is output to the coordinate conversion section 11.

The coordinate conversion unit 11 adds + to each joint angle # according to the sampling position Xk+1 output from the arithmetic processing unit 1'.

(When the robot has a 6-joint arm, K is 10□〇z+,
... θ:+m)K conversion and output to the trajectory interpolation section 12.

The locus interpolation unit 12 is as shown by the X mark in FIG.

Value of each joint angle θ−* at the middle part of each sample time point
It calculates I+1 (!=O, p-1). When calculating this interpolated value, σ is equal to 1/, el
h-1+s.

・−°θk ”・θto++a−11θto+sa jl=
Using Z0m+1 (n≧5) points, θ and θ+1 are interpolated to calculate the above σk l+*(t−o, p−1). Now l! -1, mw3, the sixth
This will be explained based on the diagram.

First, the primary mold coefficient +t at the time of 60M + 5hys and 611 is determined.

Mother-in-law, θ−11−θh913 anchors−・・・・・・・・・・・・・・・・・・
... (1) al-hojiten 111111096
．． 806811011.608906. (2) ΔT δ. -〇・-^ , 11011995111011
611, -1-1. = filΔT 8 to 41 am l θ to +1 − θkat
I ・・・・・・・・・・・・・・・・・・・・・
(418o-o (initial value)...
・・・・・・・・・・・・・・・・・・(5) If S8+0 or 8 ni ga 1+0 then d 5kmO and Sk+1 s+I If Q then th+t −
, (to + θ ÷,) ... Scream ... River ... (7
) Next, find the interpolation coefficients ck and dklek, and use them to calculate the points θ between θ and +10, l+10−〇,...
p-1).

θsu+! −θh + (ch Tl+1 + dkT:
+t + @'I'l+'l )△T-alt-)-
1 TI + Seal - 1 ΔT (i-0, 1... p-1)...
The above θk l+1 obtained as αJ and Jin
is output to the speed instruction value generating section 2 at a period of ΔT/p.

Then, it becomes possible to perform control according to the difference between the interpolation points.

Next, the operation shown in FIG. 4 will be explained.

Here, a case where the spatial coordinates are orthogonal coordinates will be explained.

(1) First, as shown in FIG.
*ek + βs, re) and the end point XN (*w, 7)
l, zw+α1βw, γN) are given, then from these data create a velocity -M (Figure 5 (a)) such that each movement in the α, β, and r axis directions ends at the same time. , based on this, calculate the point on the moving curve from the starting point to the ending point is output to the coordinate transformation section 11.

(2) The coordinate conversion unit 11 receives this Xk+I and converts each joint angle θ into +l. For example, if the robot has a six-joint arm, the joint angle θ−+hθ
2 Ya 3. It outputs θt+, .

131 In the trajectory interpolation section 12, as shown in FIG. 6(b)K.

This newly transmitted +1 to 90, and +1 to θ-10H+9 which is transmitted in the trench, eθ-, 4 to -1 etc. to θ and 11°# distribution t1 as shown in the formula, θ Rod and θ-+1
, the point θ−−10.

Find (1-0, 1...p-1). In this way, the mark point 1jk-* 'jk, 1 in Figure 6 (b)
To t-...#- is obtained as an interpolation point between the joint angle θ wrinkle and ak+1. And this joint angle θhK continues,
These interpolation points (h I; one for θ; θh-...) are output to the speed instruction value generation section 2.

(4) As a result, the speed instruction value generation unit 2 uses the joint angle 6 obtained from the front trajectory interpolation unit 12 as the current position value θ from l+1 and the encoder position counter B attached to the motor.
- and K, the angular velocity θ-Zuichi (ek t+t-θ'k
I) Generate ZbeLC+-o, 1・p-1) and send this indicated value to the speed function generator! Output lK. Then, the angular velocity instruction value 11; t is shown in FIG. 6()→.

(5) This velocity function generator 3 compares the count number of the built-in counter and the angular velocity instruction value θ[I,
When this angular velocity instruction value is large, the counter number is increased, and when it is small, it is decreased so that the two coincide. This value is then D/A converted by the D/A converter 3',
It is output to the V/F converter 5.

In the Kono V21Fl converter 5, the analog output voltage of the D/A converter 5' is converted into a frequency.

The comparison position counter 6 counts and integrates this.

The result is outputted to the closed-loop control unit as the joint angle instruction value θ as -.

(6) On the other hand, in the encoder position counter 8, the peak -
The joint angle θ' of the digital signal indicating the current position is output from the rotation angle of E7, and the difference between the signal θ low indicating the current position and the joint angle instruction value θ, and l is output to the D/A converter. The signal is sent to the position control unit 10 via 9. As a result, the motor drive circuit of the motor 7 is controlled so that the positional deviation between the command value θλ and the signal θ;- indicating the current position becomes zero. Further, the output signal θ from the encoder position counter 8 is simultaneously fed back to the speed function generator 2, and the angular velocity corresponding to the current position and the interpolation point is output from the speed instruction value generator 2.

Of course, the present invention can be easily applied to any pattern in which the moving speed of the robot's arm during the moving path is arbitrary. For example, Fig. 7 (a) shows a case where constant acceleration/deceleration is performed near the start and end points in order to move smoothly, and Fig. 7 (b) shows a case where the variation of acceleration is constant. , is extremely effective in either case.

By making the and trajectory interpolation parts independent and configuring them to perform processing in parallel, the angular velocity of the joint angle can be calculated with a sampling period that is sufficiently smaller than the time required to convert the spatial position to each joint angle. An indication value can be generated.

Furthermore, by increasing the number of interpolations (that is, by increasing p in the 0 formula), it is possible to generate instruction values sequentially at a sufficiently small cycle compared to the conventional method, and it is possible to improve the position accuracy of the moving arm. It is. Furthermore, if the number of sampling points from the starting point Xo to the ending point island is reduced and the number of interpolations is increased accordingly, the arm can be moved faster than K.

Sara K again. In addition to the above-mentioned method using the quasi-Hermite interpolation formula, interpolation methods in the trajectory interpolation section include the 5th-order aplie interpolation formula that uses the second-order differential coefficient at the start point and the end point, and the B-aplin method that uses the m-order differential coefficient. There are methods such as using @interpolation formulas, and it is not fixed.

[Brief explanation of the drawing]

Figure 1 is a conventional robot control system, Figure 2 is an explanatory diagram of its operation, Figure 3 is an explanatory diagram of the operation of the previously applied application, Figure 4 is a configuration diagram of an embodiment of the present invention, and Figures 5 and 6. is an explanatory diagram of the operation, and FIG. 7 is another velocity/acceleration relationship diagram. In the figure, 1 is an arithmetic processing unit, 2 is a speed instruction value generation section, 3 is a speed function generation section, 4 is a differentiator, 5 is a voltage/frequency converter (V/P converter), 6 is a comparison position counter, 7
is a motor, 8 is an encoder position counter, 9 is a digital to analog converter (D/A converter), 10
11 represents a position control section, 11 represents a coordinate conversion section, and 12 represents a trajectory interpolation section. Patent Applicant: Fujitsu Ltd. Representative Patent Attorney Kou Yamatani

Claims (1)

[Claims] (1) In a robot control system that has a function generating means that generates joint angles and angular velocities by calculating information on sampling positions on nine predetermined indicated trajectories of a moving controlled object, several points on the indicated trajectory are A control method for four bots, characterized in that a trajectory interpolation means is provided for calculating interpolated values from sampling positions.