JPH0762613B2 - Object shape detection method - Google Patents

Description

TECHNICAL FIELD The present invention relates to an object shape detection method, and in particular, a manipulator operator can easily detect a work target object when the work target site is a seabed or an opaque environment in manipulator work. It is useful when it is not close to or it is difficult to see the work target object.

<Prior Art> In manipulator work, a method in which an operator directly teaches the position and shape of a target object is generally used. However, when the manipulator is used outdoors, it is often difficult for a human to directly go to the work target site, and the operator may not be able to directly teach the manipulator.

In such a case, there is no choice but to adopt a method of causing the manipulator to recognize the target object.

There are the following methods (A) and (B).

(A) Method using optical measurement: A visual sensor such as an ITV camera is attached to the manipulator,
The shape and position of the target object is recognized by processing the image from the visual sensor.

(B) Method using contact sensor: A contact sensor is attached to the manipulator as a hand effector, the contact sensor is brought into contact with the target object, and the shape and position of the target object are recognized from the position of the contact sensor.

<Problems to be Solved by the Invention> However, in the method using the optical measurement described in (A) above, the system such as a required visual sensor such as an ITV camera and an image processing device is extremely large and expensive. There was a drawback that In addition, there is a drawback that a strong light source is required in a place where the work site is a place where the lighting is insufficient such as the sea floor. Further, in an environment in which an opaque substance such as dust or soot is suspended, the visual sensor cannot be used and optical measurement using a laser beam or the like is impossible.

On the other hand, in the method using the contact sensor of the above (B), since the measurement range is limited to the hand part of the manipulator, there is a drawback that the manipulator must be controlled so that the hand may trace the surface of the target object. . Such control is not easy because it requires precise positioning of the manipulator. Further, in order to accurately detect the position and shape of the target object, it is necessary to arrange a plurality of minute contact sensors at the hand of the manipulator.

In view of the above-mentioned conventional technology, the present invention can detect the shape of a target object of manipulator work even in a place where optical measurement is impossible, and also requires highly accurate positioning of the manipulator such as when using a contact sensor. It is an object of the present invention to provide a method capable of detecting the shape of a target object without the need.

<Means for Solving Problems> An object shape detection method according to the present invention which achieves the above object,
A position controllable manipulator, a force sensor for measuring a force vector and a moment vector at the tip of the manipulator, a force transferable contactor connected to the force sensor, and a position control computer for the manipulator are used. , The manipulator is operated to scan the surface of the target object by the contactor connected to the force sensor, the force vector and moment vector acting on the contactor during scanning are measured by the force sensor, and the inner product of the measured force vector・
Is calculated as the norm of the force vector ‖‖, and the scalar constant C determined by the geometrical shape of the contactor is calculated.
With and This is a method of calculating the position vector from the force sensor given by the equation to the point of action of the force of the contact, and obtaining the position of the target object surface from this position vector and the hand position of the manipulator.

<Action> The vector of force and the vector of moment are each represented by three components in the three-dimensional space, and each component is generally represented as follows using the Cartesian coordinate system xyz as shown in FIG. It

Force in x-axis: Fx Force in y-axis: Fy Force in z-axis: Fz Moment about x-axis: Mx Moment about y-axis: My Moment about z-axis: Mz Also, for each of the three components, ＝ (Fx, Fy, Fz) ^T = (Mx, My, Mz) ^T, and these are the force vector and the moment vector.

If the position vector from the coordinate origin O to the point P of force is between the force vector and the Maemont vector, the moment vector is the outer product of the position vector and the force vector, as follows: There is a relationship.

A force sensor, also called a force sensor, is generally a structure with an appropriate number of strain gauges attached.When a force and moment are applied to this, the strain of the structure is measured and The vector of the force acting on the force sensor and the vector of the moment are calculated by back calculation.

