JP3198467B2 - Sensor position and attitude path tracking control method - Google Patents

Description

DETAILED DESCRIPTION OF THE INVENTION

[0001]

BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention relates to a position and orientation of a sensor used for controlling the movement path and orientation of a robot arm and an end effector by a sensor with respect to an object to be worked in a work of assembling and processing mechanical parts. The present invention relates to a path following control method.

[0002]

2. Description of the Related Art In a conventional robot sensor position / posture path tracking control system, a sensor is attached to a robot hand along with an end effector, and the position / posture information of a path ahead of the end effector is moved while moving with the end effector. May be collected. At this time, in order to collect information stably, it is necessary to reliably capture the path within the field of view of the sensor, and for that purpose, the position and orientation of the sensor must be accurately controlled so as to follow the target path. Must. This is especially important when the field of view of the sensor is narrow.

[0003] One of the prior arts will be described with reference to the drawings. 2A and 2B show an example of a conventional method for determining the position and orientation of a sensor using past sensor data. FIG. 2A is an explanatory diagram using one-point approximation, and FIG. 2B is an explanatory diagram using two-point difference approximation. In the drawing, 1 is a target path, 2 is an end effector central axis, 3 is a sensor visual field central axis, 4 is a sensor visual field range, and D1 to Dn are sensor data on the target path 1 detected by sensors in the past.

Here, it is assumed that the sensor has a mechanism capable of rotating around the end effector center axis 2. As shown in FIG. 2A, the latest sensor data D
It is estimated that a path on the straight line L1 connecting the position indicated by n and the current position of the end effector center axis 2 continues,
The sensor is controlled using the one-point approximation method around the end effector central axis 2 so that the sensor visual field central axis 3 is located on the straight line L1.

[0005] As shown in FIG. 2B, it is estimated that the path continues on a straight line L2 connecting the positions indicated by the latest sensor data Dn and Dn-1. The sensor is controlled using the two-point difference approximation method around the end effector center axis 2 so that the center axis 3 comes. In this example, although the target path 1 is included in the sensor visual field range 4, it includes a considerable error.

[0006]

In the control method as described above, the estimation of the continuation direction of the route is performed by approximating a straight line using the past sensor data itself or the difference. When the range is narrow, or when the radius of curvature of the route is small and the estimation error is likely to be large, there is a disadvantage that it is difficult to capture the target route in the field of view of the sensor. In view of the above-mentioned problems of the related art, the present invention controls the position and orientation of a sensor based on a more advanced estimated route, so that the sensor can reliably capture the route in its field of view. It does not provide a law.

The above object can be attained by the present invention employing the following novel characteristic configuration method. That is,
A feature of the present invention is that a path output from a sensor which is attached to a robot hand along with an end effector and predicts and detects the position and orientation of the end effector target path on a work object in front of the end effector. From the position and orientation data and the position and orientation of the sensor represented in the reference coordinate system, a path function representing the position and orientation of the end effector target path in the reference coordinates is determined and output, and based on the path function. In a position and orientation path following control method for controlling the position and orientation of the end effector of the robot, a current position on a target path drawn by a path function of the end effector, and a distance between the end effector and the sensor. From equation, the equation of the sensor field range center axis following behavior is calculated, and then the path that the sensor should take The position and orientation of the end-effector target path is determined by establishing a simultaneous equation based on the equation and the path function, calculating the solution thereof, and ensuring that the sensor captures the end effector target path in the field of view. This is a position and orientation path following control method.

[0008]

According to the present invention, the position / posture of a path at the time of newly performing sensing using a path function sequentially generated based on discrete path information from a sensor by taking the above-described method. Is estimated, and the position and orientation of the sensor are controlled based on the values, and the following behavior of the central axis of the sensor visual field range is expressed by an equation.

From the solution when the equation and the latest path function are combined, the position that the center axis of the sensor field of view should take,
Since the attitude is determined and the angle of the sensor around the central axis of the end effector and the attitude of the sensor are controlled based on the result, the center axis of the sensor visual field range is obtained by using the estimated value of the advanced path obtained from the path function. Is accurately controlled to the target path, and the end effector path is reliably captured.

[0010]

BRIEF DESCRIPTION OF THE DRAWINGS FIG. FIG.
FIG. 3 is an explanatory diagram of a sensor position / posture path following control method according to the present embodiment. In the drawing, 5 is a path drawn by the latest path function obtained from the sensor data D1 to Dn, and 6 is a section D1 immediately before the path
A path described by a path function of Dn-1 to 7 is a three-dimensional solid coordinate system composed of three axes XE, YE, and ZE fixed to the end effector. The same reference numerals as in the conventional example of FIG. 2 denote the same parts. Here, for simplicity of explanation, it is assumed that the sensor only has a degree of freedom to rotate around the central axis 2 of the end effector, and its posture is not changed.

