Flexible body shape detection device, flexible body shape detection method, flexible body shape control method, program, and recording medium

Abstract

To provide a linear flexible body shape detection device, a linear flexible body shape detection method, a linear flexible body shape control method, a program, and a recording medium that can detect, with a simple configuration, the shape of a linear flexible body in real time by using a force sensor provided on at least one end of the linear flexible body.SOLUTION: A flexible body shape detection device has: a force sensor that detects a load and torque applied in three-dimensional directions; a flexible body that is connected to the force sensor at one end; and an operation unit that calculates a shape along an extension direction from one end to the other end of the flexible body, on the basis of information on the load and information on the torque applied from one end of the flexible body to the force sensor.SELECTED DRAWING: Figure 1

Description

Technical Field

[0001] The present invention relates to a flexible body shape detection device, a flexible body shape detection method, a flexible body shape control method, a program, and a recording medium.

Background Art

[0002] Conventionally, it has been difficult to grasp in real time the shape of a linear flexible body such as a string, rope, chain, or wire having a small elastic force along the extension direction (longitudinal direction), which draws a catenary curve when one end and the other end are supported and hung down. If the shape of such a flexible body can be grasped by a simple method, for example, at a production site or the like, the shape of a wire or rope for suspending a manufactured product can be grasped, and the suspended object can be moved efficiently and safely, or it can be applied to the shape control of a soft robot composed of strings or ropes.

[0003] Conventionally, as a method for grasping the shape of a linear flexible body, for example, Non-Patent Document 1 discloses a method of using a visual sensor to estimate the force applied to an object from the deformed shape of a linear flexible object and detect the contact state of the object. Further, for example, Non-Patent Document 2 discloses a method of modeling the deformation of a linear flexible object such as rubber or wire using an imaging device such as a camera.

Prior Art Documents

Non-Patent Documents

[0004]

Non-Patent Document 1

Non-Patent Document 2

Summary of the Invention

Problems to be Solved by the Invention

[0005] However, the methods for detecting the shape of a linear flexible body disclosed in Non-Patent Documents 1 and 2 are both based on image information such as a visual sensor or a camera. The detection device is complex and requires the use of image information with a large amount of data, making it difficult to detect the shape of the flexible body quickly in real time. Also, when there is an obstacle between the camera and the linear flexible body, there is a problem that the shape cannot be detected (occlusion).

[0006] This invention was proposed in view of the above problems, and an object thereof is to provide a flexible body shape detection device, a flexible body shape detection method, a flexible body shape control method, a program, and a recording medium that can detect the shape of a flexible body in real time with a simple configuration by a force sensor provided at at least one end of the flexible body.

Means for Solving the Problems

[0007] The inventors of the present invention have found a method for estimating in real time the shape from one end to the other end of a flexible body based only on the information of a force sensor joined to at least one end of the flexible body by a simple calculation method capable of real-time calculation based on the information of the force sensor.

[0008] (1) The flexible body shape detection device according to Aspect 1 of the present invention includes a force sensor that detects loads and torques applied in three-dimensional directions respectively, a flexible body with one end connected to the force sensor, and an arithmetic unit that calculates the shape along the extension direction from one end to the other end of the flexible body based on the information of the load and the information of the torque applied from one end of the flexible body to the force sensor. It is characterized by having these components.

[0009] (2) The flexible body shape detection device according to Aspect 2 of the present invention is, in Aspect 1, characterized in that the force sensor is a 6-axis force sensor that detects the load along the three-dimensional directions of the X-axis, Y-axis, and Z-axis that are perpendicular to each other, and the respective rotational forces around the X-axis, Y-axis, and Z-axis.

[0010] (3) The flexible body shape detection device according to Aspect 3 of the present invention is, in Aspect 1 or 2, characterized in that the force sensor is further connected to the other end of the flexible body.

[0011] (4) The flexible body shape detection device according to Aspect 4 of the present invention is, in any one of Aspects 1 to 3, characterized in that the flexible body is a linear flexible body having a uniform elastic force in the extension direction.

