JP6558727B2 - 3D shape estimation device and 3D shape estimation method of flexible elastic body - Google Patents

Description

The present invention relates to a three-dimensional shape estimation device and a flexible elastic body three-dimensional shape estimation method.

For the development of advanced robot technology, the evolution of sensor technology for detecting the posture and force of movable bodies is indispensable.
Non-Patent Documents 1 and 2 disclose sensors applied to such robots.

“Shape Tape” Solid Ray Laboratory Co., Ltd. [Search April 23, 2015], Internet <URL: http://www.solidray.co.jp/product/3dglove/shapetape.html> “Master Sensor Robot Flexible Command Tube” Asahi Electric Co., Ltd. [Search April 23, 2015], Internet <URL: http://www.kyokko.co.jp/technology/fst-03.html>

  The sensor devices disclosed in Non-Patent Document 1 and Non-Patent Document 2 described above incorporate a large number of sensor modules and detect the posture of the movable body. If the number of such sensors can be reduced, it will lead to the realization of a lower cost sensor.

The present invention has been made in view of the above points, and a three-dimensional shape estimation device and a flexible device capable of quickly estimating the shape of a flexible elastic body attached to a six-axis sensor with one six-axis sensor. An object of the present invention is to provide a method for estimating the three-dimensional shape of an elastic body.

In order to solve the above problems, a three-dimensional shape estimation device of the present invention includes a six-axis sensor that detects a force and torque in a three-dimensional direction, and an elastic body that is attached to the six-axis sensor and deforms due to an external force applied to the tip. And a coordinate calculation unit for estimating the three-dimensional shape of the elastic body.
The coordinate calculation unit regards the elastic body as a discretized model composed of an assembly of a pseudo rigid body having a predetermined length and a joint having a spring constant, and information on the spring constant and force obtained from the six-axis sensor. Based on the torque information, the bending shape in which the elastic body makes a static balance with respect to the external force is estimated and calculated.

The present invention provides a three-dimensional shape estimation device and a flexible elastic body three-dimensional shape estimation method that can quickly estimate the shape of a flexible elastic body attached to a six-axis sensor with a single six-axis sensor. can do.
Problems, configurations, and effects other than those described above will be clarified by the following description of embodiments.

It is the schematic which shows the whole structure of the elastic body shape estimation system which is an example of embodiment of this invention. It is a block diagram which shows the hardware constitutions of information processing apparatus. It is a block diagram which shows the software function of information processing apparatus. It is a conceptual diagram of the arithmetic processing in a coordinate calculating part. It is a flowchart explaining the arithmetic processing in an information processing apparatus.

[overall structure]
FIG. 1 is a schematic diagram showing an overall configuration of an elastic body shape estimation system 101 as an example of an embodiment of the present invention.
The elastic body shape estimation system 101 includes a six-axis sensor 102 and an information processing apparatus 105 such as a personal computer.
The six-axis sensor 102 is attached with one end of a belt-like elastic body 103 that is an object to be estimated by the elastic body shape estimation system 101.
The belt-like elastic body 103 may be, for example, a well-known synthetic resin such as polyethylene terephthalate (PET resin) or polyimide, or a metal such as a steel plate, but is an elastic body that is formed of a flexible material. Are formed with a uniform thickness and width. Therefore, the belt-like elastic body 103 has substantially the same spring constant over its entire length.

The 6-axis sensor 102 is a sensor that outputs a force in each of the x-axis, y-axis, and z-axis directions and a torque centered on each of the x-axis, y-axis, and z-axis. An analog signal output from the 6-axis sensor 102 is connected to the information processing apparatus 105 through the interface unit 104 including the A / D converter 207. The interface that connects between the interface unit 104 and the information processing apparatus 105 is not particularly limited. Various types of interfaces such as a form mounted on a PCI bus provided in the information processing apparatus 105, which is a general personal computer, and USB and LAN cables can be used.
The information processing apparatus 105 receives various initial values input in advance and signals from the 6-axis sensor 102 and performs arithmetic processing to display an image imitating the shape of the elastic band 103 attached to the 6-axis sensor 102. Displayed on the unit 106.
Note that the elastic body shape estimation system 101 of the present embodiment uses the belt-like elastic body 103 in which the spring constant can be easily set, but the elastic body does not necessarily have a belt-like shape.

FIG. 2 is a block diagram illustrating a hardware configuration of the information processing apparatus 105.
In an information processing apparatus 105 composed of a general personal computer, a CPU 201, a ROM 202, a RAM 203, a nonvolatile storage 204, a display unit 106, and an operation unit 205 are connected to a bus 206. In addition, an A / D converter 207 built in the interface unit 104 that converts an analog signal output from the 6-axis sensor 102 into digital data is also connected to the bus 206.