When an arbitrary position of the contactor comes into contact with the surface of the target object, the reaction force is applied to the contactor. This force is transmitted to the force sensor, and the force vector and the moment vector are measured.
When these vectors are known, the geometrical shape of the contact is known, so the position vector from the force sensor to the point of action of the force of the contact, that is, the contact position with the contact on the surface of the target object is calculated. Determined by. When this position vector is known, the force sensor is present at the tip of the manipulator, so the position of the target object surface can be known from the position vector and the manipulator hand position. Therefore, the shape of the target object can be known by operating the manipulator and scanning the surface of the target object with the contactor.

<Example> An example of the present invention will be described below with reference to the drawings.

FIG. 1 is a schematic view showing a first embodiment of an apparatus for realizing the present invention, 1 is a manipulator with 6 degrees of freedom, and a force sensor 2 is provided at its tip. This force sensor 2
A contact 3 having a linear shape for transmitting a force to the force sensor 2 is connected to the. Reference numeral 4 denotes a computer that controls the position of the manipulator 1 and performs required calculations for shape detection. Reference numeral 5 is a target object for shape detection. To detect the shape, the manipulator 1 is operated to bring the contactor 3 into contact with the target object 5.

When the reaction force (contact force) is applied to an arbitrary position of the contactor 3 by contacting the target object 5, this force is transmitted to the force sensor 2 via the contactor 3. The force sensor 2 measures three orthogonal force components on the three-dimensional coordinate and three moment components around each orthogonal axis of the three-dimensional coordinate. The position vector from the force sensor 2 to the point of application of the contact force can be estimated based on the force information of these six pieces and the shape characteristics of the contactor 3. On the other hand, the position of the force sensor 2 itself can be calculated from the arm length of the manipulator 1 and the joint rotation angle.

From the above, the position of the target object 5 with respect to the manipulator 1 is given by the following equation (1).

Where Ti (i = 1,2 ... 6) is the transformation matrix that represents rotation, θi (i = 1,2 ... 6) is the joint rotation angle, and ▲ ▼ (i = 0,1 ... 6) is the manipulator arm. Is a vector that represents. Target object 5 by contactor 3
The position obtained each time by scanning the surface of ▲ ▼
The shape can be seen from.

FIG. 2 is a diagram illustrating a method of estimating the point of action P of the contact force from the force information. When the force acts on the action point P represented by the position vector formed by the three-dimensional coordinates (x, y, z) specific to the force sensor 2, the force measurement value (F
x, Fy, Fz) ^T and measured moment value (Mx, My, Mz) ^T satisfy the following equations (2) and (3), respectively.

= ... Formula (2) = × ... Formula (3) where Fi (i = x, y, z) is the force in each axial direction, Mi (i = x,
y, z) is the moment about each axis, and × is the cross product of the vectors.

Then, solving equations (2) and (3), the action point P
The position vector of is calculated by the following equation (4).

However, ‖ and ‖ are norms defined as the inner product of the vectors, and C is the shape of the contactor 3, that is, a scalar constant determined from the geometric conditions. The value of the undetermined constant C is given by the following equation (5) when the contactor 3 exists in a plane (a ₁ x + a ₂ y + a ₃ z + a ₄ = 0), for example.

When friction does not occur between the contactor 3 and the target object 5, the normal line of the surface of the target object 5 is given by.

When performing shape detection, the computer 4 operates the manipulator 1 and uses its degree of freedom to scan the surface of the target object 5 with the contactor 3 extending from the force sensor 2. In addition to this position control, the computer 4 (a) means for obtaining the norm of the force vector ‖‖, and (b) means for obtaining the scalar constant C determined by the geometrical shape of the contactor 3 using the above equation (5) or the like. And (c) means for obtaining the position vector from the force sensor 2 to the surface of the target object 5 given by the previous equation (4), and (d) calculation of the position vector and the hand position of the manipulator 1 from the previous equation (1 ) Has a means for obtaining the position ▲ ▼ of the surface of the target object 5.