Specific means (not shown) for realizing this embodiment include an end effector attached to the hand of the robot, the position of an end effector path on a work object (not shown) in front of the end effector, and A sensor for detecting and predicting the attitude, path function determining means for obtaining a path function from the detected data and the position and attitude of the sensor expressed in a reference coordinate system, and a robot control for driving an end effector by inputting the path function And an integrated control system employing a path following control device including the robot driven by the robot control means.

An execution procedure of this embodiment will be described with reference to the drawings. In the present embodiment, the path following control device is operated, and as shown in FIG. 1, a path function determining means (not shown) sequentially generates a path function while collecting path information on a work target, as shown in FIG. The position / posture of the end effector is controlled based on the function to follow the target path 1.

At this time, a path function Pi-1 (S) representing the path 6 immediately before the (i-1) -th section is generated, and thereafter, when the latest sensor data Dn is obtained, the latest path function Pi-1 (S) is obtained. A path function Pi (S) representing the path 5 is generated. Based on the path function Pi (S), the end effector further moves on the target path 1 by changing the position / posture according to a command from the robot control means. Perform sensing.

Since the central axis 3 of the sensor visual field range 4 can take a position on the circumference centered on the central axis 2 of the end effector, the following behavior direction is expressed by an equation, and the drawing line is represented by the sensor visual field range. 4 is a central axis 3 following behavior direction line L3. The estimated value of the route 5 is calculated from the route information by using the route function Pi.
(S) is obtained. The central axis 3 following behavior direction formula of the sensor visual field range 4 and the path function Pi (S) formula are simultaneously solved to solve the intersection, and the sensor is controlled by selecting a solution on the front side of the end effector from the two solutions. The position of the prediction to be performed is obtained.

Accordingly, the angle of the sensor around the central axis 2 of the end effector is calculated from the position data, and the robot control means gives a command to the end effector to perform position / posture path tracking control. This calculation is easily performed if the sensor outputs path information in a coordinate system 7 in which the central axis 3 following behavior direction line L3 of the sensor visual field range 4 and the path function Pi (S) are fixed to the end effector. be able to.

Further, when the sensor has a degree of freedom for changing the attitude, the center axis 3 of the sensor visual field range 4 is determined by taking the normal direction of the work surface given from the path function into consideration. The attitude of the sensor can be controlled so as to coincide with the line direction. The degree of overlap between the central axis 3 of the sensor visual field range 4, the sensor visual field range 4 and the target path 1 is accurate, and when compared with the conventional method shown in FIGS. It is obvious at a glance that the position / posture path following control in the example is excellent.

[0017]

As described above, according to the present invention, by estimating the position and orientation of a route at the time of performing a new sensing, which cannot be dealt with by the conventional technique, it is possible to estimate a route based on a more advanced route estimation value. The position and orientation of the sensor can be controlled, and the sensor has excellent utility and usefulness, such as enabling the sensor to reliably grasp the path in its field of view.

[Brief description of the drawings]

FIG. 1 is an explanatory diagram of a sensor position / posture path following control method according to an embodiment of the present invention.

FIG. 2A is an explanatory diagram of a conventional sensor position / posture path tracking control method using a one-point approximation method, and FIG. 2B is an explanatory diagram of the same using a two-point difference approximation method.

[Explanation of symbols]

1: Target path 2: End effector center axis 3: Sensor field range center axis 4: Sensor field range D1 to Dn: Sensor data 5: Path drawn by the latest path function obtained from sensor data D1 to Dn 6 .... Path drawn by the path function of the previous section D1 to Dn-1 7. Coordinate system

──────────────────────────────────────────────────続 き Continuation of the front page (56) References JP-A-2-3805 (JP, A) JP-A-2-7108 (JP, A) JP-A-64-42706 (JP, A) JP-A-59-1984 223817 (JP, A) (58) Field surveyed (Int. Cl. ⁷ , DB name) G05B 19/19 B25J 9/10 B25J 13/08 G05B 19/4155 G05D 3/12

Claims (1)

(57) [Claims] 1. A position of a path output from a sensor which is attached to a hand of a robot together with an end effector and outputs a position and orientation of the end effector target path on a work object in front of the end effector. And the orientation data, and from the position and orientation of the sensor represented in the reference coordinate system, determine and output a path function representing the position and orientation of the end effector target path in the reference coordinates, and based on the path function, In a position and orientation path following control method for controlling the position and orientation of the end effector of the robot, the method further comprises: determining a current position on a target path described by a path function of the end effector, and a distance between the end effector and the sensor. Calculating the equation of the sensor field-of-view range center axis following behavior, and then on the path that the sensor should take The position and orientation are obtained by establishing a simultaneous equation based on the equation and the path function, calculating and obtaining a solution thereof, and the sensor surely captures the end effector target path in the field of view. Sensor position and orientation path following control method.