FIG. 3 is a block diagram illustrating software functions of the information processing apparatus 105.
Data of the six-axis sensor 102 input through the A / D converter 207 is input to the coordinate calculation unit 302 that is a function of the input / output control unit 301.
The coordinate calculation unit 302 performs calculation processing to be described later together with the initial value input from the operation unit 205, and estimates the shape of the belt-like elastic body 103 attached to the six-axis sensor 102. The estimation result is output as coordinate information of a plurality of points in the three-dimensional space. The coordinate information of a plurality of points output by the coordinate calculation unit 302 is input to an image drawing processing unit 303 that is a function of the input / output control unit 301.
The image drawing processing unit 303 creates a simulation image of the belt-like elastic body 103 in the three-dimensional space based on the coordinate information of a plurality of points. The simulation image created by the image drawing processing unit 303 is displayed on the display unit 106.

[principle]
FIG. 4 is a conceptual diagram of calculation processing in the coordinate calculation unit 302.
The elastic body shape estimation system 101 according to the present embodiment basically uses an elastic body having a uniform spring constant over the entire length as a target of estimation calculation. Although this elastic body is a continuum, the calculation by a computer becomes possible by discretizing the continuum. In other words, the elastic body has a predetermined spring constant as a discretized model of the pseudo rigid body 401 of a certain length, the joint 402 that connects the pseudo rigid bodies 401, and the pseudo rigid bodies 401 that are connected in three dimensions. It can be regarded as an assembly of spring joints 404 formed of springs 403. If each pseudo rigid body 401 is a vector and is considered to have a configuration in which the rotations of the vectors are connected, the shape of the belt-like elastic body 103 can be estimated and calculated.

The total length of the belt-like elastic body 103 is L, and the number of divisions in which the belt-like elastic body 103 is pseudo-divided by the spring joint 404 is n. Let the length of each pseudo-rigid body be l = L / n. Each joint 402 generally has three degrees of freedom, and the respective angles are θ _{T, I} , θ _{N, i} , θ _{B, I} ∈C. The index i starts from 1 and goes to n.

  First, as a preparation for the estimation calculation in the present embodiment, the calculation formula of the rotation operator R (a, θ) is described below.

  Next, constants used for the estimation calculation in the present embodiment will be described below.

  Next, set values used for the estimation calculation in the present embodiment are described below.

  However, it is assumed that the reference coordinate system has the same orientation as the 6-axis sensor coordinate system, and the origin also coincides with the origin of the 6-axis sensor 102.

  Next, values obtained from the 6-axis sensor in the estimation calculation in the present embodiment will be described below.

  The estimation calculation in this embodiment is a recurrence calculation. The initial value of i, which is an index for iterative calculation processing (index for recurrence calculation), is 1.

  The recurrence calculation repeats the following equations (14) to (24).

Finally, the shape of the belt-like elastic body 103 can be estimated by obtaining the position p _{i of the} pseudo joint and the posture F _i of the pseudo rigid body 401 obtained by Expression (24). That is, by drawing a _{frame p 0, p 1, p 2} ... plot of _{p n,} corresponding to _{F 0, F 1, F 2} ... F n in three-dimensional space by computer graphics, the strip elastic member 103 Can be drawn.

[Operation]
FIG. 5 is a flowchart for explaining arithmetic processing in the information processing apparatus 105.
When the process is started (S501), the input / output control unit 301 performs a coordinate operation on the spring constants p ₀ and F ₀ that are input from the operation unit 205, which are initial values necessary for the calculation, and the pseudo joint length l. Set in the unit 302 (S502). Next, the coordinate calculation unit 302 acquires f _s and m _s from the A / D converter 207 (S503). As described above, steps S502 and S503 are initial value setting steps. Then, a counter variable i, which can be an index of recurrence calculation, is initialized to 1 (S504). Step S504 can be said to be an index initialization step.

In addition, it can be said that Formula (14) performed in step S505 is a local torque calculation step which calculates the torque added to a joint.
Similarly, equation (15) executed in step S506 can be said to be a first axis direction calculating step for calculating the direction of the first axis of the joint.
Similarly, equation (16) executed in step S507 can be said to be a first axis angle calculation step for calculating the angle of the first axis of the joint.
Similarly, equation (17) executed in step S508 can be said to be a first subframe posture calculation step that is affected by the rotation of the first axis of the joint.
Similarly, equation (18) executed in step S509 can be said to be a second axis direction calculating step for calculating the direction of the second axis of the joint.
Similarly, equation (19) executed in step S510 can be said to be a second axis angle calculating step for calculating the angle of the second axis of the joint.
Similarly, equation (20) executed in step S511 can be said to be a second subframe posture calculation step that is affected by the rotation of the second axis of the joint.
Similarly, equation (21) executed in step S512 can be said to be a third axis direction calculating step for calculating the direction of the third axis of the joint.
Similarly, equation (22) executed in step S513 can be said to be a third axis angle calculating step for calculating the angle of the third axis of the joint.
Similarly, equation (23) executed in step S514 can be said to be a pseudo rigid body posture calculating step for calculating a hand side pseudo rigid body posture that is affected by the rotation of the third axis of the joint.
Similarly, equation (24) executed in step S515 can be said to be a joint position calculation step of calculating the next hand side joint position of the joint from the length and posture of the hand side pseudo rigid body.