[0012] (5) The flexible body shape detection method according to Aspect 5 of the present invention includes Step 1 of detecting information on the load in the three-dimensional direction applied from the flexible body to the force sensor and information on the torque in the three-dimensional direction applied from the flexible body to the force sensor; Step 2 of calculating a position in the middle between one end and the other end of the flexible body based on the gravity potential minimum condition from the information on the load and the information on the torque obtained in Step 1, and obtaining shape data in the three-dimensional direction from one end to the other end of the flexible body; and Step 3 of calculating the angle in the three-dimensional direction for each divided section obtained by dividing the flexible body into a plurality of sections from the information on the shape data in the three-dimensional direction obtained in Step 2. It is characterized by including at least these steps.

[0013] (6) The method for controlling the shape of a flexible body according to Aspect 6 of the present invention includes at least a step 4 of calculating the center of gravity of the flexible body based on the shape data of the flexible body obtained by the method for detecting the shape of the flexible body according to Aspect 5, and a step 5 of controlling the center of gravity of the flexible body based on the center of gravity of the flexible body obtained in step 4 to control the shape along the extension direction of the flexible body.

[0014] (7) The program according to Aspect 7 of the present invention causes a computer to execute steps including: a step 1 of detecting information on the three-dimensional load applied from the flexible body to the force sensor and information on the three-dimensional torque applied from the flexible body to the force sensor; a step 2 of calculating a position in the middle between one end and the other end of the flexible body based on the gravitational potential minimum condition from the load information and the torque information obtained in step 1, and obtaining shape data in the three-dimensional direction from one end to the other end of the flexible body; and a step 3 of calculating the angle in the three-dimensional direction for each divided section obtained by dividing the flexible body into a plurality of sections from the information on the shape data in the three-dimensional direction obtained in step 2.

[0015] (8) The recording medium according to Aspect 8 of the present invention records a program that causes a computer to execute steps including: a step 1 of detecting information on the three-dimensional load applied from the flexible body to the force sensor and information on the three-dimensional torque applied from the flexible body to the force sensor; a step 2 of calculating a position in the middle between one end and the other end of the flexible body based on the gravitational potential minimum condition from the load information and the torque information obtained in step 1, and obtaining shape data in the three-dimensional direction from one end to the other end of the flexible body; and a step 3 of calculating the angle in the three-dimensional direction for each divided section obtained by dividing the flexible body into a plurality of sections from the information on the shape data in the three-dimensional direction obtained in step 2.

Advantages of the Invention

[0016] According to the present invention, it is possible to provide a flexible body shape detection device, a flexible body shape detection method, a flexible body shape control method, a program, and a recording medium that can detect the shape of a flexible body in real time with a simple configuration by a force sensor provided at at least one end of the flexible body.

Brief Description of the Drawings

[0017]

Figure 1

Figure 2

Figure 3

Figure 4

Figure 5

Figure 6

Figure 7

Modes for Carrying Out the Invention

[0018] Hereinafter, with reference to the drawings, a flexible body shape detection device, a flexible body shape detection method, a flexible body shape control method, a program, and a recording medium according to an embodiment of the present invention will be described. Note that the embodiments shown below are specifically described in order to better understand the gist of the invention, and do not limit the present invention unless otherwise specified. In addition, the drawings used in the following description may show the main parts enlarged for convenience of understanding the features of the present invention, and the dimensional ratios of each component are not necessarily the same as the actual ones.

[0019] (Flexible Body Shape Detection Device) A flexible body shape detection device according to an embodiment of the present invention will be described. FIG. 1 is a schematic diagram showing a flexible body shape detection device according to the present embodiment. A flexible body shape detection device (hereinafter simply referred to as a shape detection device) 10 according to the present embodiment includes at least a force sensor 11, a linear flexible body 12 which is an example of a flexible body and one end 12a of which is connected to a detection unit 11a of the force sensor 11, and a calculation unit 13 that calculates the shape of the linear flexible body 12 based on a load signal and a torque (rotational force) signal output from the force sensor 11.