When the surface of the target object 5 is scanned by the contactor 3, the computer 4 uses the force information detected by the force sensor 2 to determine the above-mentioned (a).
The position of the surface of the target object 5 is obtained by means of (d). That is, the shape is detected. In this case, since the position of the object can be detected regardless of which part of the contactor 3 contacts the target object 5, the shape of the target object 5 can be accurately detected even if the positioning accuracy of the manipulator 1 is not high. .

Here, the experimental results of the method for estimating the position from the force sensor 2 to the force application point described in FIG. 2 are shown in FIGS.
Shown in FIG. In the experiments shown in these figures, z
External force is applied to the action point on the axis. As the contactor 3, an aluminum round bar having a length of 110 mm was used. The force sensor 2 used has an accuracy of about ± 1N.

Figure 3 shows the estimated value from the position z of the true point of action and the force information when the external force is constant at = (0,10N, 0) ^T.
Show the relationship with lz. From this experiment, it was confirmed that the position of the action point can be estimated with an accuracy of 0.9% of the average error.

FIG. 4 shows the estimated value lz from the force information when the position of the action point is fixed as = (0,0,100 mm) ^T and the direction arg () of the force is changed. From this experiment, it was confirmed that the position of the action point can be estimated with an accuracy of 4.2% standard deviation regardless of the direction of force arg ().

FIG. 5 shows the estimated value lz from the force information when the position of the action point is fixed as = (0,0,100 mm) ^T and this time the magnitude of the force is changed. From this experiment, it was confirmed that the position of the action point can be estimated with an accuracy of 3.3% or less standard deviation regardless of the magnitude of force.

Next, an experiment using the telephone handset 6 as a model for shape detection will be described with reference to FIGS. 6 and 7. Using the object shape detection device shown in FIG. 1, a portion 7 indicated by a broken line in the handset 6 of FIG. 6 was traced and its contour shape was detected. The detection result 8 is plotted with square marks in FIG.

FIG. 8 shows a second embodiment of the invention, in which a force sensor 2 is connected to a contact 9 having a hemispherical surface. The manipulator 1, the force sensor 2, the calculator 4, and the target object 5 are the same as those having the same reference numerals in FIG. However,
The constant C depending on the shape of the contactor 9 calculated by the computer 4 defines the vector inner product as the norm of the vector instead of the equation (5). Is a constant that satisfies Note that r is the radius of the hemisphere of the contactor 9. Where (×) / ‖‖ ＝ (Lx, Ly, Lz) ^T , ＝ (Fx, Fy, F
z) ^{Putting T} , the constant C is Given in. However, the _{^{b 1 = F 2 x + F}} 2 y + F 2 z b 2 = LxFx + LyFy + LzLz b 3 = L 2 x + L 2 y + L 2 z + r 2.

Also in the case of the embodiment shown in FIG. 8, the computer 4 is operated by the manipulator 1.
The surface of the target object 5 is scanned by the contactor 9 connected to the force sensor 2 by position control, and the position of the surface of the target object 5 is obtained from the force information detected by the force sensor 2. That is, the computer 4 obtains the vector of force and the vector of moment detected by the force sensor 2, and further the norm of vector = ‖
‖ Is calculated, and then the contactor 9 is calculated by the formula (6) or (7).
The scalar constant C determined by the geometrical shape of is calculated, the position vector of the action point given by the previous equation (4) is obtained, and the object given by the previous equation (1) is calculated from this position vector and the hand position of the manipulator 1. Position of surface of object 5
Ask for ▼.

In the embodiment shown in FIG. 8, since the contactor 9 has a surface contact portion, there is an advantage that the effective detection range is wide.

Here, the derivation process of the above-mentioned formula (1), formula (4), formula (5), formula (6) and formula (7) will be described collectively.

<Derivation of Equation (1)> First, derivation of Equation (1) will be described with reference to FIG. Equation (1) is a kinematic equation of the manipulator. 9 shows the tip portion of the manipulator 1 in FIG. 1, and the symbol J _{i in} FIG. 9 represents the coordinate system specific to the i-th joint.