Next, the input / output control unit 301 increments the counter variable i by 1 (S516), and checks whether i exceeds the division number n (S517). If i does not exceed the division number n (NO in S517), the input / output control unit 301 repeats the arithmetic processing from step S505 again.
In step S517, if i exceeds the division number n (YES in S517), all the pi necessary for drawing the belt-like elastic body 103 have been prepared, so the image drawing processing unit 303 determines based on l and pi. Then, the elastic band 103 is displayed on the display unit 106 (S518).
Next, the input / output control unit 301 confirms whether or not the end of the process is instructed from the operation unit 205 or the like (S519). If the end of the process is not instructed (NO in S519), the input / output control unit 301 repeats the process from step S503 again. That is, f _s and m _s are acquired again from the A / D converter 207, and the calculation process (S505 to S515) and the drawing process (S518) are repeated.
If the end of the process is instructed in step S519 (YES in S519), the input / output control unit 301 ends the series of processes (S511).

Note that the process of step S518 may be performed after step S515 and before step S517. In that case, an image of the pseudo rigid body 401 extending from the 6-axis sensor 102 one by one is displayed on the display unit 106.
As described above, from the information on the force and torque that can be acquired by the 6-axis sensor 102 with the discrete spring constant and the installation position and orientation of the elastic body installed on the 6-axis sensor 102 as known values, the equations (14) to (24 ) by calculating a can output the position p _n and orientation F _n of the shape and the elastic body leading end of the elastic body.

This embodiment can be applied as follows.
(1) In the above-described embodiment, the calculation is performed on the assumption that the discrete spring constant of the elastic body is uniform over the entire length. However, even when the elastic body has a non-uniform spring constant, If the spring constant can be derived, calculation processing by the coordinate calculation unit 302 is possible. For example, an elastic body whose base is formed in a taper shape and the like in which the change in the spring constant can be expressed by a function of the elastic body length can be processed in principle.

  (2) Although the three-dimensional shape estimation system shown in FIGS. 1 to 3 is a system for estimating the shape of an existing elastic body, it is naturally possible to estimate the shape of an elastic body that does not exist. . That is, it is possible to simulate the shape of the elastic body and to estimate the force applied to the elastic body.

In this embodiment, the elastic body shape estimation system 101 was disclosed.
When a force is applied to the tip of the elastic body installed on the 6-axis sensor 102, the discrete spring constant and the installation position and orientation of the elastic body installed on the 6-axis sensor 102 are set to known values. By calculating the equations (14) to (24) from the information on the force and torque applied to the installation position of the elastic body that can be obtained, the shape of the elastic body and the position and posture of the elastic body tip can be output.
Since the calculation process is completed in a very short time, every time the force applied to the elastic body changes, the image that is the result of the estimation calculation changes in real time.

The embodiments of the present invention have been described above. However, the present invention is not limited to the above-described embodiments, and other modifications and application examples are provided without departing from the gist of the present invention described in the claims. including.
For example, the above-described embodiment is a detailed and specific description of the configuration of the apparatus and the system in order to explain the present invention in an easy-to-understand manner, and is not necessarily limited to one having all the configurations described. Further, a part of the configuration of one embodiment can be replaced with the configuration of another embodiment, and the configuration of another embodiment can be added to the configuration of one embodiment. Moreover, it is also possible to add, delete, and replace other configurations for a part of the configuration of each embodiment.
Each of the above-described configurations, functions, processing units, and the like may be realized by hardware by designing a part or all of them with, for example, an integrated circuit. Further, each of the above-described configurations, functions, and the like may be realized by software for interpreting and executing a program that realizes each function by the processor. Information such as programs, tables, and files that realize each function must be held in a volatile or non-volatile storage such as a memory, hard disk, or SSD (Solid State Drive), or a recording medium such as an IC card or an optical disk. Can do.
In addition, the control lines and information lines are those that are considered necessary for the explanation, and not all the control lines and information lines on the product are necessarily shown. Actually, it may be considered that almost all the components are connected to each other.

  DESCRIPTION OF SYMBOLS 101 ... Elastic body shape estimation system, 102 ... 6 axis | shaft sensor, 103 ... Strip | belt-shaped elastic body, 104 ... Interface unit, 105 ... Information processing apparatus, 106 ... Display part, 201 ... CPU, 202 ... ROM, 203 ... RAM, 204 ... Non-volatile storage, 205 ... operation unit, 206 ... bus, 207 ... A / D converter, 301 ... input / output control unit, 302 ... coordinate calculation unit, 303 ... image drawing processing unit, 401 ... pseudo rigid body, 402 ... joint, 403 ... Spring, 404 ... Spring joint