[0020] In the present embodiment, a 6-axis force sensor is used as the force sensor 11. Such a 6-axis force sensor detects a load along three-dimensional directions (X-axis, Y-axis, and Z-axis perpendicular to each other) applied to the detection unit 11a in an arbitrary space and a torque (rotational force) around the X-axis, Y-axis, and Z-axis, and outputs these detection signals to the calculation unit 13. As such a force sensor 11, a known 6-axis force sensor can be used.

[0021] In addition to the 6-axis force sensor, as the force sensor 11, for example, a 3-axis force sensor that detects only a load along three-dimensional directions (X-axis, Y-axis, and Z-axis perpendicular to each other) in an arbitrary space can also be used, and the type of the force sensor 11 is not limited.

[0022] The linear flexible body (flexible body) 12 is an elongated flexible body extending linearly, and one end 12a is fixed to the detection unit of the force sensor 11. Further, the other end 12b may be connected to an arbitrary member movable in a three-dimensional space. Examples of such a linear flexible body 12 include a string, a rope, a wire formed of, for example, a fiber or a resin, a chain, or a chain formed by connecting a plurality of rigid parts to be bendable with respect to each other.

[0023] When such a linear flexible body 12 supports one end 12a and makes the other end 12b a free end, it has flexibility to hang vertically along the vertical direction due to its own weight, and when one end and the other end are supported and suspended, it may have flexibility to form a hanging shape approximating a catenary curve.

[0024] The arithmetic unit 13 includes an A / D converter (converter) 15 and an information processing device 16 composed of a personal computer. The A / D converter 15 converts the analog signal output from the force sensor 11 into a digital signal and inputs it to the information processing device 16. The A / D converter 15 is not particularly limited, and it can be mounted on the PCI bus provided in the information processing device 16, or various interfaces such as USB and LAN cables can be used.

[0025] The information processing device 16 can use a general personal computer. For example, it may include a CPU, a ROM, a RAM, a display unit (display) 17, an operation unit (keyboard, mouse), a non-volatile storage (hard disk device), etc.

[0026] The information processing device 16 performs arithmetic processing based on various preset values and the output signal of the force sensor 11, estimates the shape of the linear flexible body 12 with one end 12a fixed to the force sensor 11, and displays the shape on, for example, the display unit 17. A specific example of such arithmetic processing (the arithmetic method of the arithmetic unit) will be described later.

[0027] According to the shape detection device 10 configured as described above, by simply detecting the three-axis load and torque applied to the force sensor 11 that locks one end 12a of the linear flexible body 12, the shape over the entire length from one end 12a to the other end 12b of the linear flexible body 12 can be detected in real time.

[0028] As an application example of such a shape detection device 10, for example, when a force sensor 11 is provided on a drone (unmanned aerial vehicle), a transported object is locked to the other end of a linear flexible body 12 such as a wire and suspended, and the drone transports the transported object in the air. By detecting the shape of the linear flexible body 12 in real time, the amount of deflection of the linear flexible body 12 can be grasped, and the drone can be controlled so that the amount of deflection decreases, thereby safely transporting the transported object.

[0029] Also, for example, in the control of a soft robot equipped with the linear flexible body 12, by detecting the shape change of the linear flexible body 12 in real time, it becomes possible to perform damping control or the like of the soft robot.

[0030] In this embodiment, the force sensor 11 is connected only to one end 12a of the linear flexible body 12. However, in addition to this, for example, as shown in FIG. 2, a first force sensor 21 is provided at one end 12a of the linear flexible body 12, and a second force sensor 22 is provided at the other end 12b of the linear flexible body 12. A configuration may be adopted in which the shape of the linear flexible body 12 is detected based on the output information of these two first force sensor 21 and second force sensor 22.

[0031] In this embodiment, a linear flexible body is given as an example of the flexible body, but the flexible body is not limited to being linear. The linear flexible body, which is an example of the flexible body in this embodiment, is a member having flexibility such that when one end and the other end are supported and suspended, a hanging shape approximating a catenary curve is formed.