Therefore, if the position of the target object 5 viewed from the coordinate system J ₆ peculiar to the sixth joint is ▲ ▼, Becomes Next, the position ▲ ▼ of the target object 5 viewed from the coordinate system J ₅ peculiar to the fifth joint is Is represented. Here, T ₆ (θ ₆ ) represents the rotation of the sixth joint, and is a matrix that performs coordinate conversion from the sixth joint coordinate system J ₆ to the fifth joint coordinate system J ₅ . Next, the position ▲ ▼ of the target object 5 viewed from the coordinate system J ₄ peculiar to the fourth joint is Is represented. T ₅ (θ ₅ ) represents the rotation of the fifth joint,
This is a matrix for performing coordinate conversion from the fifth joint coordinate system J ₅ to the fourth joint coordinate system J ₄ .

Hereinafter, the same operation is performed to sequentially convert the third, second, and first joint coordinate systems, and finally the target object 5 in the reference coordinate system J ₀ is converted.
The position ▲ ▼ of is obtained as in the previous equation (1).

<Deriving Expression (4)> From the above Expressions (2) and (3), the following Expression (8) is established.

= × Equation (8) When this equation (8) is regarded as a vector equation for l and solved, the previous equation (4) is obtained. The proof is given below.

First, from equation (8), due to the nature of the vector cross product, and are orthogonal. Therefore, let K be a scalar constant, and let = K (×) ... Equation (9) and substitute into Equation (8): = K (×) × ∴ = K [-(・) + (・)]… We obtain equation (10). Here, (·) represents the inner product of the vectors. From the equation (8), because of the property of the outer product, and are orthogonal,
・ = 0. Therefore, equation (10) becomes = K (•) ... equation (11), and if a vector that is not perpendicular to is taken, the scalar constant K is determined as K = 1 / (•) ... equation (12), and The special solution is given by the following equation (13).

▲ ▼ = (・) / (・)… Equation (13) Therefore, the condition of the vector (not perpendicular to the above)
If is selected to satisfy, the special solution ▲ ▼ is given by the following equation (14).

▲ ▼ = (×) / (・) ＝ (×) / ‖‖… Equation (14) Next, for an arbitrary solution (general solution), as in Equation (8), = ×, and a special solution ▲ ▼ About =
Since it is ▲ ▼ ×, (-▲ ▼) × = 0 ... Expression (15) is established. According to the equation (15), (-▲ ▼) is parallel to, so that − ▲ ▼ = C. However, C is an arbitrary scalar constant.

As a result, from the equations (14) and (16), the following equation (4) is established.

= (×) / ‖‖ + C ... Formula (4) <Derivation of Formula (5)> The vector in Formula (4) is expressed as follows.

(×) / ‖‖ = (Lx, Ly, Lz) ^T Equation (17) = (Fx, Fy, Fz) ^T Equation (18) As a result, the equation (4) becomes the following equation (19).