Claims (5)

A 6-axis sensor that detects force and torque in a three-dimensional direction;
An elastic body attached to the six-axis sensor and deformed by an external force applied to the tip;
A coordinate calculation unit for estimating a three-dimensional shape of the elastic body;
Comprising
The coordinate calculation unit regards the elastic body as a discretized model composed of an assembly of a pseudo rigid body having a predetermined length and a joint having a spring constant, and is obtained from the spring constant and the six-axis sensor. Based on the information on the force and the information on the torque, the elastic body estimates and calculates a bent shape that makes a static balance with the external force.
3D shape estimation device. The coordinate calculation unit is composed of a collection of joints having a pseudo-rigid body having a predetermined length and a spring constant.
τ _i = M _s -P _i-1 × f _s
The three-dimensional shape estimation apparatus according to claim 1, wherein the elastic body is estimated as a discretized model that satisfies the above-described condition, and a bending shape in which the elastic body is statically balanced with respect to the external force is estimated and calculated.
Where i is a natural number and τ _i Is a local torque applied to the i-th joint which is the i-th joint, m _s Is a torque vector read from the 6-axis sensor, and p _i-1 Is the position vector of the i-th joint, f _s Is a force vector read from the 6-axis sensor. Furthermore,
An image drawing processing unit for drawing an estimated shape of the elastic body based on a calculation result of the coordinate calculation unit;
The three-dimensional shape estimation apparatus according to claim 2, comprising: The coordinate calculation unit is
As an initial value of the arithmetic processing, set the position and orientation in the three-dimensional direction where the elastic body is installed on the 6-axis sensor, the spring constant, and the length of the pseudo rigid body,
Obtaining information on the force and torque obtained from the six-axis sensor;
Set the recurrence calculation index to the initial value, calculate the torque applied to the joint,
Calculating the orientation of the first axis of the joint, the angle of the first axis of the joint, and the first subframe posture affected by the rotation of the first axis of the joint;
Calculating a second sub-frame posture affected by the orientation of the second axis of the joint, the angle of the second axis of the joint, and the rotation of the second axis of the joint;
Calculating the orientation of the third axis of the joint, the angle of the third axis of the joint, and the pseudo-rigid posture on the hand side affected by the rotation of the third axis of the joint;
Calculate the next hand side joint position of the joint from the length of the pseudo-rigid body on the hand side and the posture;
The three-dimensional shape estimation apparatus according to claim 3. An elastic body attached to a six-axis sensor that detects a force and torque in a three-dimensional direction is regarded as a discretized model composed of a pseudo rigid body having a predetermined length and an assembly of joints having a spring constant, and the spring constant And a three-dimensional shape estimation method for a flexible elastic body that estimates and calculates a bent shape of the elastic body based on the information on the force and the information on the torque obtained from the six-axis sensor ,
The three-dimensional shape estimation method includes:
As an initial value of the arithmetic processing, an initial value setting step for setting a position / attitude in the three-dimensional direction where the elastic body is installed on the six-axis sensor, the spring constant, and the length of the pseudo rigid body;
A sensing step for obtaining the force information and the torque information obtained from the six-axis sensor;
An index initialization step for setting the recursion calculation index to an initial value;
A local torque calculating step for calculating a torque applied to the joint;
A first axis direction calculating step of calculating a direction of the first axis of the joint;
A first axis angle calculating step of calculating an angle of the first axis of the joint;
A first subframe posture calculating step that is affected by the rotation of the first axis of the joint;
A second axis direction calculating step for calculating the direction of the second axis of the joint;
A second axis angle calculating step for calculating an angle of the second axis of the joint;
A second sub-frame posture calculation step affected by the rotation of the second axis of the joint;
A third axis direction calculating step for calculating the direction of the third axis of the joint;
A third axis angle calculating step for calculating an angle of the third axis of the joint;
A pseudo rigid body posture calculating step for calculating a pseudo rigid body posture on the hand side affected by the rotation of the third axis of the joint;
A joint position calculating step of calculating a next hand side joint position of the joint from the length of the pseudo-rigid body on the hand side and the posture;
A method for estimating a three-dimensional shape of a flexible elastic body.

Images