[0032] (Method for detecting the shape of a flexible body, method for controlling the shape of a flexible body, program, storage medium) Next, a method for detecting the shape of a flexible body and a method for controlling the shape of a flexible body using the flexible body shape detection device 10 described above will be described. FIG. 3 is a flowchart showing a method for detecting the shape of a flexible body and a method for controlling the shape of a flexible body according to an embodiment of the present invention. FIG. 4 is a schematic diagram for explaining the method for detecting the shape of a flexible body according to this embodiment. In this embodiment, as the linear flexible body (flexible body) 12, which is an example of the flexible body, a structure in which a plurality of rigid members are linearly connected to each other so as to be rotatable (one-dimensional structure) is used.

[0033] The method for detecting the shape of the flexible body first detects, by the force sensor 11, information on the load along the three-dimensional directions (X-axis, Y-axis, and Z-axis perpendicular to each other) applied to the detection unit 11a from one end 12a of the linear flexible body 12 curved in an arbitrary shape, here a hyperbolic shape, and information on the torque in the three-dimensional directions (step 1).

[0034] Next, an output signal including load information and torque information output from the force sensor 11 in step 1 is input to the arithmetic unit 13. The arithmetic unit 13 executes a pre-installed flexible body shape detection program, and for each of a plurality of divided sections, calculates the position in the middle, that is, the shape, from one end 12a to the other end 12b of the linear flexible body 12 based on the flexible body shape detection program. Thereby, shape data in the three-dimensional directions (X-axis, Y-axis, Z-axis) from one end 12a to the other end 12b of the linear flexible body 12 can be obtained (step 2). The obtained shape data of the linear flexible body 12 can be displayed as a graphic on, for example, a display unit 17.

[0035] Note that in the case where the linear flexible body 12 is formed by linearly connecting a plurality of rigid members to be rotatable relative to each other, each member is taken as one divided section. Further, in the case where the linear flexible body 12 is a string, rope, wire, etc. (discrete one-dimensional continuum) formed of a uniform flexible material in the longitudinal direction, it may be divided into an arbitrary number from one end to the other end to form divided sections.

[0036] Here, an example of the arithmetic method for shape detection included in the flexible body shape detection program will be described. The arithmetic method for estimating the shape of the linear flexible body 12 is geometrically expressed, simple enough for real-time calculation, and capable of high-speed operation. Such an arithmetic method mainly targets one-dimensional continua, but is also applicable to discrete one-dimensional structures such as chains and chains in which a plurality of rigid bodies are connected in series by freely jointed springs. In the case of a discrete structure, it is not impossible to arrange joint angle sensors at each degree-of-freedom joint, but based on the following arithmetic method, it is possible to estimate the shape of the linear flexible body 12 in real time only from the information of the force sensor connected to one end.