= (Lx, Ly, Lz) ^T + C (Fx, Fy, Fz) ^T = (Lx + CFx, Ly + CFy, Lz + CFz) Equation (19) Equation (19) of the plane in which the contactor 3 described above exists (a ₁₎ Since x + a ₂ y + a ₃ z + a ₄ = 0 is satisfied, the following equation (20)
Is established.

a ₁ (Lx + CFx) + a ₂ (Ly + CFy) + a ₃ (Lz + CFz) + a ₄ = 0… Equation (2
0) By solving this equation (20) for C, the following equation (5) is obtained.

C =-(a ₁ Lx + a ₂ Ly + a ₃ Lz + a ₄ ) / (a ₁ FX + a ₂ Fy + a ₂ Fz) ... Formula (5) <Derivation of Formula (6)> When the position vector of the contact point is on the spherical surface of radius r, the following expression (21) is established by setting the inner product of the vector as the norm of the vector.

‖‖ = r ² Equation (21) Then, by substituting the equation (21) into the equation (4), the following is obtained and the previous equation (6) is obtained.

<Derivation of Expression (7)> When the left side of Expression (6) is expanded using Expressions (17) and (18), the following Expression (22) is obtained. However, b ₁ , b ₂ and b ₃ have already been defined in relation to the equation (7).

‖ ^{(Lx, Ly, Lz) T} + C (Fx, Fy, Fz) T ‖ = ‖ (Lx, CFx, Ly + CFy , Lz + CFz) T ‖ ^{= ((Lx + CFx) 2} + (Ly + CFy) 2 + (Lz + CFz) 2) = (B ₁ C ² + 2b ₂ ² C + b ₃ + r ² ) Equation (22) Therefore, Equation (6) becomes Equation (23) below.

b ₁ ² C ² ＋ 2b ₂ ² C ＋ b ₃ ＋ r ² ＝ r ² ∴b ₁ ² C ² ＋ 2b ₂ ² C ＋ b ₃ = 0 Equation (23) When this equation (23) is solved for C, the following equation ( 7) is obtained.

<Effects of the Invention> According to the present invention, the shape of an object is detected by using a force sensor and a contact, so that the shape of the object can be detected even in an environment where optical measurement cannot be performed, such as undersea work.

Further, according to the present invention, no matter which part of the contactor comes into contact with the object, that is, as long as the object exists within the measurement range of the contactor, the contact force is transmitted to the force sensor and the position of the object can be detected. Therefore, the object shape can be detected without controlling the position of the manipulator with high accuracy as in the case where the conventional contact sensor is used as the hand effector.

Further, when the present invention is applied to the manipulator work, it is possible to feed back the shape information of the target object in real time to perform highly accurate control.

[Brief description of drawings]

1 to 8 relate to the present invention, and FIG. 1 corresponds to the first of the present invention.
FIG. 2 is a schematic diagram of an object shape detection device that is an embodiment of FIG. 2, and FIG. 2 is an explanatory diagram of a method of estimating the action point P of the contact from force information.
FIG. 3 is a diagram showing the relationship between the position of the point of action of force and the estimated value of that position, FIG. 4 is a diagram showing the relationship between the direction of force and the estimated value of the point of action of force, and FIG. Showing the relationship between the magnitude of the force and the estimated value of the point of action of force, FIG. 6 is a diagram showing an example of the shape detection target object, and FIG. 7 is the shape of the object of FIG. 6 using the apparatus of FIG. FIG. 8 is a diagram showing the result of detection of the above, FIG. 8 is a schematic diagram showing the device of the second embodiment, and FIG. 9 is an explanatory diagram for deriving the formula (1). In the drawings, 1 is a manipulator, 2 is a force sensor, 3 is a linear contactor, 4 is a calculator, 5 is a target object, 6 is a telephone handset of a shape detection target example, 7 is a trace trajectory, and 8 is a shape detection result. , 9 are spherical contacts.

─────────────────────────────────────────────────── ─── Continuation of the front page (51) Int.Cl. ⁶ Identification code Internal reference number FI technical display location G05B 19/4063

Claims (1)

[Claims] 1. A manipulator capable of position control, a force sensor provided at a tip of the manipulator for measuring a force vector and a moment vector, a contact capable of transmitting force connected to the force sensor, and a manipulator. Using the computer for position control described above, the manipulator is operated to scan the surface of the target object with the contactor connected to the force sensor, and the force vector and moment vector acting on the contactor during scanning are measured by the force sensor. , Inner product of the measured force vector
Is calculated as the norm of the force vector ‖‖, and the scalar constant C determined by the geometrical shape of the contactor is calculated.
With and An object shape detection method characterized by calculating a position vector from a force sensor given by the following equation to a point of action of a force of a contact, and obtaining a position of a target object surface from the position vector and a hand position of a manipulator.