[0037] As shown in FIG. 4, as an example of a linear flexible body (flexible body), consider a model in which a plurality of rigid bodies (rods) are linearly connected by springs that are freely jointed. Let the number of divisions in discretization be \(n\). Let the numbers of the rigid bodies and spring joints be \(i\in\{0,1,2,\cdots,n\}\), and let the position of the \(i\)-th spring joint and the attitude of the rigid body be \(p\) i-1 \(\in\mathbb{R}\) 3 , \(F\) i \(\in SO(3)\) respectively. Note that in this index, \(p\) n represents the tip position of the discretized rod. The base part of each rod is fixed, and the force and moment applied there are \(f\) b , \(m\) b \(\in\mathbb{R}\) 3 respectively. At this time, the moment balance at each spring joint can be expressed by the discrete version of the following equation (1) (Euler's equation).

[0038]

Equation

[0039] Here, when \(K\) d = 0, the following equation (2) is obtained.

Equation

Equation

Equation

Number

[0040] Equation (4) means that the cross product of two vectors p i -p i-1 and x i is 0, that is, the directions of these two vectors are either exactly the same or exactly opposite. The former vector has a geometric meaning of a directed line segment representing the position and orientation of the i-th rigid body in the rigid body-spring joint series model obtained by discretizing the continuum rod. On the other hand, the latter vector is the difference between the force f b acting on the base of the elastic rod and the gravitational force on the part from the base of the elastic rod to the i-th joint. This corresponds to the force that pulls the segment of the i-th rigid body from the i-th joint side. Since the rod extends in the direction of pulling the rigid body, the directions of these two vectors are equal.

[0041] Also, from kinematic constraints, if the orientation of the rigid body is determined, the point that has advanced by 1 in that direction is the position of the next joint. Therefore, the recurrence formulas of Equations (6) and (7) are obtained.

Number

Number

[0042] The above calculation method is a geometric representation, which is very simple, allows for high-speed calculation, and can be implemented in real time. Considering the limit as the number of divisions n approaches infinity, the force x that pulls each rigid bodyi It will act in the tangent direction of the shape curve of the linear flexible body, which corresponds to the tension acting on a one-dimensional continuum. Therefore, Equation (4) can be regarded as a discrete version of the condition that the tension should satisfy. In such a calculation method, the moment mb applied to the base does not appear. This corresponds to the fact that in a linear flexible body, since the rigid body can be regarded as a structure connected by free joints, the moment is not transmitted and the shape is determined only by the tension acting on the continuum. Incidentally, from these, an example of an algorithm for estimating the shape of the flexible body is shown in FIG. 5.

[0043] Next, based on the shape between one end 12a and the other end 12b of the linear flexible body 12 obtained in Step 2, the arithmetic unit 13 calculates the joint angle in the three-dimensional direction for each divided section obtained by dividing the linear flexible body 12 into a plurality of sections (Step 3). Joint angle θ i To obtain, set e x T θ i = 0 and solve the following Equation (8).

Equation

[0044] When the linear flexible body is composed of one-degree-of-freedom free joints and moves in a vertical plane, the angle θ i of the i-th joint can be obtained by the following Equations (9) and (10).

Equation

Equation

[0045] In this case, the shape curve is known to be a hyperbolic function called a catenary curve, but the plane curve actually drawn by the above formula coincides with the hyperbolic function. Therefore, in the method for detecting the shape of the flexible body according to the present embodiment, it is possible to estimate the overall shape of the linear flexible body and further the position of the other end of the linear flexible body from the detection value by the base force sensor.

[0046] Next, when performing shape control of the flexible body using such a method for detecting the shape of the flexible body, for example, based on the shape data of the linear flexible body, the center of gravity of the linear flexible body is calculated (step 4). Then, based on the center of gravity of the linear flexible body obtained in step 4, by controlling the position of the center of gravity of the linear flexible body, the shape along the extension direction of the linear flexible body can be controlled (step 5).

[0047] The program of the present embodiment may be a program that causes a computer to execute operations and control related to the method for detecting the shape of the flexible body as described above. Further, the storage medium of the present embodiment may be a memory, hard disk, USB memory, CDROM, etc. on a computer that stores the above-described program.

[0048] As described above, in the flexible body shape detection device and the flexible body shape detection method of the present invention, it is possible to estimate in real time the shape of the flexible body with extremely small elasticity along the extension direction in a three-dimensional space based on the information of the force sensor attached to one end of the flexible body.

[0049] Although the embodiments of the present invention have been described above, such embodiments are presented as examples and are not intended to limit the scope of the invention. These embodiments can be implemented in various other forms, and various omissions, replacements, and changes can be made without departing from the gist of the invention. These embodiments and their modifications are included in the scope and gist of the invention, and are also included in the invention described in the claims and its equivalent scope.

Example

[0050] The effects of the present invention were verified. In the verification, a flexible body shape detection device as shown in FIG. 1 was prepared. As an example of the flexible body, a multi-joint arm in which a plurality of rigid bodies made of resin are linearly connected by joints that are rotatable relative to each other was used (FIGS. 6 and 7). This multi-joint arm is a mechanism in which rigid links are connected in series by one-degree-of-freedom revolute joints and can operate in a vertical plane by wire drive. The number of joints, i.e., the number of divisions n, of the multi-joint arm used in the verification was 17, the mass m was 0.49 kg, and the overall length was 0.75 m. In this verification, wire drive was not used. One end of this multi-joint arm was fixed to a six-axis force sensor (TKB0554014R0A00: manufactured by Repton Co., Ltd., rated force for each axis ±400 N, rated torque for each axis ±4 Nm), and the other end was held by hand.

[0051] The measured values of the six-axis force sensor were input to a notebook PC via serial communication, and the shape was estimated according to the above-described flexible body shape detection method. Then, the shape of the obtained linear flexible body was drawn using gnuplot and displayed on the monitor of the notebook PC (FIGS. 6 and 7). According to FIGS. 6 and 7, it was confirmed that a shape similar to the rod shape determined by the holding position of the multi-joint arm was displayed on the monitor of the notebook PC.

[0052] Also, it was confirmed that the estimated shape changed in accordance with the change in the holding position on the other end side of the multi-joint arm. At this time, the drawing on the monitor of the notebook PC showed the shape after the change without delay almost simultaneously when the other end side of the multi-joint arm was moved. Therefore, the operation and effects of the present invention were confirmed.

Explanation of Signs

[0053] 10... Shape detection device (flexible body shape detection device) 11... Force sensor 12... Linear flexible body (flexible body) 13... Arithmetic unit

Claims

1. A force - sensing sensor that detects loads and torques applied in three - dimensional directions respectively, A flexible body with one end connected to the force - sensing sensor, An arithmetic unit that calculates the shape along the extension direction from one end to the other end of the flexible body based on the information of the load and the torque applied from one end of the flexible body to the force - sensing sensor. A flexible - body shape detection device characterized by comprising these components.

2. The force - sensing sensor is a six - axis force - sensing sensor that detects the load along the three - dimensional directions of the X - axis, Y - axis, and Z - axes perpendicular to each other and the respective rotational forces around the X - axis, Y - axis, and Z - axes. The flexible - body shape detection device according to Claim 1, characterized by this.

3. The flexible - body shape detection device according to Claim 1 or 2, characterized in that the force - sensing sensor is further connected to the other end of the flexible body.

4. The flexible - body shape detection device according to Claim 1 or 2, characterized in that the flexible body is a linear flexible body having a uniform elastic force in the extension direction.

5. Step 1: Detecting information on the load in three - dimensional directions applied from the flexible body to the force - sensing sensor and information on the torque in three - dimensional directions applied from the flexible body to the force - sensing sensor; Step 2: Calculating a position in the middle between one end and the other end of the flexible body based on the gravity potential minimum condition from the information on the load and the torque obtained in Step 1, and obtaining shape data in the three - dimensional directions from one end to the other end of the flexible body; Step 3: Calculating the angle in the three - dimensional directions for each divided section obtained by dividing the flexible body into a plurality of sections from the information on the shape data in the three - dimensional directions obtained in Step 2; A flexible - body shape detection method characterized by comprising at least these steps.

6. Step 4: Calculating the center of gravity of the flexible body based on the shape data of the flexible body obtained by the flexible - body shape detection method of Claim 5. Step 5 of controlling the center of gravity of the flexible body based on the center of gravity of the flexible body obtained in the previous step 4 to control the shape along the extension direction of the flexible body; The method for controlling the shape of a flexible body according to claim 5, characterized by comprising at least the above.

7. For a computer, Step 1 of detecting information on the three-dimensional load applied from the flexible body to the force sensor and information on the three-dimensional torque applied from the flexible body to the force sensor; Step 2 of calculating the position in the middle between one end and the other end of the flexible body based on the gravitational potential minimum condition from the load information and the torque information obtained in the previous step 1, and obtaining shape data in the three-dimensional direction from one end to the other end of the flexible body; Step 3 of calculating the angle in the three-dimensional direction for each divided section obtained by dividing the flexible body into a plurality of sections from the information on the shape data in the three-dimensional direction obtained in the previous step 2; A program characterized by executing each of the above steps.

8. For a computer, Step 1 of detecting information on the three-dimensional load applied from the flexible body to the force sensor and information on the three-dimensional torque applied from the flexible body to the force sensor; Step 2 of calculating the position in the middle between one end and the other end of the flexible body based on the gravitational potential minimum condition from the load information and the torque information obtained in the previous step 1, and obtaining shape data in the three-dimensional direction from one end to the other end of the flexible body; Step 3 of calculating the angle in the three-dimensional direction for each divided section obtained by dividing the flexible body into a plurality of sections from the information on the shape data in the three-dimensional direction obtained in the previous step 2; A recording medium characterized by recording a program for executing each of the above steps.

Images