JP2022113567A - Control device, position estimation method and program - Google Patents

Abstract

[Problem] To provide a method for estimating the position of an obstacle when the manipulator comes into contact with the obstacle during operation. [Solution] A control device includes a contact detection unit that detects when the arm of the manipulator comes into contact with an obstacle, and a position estimation unit that estimates the position of the obstacle based on the movement direction of a representative point set on the link of the arm where the contact was detected, and the distance from the representative point to the obstacle. [Selected Figure] Figure 1

Description

The present disclosure relates to a control device, position estimation method, and program.

In manipulator control, the angle and angular velocity of each joint are controlled so that the position and posture of the hand follow a desired target trajectory. When the manipulator comes into contact with an object, the obstacle prevents the manipulator from driving, so if the manipulator tries to continue working, the motor, the manipulator body, or the obstacle may be damaged. Patent document 1 estimates the disturbance torque of the manipulator link (the part between the joints), detects the contact between the manipulator and the obstacle from the estimated disturbance torque, returns it to its original position, stops the manipulator A control for mitigating collisions is disclosed. When the manipulator comes into contact with an obstacle, there is a demand for a control method that allows flexible selection of other actions (for example, avoiding obstacles and reaching the target position, etc.) in addition to collision mitigation. ing. Non-Patent Document 1 discloses a method of detecting contact between a manipulator and an obstacle, and a method of identifying a link on which contact has occurred.

Japanese Patent Application Laid-Open No. 2006-167820

A. D. Luca et al, “Collision Detection and Safe Reaction with the DLR-III Lightweight Manipulator Arm”, IEEE/RSJ International Conference on Intelligent Robots and Systems, 2006.

After the manipulator comes into contact with an obstacle, it is necessary to know the position of the obstacle in order to select various subsequent actions.

The present disclosure provides a control device, position estimation method, and program that can solve the above problems.

According to an embodiment of the present disclosure, a control device includes a contact detection unit that detects contact of an arm of a manipulator with an obstacle, and a representative of the arm that is set on a link where the contact is detected. a position estimating unit that estimates the position of the obstacle based on the moving direction of the point and the distance from the representative point to the obstacle.

According to an embodiment of the present disclosure, a position estimation method comprises the steps of detecting contact of an arm of a manipulator with an obstacle; and estimating the position of the obstacle based on the direction of movement of and the distance from the representative point to the obstacle.

According to one embodiment of the present disclosure, the program causes the computer to detect that an arm of the manipulator has come into contact with an obstacle, and to move a representative point set on a link of the arm where the contact is detected. A process of estimating the position of the obstacle based on the direction and the distance from the representative point to the obstacle is executed.

According to the control device, position estimation method, and program described above, the position of the obstacle can be estimated.

It is a figure showing an example of a manipulator concerning an embodiment. It is a flow chart which shows a flow of control at the time of obstacle contact concerning a first embodiment. 6 is a flowchart of obstacle position estimation processing according to the first embodiment. FIG. 11 is a first diagram illustrating an example of obstacle position estimation processing according to the first embodiment; FIG. 7 is a second diagram illustrating an example of obstacle position estimation processing according to the first embodiment; It is a figure explaining an example of the estimation result of the obstacle position which concerns on 1st embodiment. It is a figure explaining the process which estimates the shape of the obstacle which concerns on 2nd embodiment. It is a figure which shows an example of the hardware constitutions of the control apparatus of embodiment.

<Embodiment>
The manipulator of the present disclosure will be described below with reference to FIGS. 1-8.

(Constitution)
FIG. 1 is a diagram showing an example of a manipulator according to an embodiment.
A manipulator 100 shown in FIG. 1 includes a control device 10 and an arm 20 . Arm 20 includes links 201A-201D and joints 202A-202C. The proximal end of the link 202D is fixed to the device 30 that serves as the base of the moving body or the like. The distal end of link 201D is connected to the proximal end of link 201C via joint 202C. The distal end of link 201C is connected to the proximal end of link 201B via joint 202B. The distal end of link 201B is connected to the proximal end of link 201A via joint 202A. A working tool such as an end effector (not shown) is attached to the tip of the link 201A. The links 201A to 201D may be collectively referred to as the link 201, and the joints 202A to 202C may be collectively referred to as the joint 202. Arm 20 may have a different number of links and a different number of joints. The joint 202 incorporates a motor 203 and an angle sensor 204 . The orientation of the link 201 connected to the joint 102 is controlled by the rotation of the motor 203 . An angle sensor 204 measures the angle and angular velocity of the joint 202 . The control device 10 controls the motor 203 and controls the posture of the arm 20 while monitoring the angle and angular velocity of the joint 202 measured by the angle sensor 204 . The control device 10 may be provided inside the arm 20 (for example, the link 201D) or may be provided separately from the arm 20 . It should be noted that sensors for detecting contact with an obstacle or the like are not provided outside the arm 20 .

The control device 10 includes a sensor information acquisition section 11 , an arm control section 12 , a contact detection section 13 , an obstacle position estimation section 14 , a storage section 15 and an obstacle shape estimation section 16 .
The sensor information acquisition unit 11 continuously acquires the angle and angular velocity detected by the angle sensor 204 at predetermined time intervals.
The arm control unit 12 receives instruction information about the target position of the tip of the arm 20 and the target posture of the arm 20 and controls the operation of the arm 20 . For example, the arm control unit 12 calculates a target trajectory of the arm 20, a target angle of each joint 202, and the like, and controls the motor 203 so that the angles and angular velocities acquired by the sensor information acquisition unit 11 become target values. Any known control method may be used for the arm control unit 12 to control the joint 202 .

The contact detection unit 13 detects contact of the link 201 with an obstacle. The contact detection unit 13 calculates the disturbance torque generated in the link 201 and determines that the link 201 has come into contact with an obstacle if the calculated value is equal to or greater than a predetermined threshold value (for example, 0 or a value that can be regarded as 0). Contact with an obstacle is detected by this determination. A method for calculating the disturbance torque generated in the link 201 and a method for detecting contact with an obstacle are described in Patent Document 1 and Non-Patent Document 1. For example, the contact detection unit 13 detects contact with an obstacle by methods similar to those described in these documents. As described in Non-Patent Document 1, the contact detection unit 13 detects contact with an obstacle for each link 201 .

The obstacle position estimating unit 14 estimates the position of the obstacle and records information on the estimated position in the storage unit 15 . A method of estimating the position of an obstacle will be described later.
The storage unit 15 stores information necessary for controlling the arm 20 . The storage unit 15 also stores information on the obstacle position estimated by the obstacle position estimation unit 14 .
The obstacle shape estimation unit 16 calculates the shape and range of the obstacle based on the information on the position of the obstacle stored in the storage unit 15, and outputs information indicating the shape of the obstacle and the range in which the obstacle exists. .

<First embodiment>
The first embodiment relates to obstacle position estimation processing. The obstacle position estimation processing of the manipulator 100 will be described with reference to FIGS. 2 to 6. FIG.

(motion)
FIG. 2 is a flowchart showing the flow of control when contacting an obstacle according to the first embodiment.
While the arm control unit 12 controls the operation of the arm 20 based on the instruction information, in parallel, the control device 10 monitors the value of the angle sensor 204 acquired by the sensor information acquisition unit 11, and performs the following processing. Repeat continuously.
The contact detection unit 13 estimates disturbance torque generated in each of the links 201A to 201D (step S11). For example, the contact detection unit 13 estimates the disturbance torque generated in each link 201 based on the value measured by the angle sensor 204 by the method described in Patent Document 1 or Non-Patent Document 1. Next, the contact detection unit 13 performs contact determination depending on whether or not the value of the disturbance torque is equal to or greater than the threshold (step S12). A threshold is, for example, a value that can be regarded as 0. The contact detection unit 13 determines that contact has occurred when the disturbance torque is equal to or greater than the threshold, and determines that contact has not occurred when the disturbance torque is less than the threshold. The contact detection unit 13 performs this contact determination for each of the links 201A-201D. If it is determined that contact has not occurred on any of the links 201 (step S12; No), this processing flow ends.

When it is determined that contact has occurred on any link (step S12; Yes), the contact detection unit 13 identifies the contact link (step S13). A contact link is a link 201 that identifies that it has come into contact with an obstacle. For example, when contact with an obstacle occurs in link 201B, in many cases, disturbance torques equal to or greater than a threshold are detected in links 201B, 201C, and 201D. The contact detection unit 13 identifies, as the contact torque, the link on the leading end side among the links 201 for which the disturbance torque equal to or greater than the threshold is detected. In this example, the contact detection unit 13 identifies the link 201B as the contact link. That is, when the disturbance torque exceeds the threshold value in a plurality of links, the contact detection unit 13 identifies the link closest to the leading end as the contact link.

Next, the obstacle position estimation unit 14 estimates the obstacle position (step S14). The obstacle position estimator 14 detects an obstacle in the direction in which the arm 20 was moving at the time the contact was detected, and detects the obstacle in the procedure shown in FIG. FIG. 3 is a flowchart of obstacle position estimation processing according to the first embodiment. First, the obstacle position estimation unit 14 sets a representative point (step S21). A representative point is a joint 202 connected to the contact link 201 identified in step S13. If the joints 202 are located at both ends, the obstacle position estimator 14 identifies one of them as a representative point. Which of the joints 202 at both ends is to be the representative point will be described later with reference to FIGS. 4 and 5. FIG. Next, the obstacle position estimation unit 14 calculates the moving direction of the representative point (step S22). The obstacle position estimator 14 calculates the moving direction of the representative point of the link 201 (contact link) in contact with the obstacle from the joint angle or joint angular velocity of the arm 20 at the time of contact detection. Next, the obstacle position estimating unit 14 estimates one point existing on the movement direction calculated in step S22 as the obstacle position (step S23).

Here, the processing shown in the flowchart of FIG. 3 will be described with a specific example.
FIG. 4 is a first diagram illustrating an example of obstacle position estimation processing according to the first embodiment.
FIG. 4 shows a method of estimating the position of an obstacle in the moving direction of the representative point, using the joint 202 on the distal end side (hand side) of the contact link as a representative point. FIG. 4 shows a situation in which the obstacle 400 contacts the link 201B while moving the joint 202 of the entire arm 20 and moving the hand diagonally downward to the left as indicated by the arrow YA1. At this time, the link 201B is identified as the contact link in step S13. While the arm 20 is moving by moving all the joints 202, or while moving the joint 202 on the distal end side of the arm 20 as seen from the contact link, the contact detection unit 13 detects contact with the obstacle. is detected, the obstacle position estimation unit 14 sets the joint 202 on the tip side of the specified contact link as a representative point (step S21). In FIG. 4, since the contact link is the link 201B, the obstacle position estimator 14 sets the joint 202A as the representative point.

Next, the obstacle position estimating section 14 calculates the moving direction of the representative point as follows (step S22). Assume that the joint angle q of the arm 20 and the joint angular velocity q·(dot representing differentiation). Here, the joint angle q and the joint angular velocity q· are vectors of the sum of angles and angular velocities of all the joints 202 of the arm 20 . Then, the position of the representative point can be represented by the following formula (1).
Position of representative point: _{xk=fk (} q) (1)
where f _k (q) is the geometric expression of the position of the representative point.
The speed of the representative point can be expressed by the following equation (2).

Here, J is the Jacobian matrix disclosed in Non-Patent Document 1, "III. DETECTION AND REACTION WITH RIGID ROBOTS."
The obstacle position estimator 14 calculates the moving direction of the representative point from the following equation (3).

Next, the obstacle position estimation unit 14 estimates the obstacle position as follows (step S23). The obstacle position estimating unit 14 estimates the obstacle position μ by the following formula (4) based on the movement direction calculated by the formula (3) in step S22. Note that d indicates the distance (shortest distance) from the contact link to the obstacle 400 (d in FIG. 4). For the distance d, for example, the radius R (R1÷2 in FIG. 4) of the link 201B of the arm 20 is set. Note that setting the link radius R to the distance d is an example, and another value may be set to the distance d.

By substituting the angles and angular velocities measured by the angle sensors 204 of all the joints 202 into equation (2), and by substituting the link radius R into d in equation (4), the obstacle position μ is calculated. can be done. The position of the calculated obstacle position is shown as obstacle position μ0 in FIG. The obstacle position estimating unit 14 calculates the position _xk of the representative point and the moving direction of the representative point (equation (3) ) is calculated, and the obstacle position μ0 is calculated by equation (4). The obstacle position μ0 is, for example, three-dimensional position information.

Reference is now made to FIG. FIG. 5 is a second diagram illustrating an example of obstacle position estimation processing according to the first embodiment.
FIG. 5 shows a method of estimating an obstacle position in the moving direction of the joint 202 on the proximal side (base side) of the contact link. FIG. 5 shows a situation in which an obstacle 401 is contacted while moving the elbow joint 202B in the direction of the arrow YA2 without moving the wrist joint 202A of the arm 20. FIG. At this time, the link 201B is identified as the contact link in step S13. When the elbow joint 202B is moved even though the wrist joint 202A is fixed, it may not be appropriate to estimate the obstacle position in the moving direction of the wrist joint 202A. Therefore, by changing the representative point according to the movement of the arm 20, more accurate obstacle position estimation is performed. For example, when the contact detection unit 13 detects contact with the obstacle 401 while the joint 202 on the proximal side is moving without moving the joint 202 on the distal side as viewed from the contact link, the obstacle position The estimating unit 14 sets the joint 202 on the base end side of the specified contact link as a representative point (step S21). In FIG. 5, the contact link is the link 201B, so the obstacle position estimator 14 sets the joint 202B as the representative point.

Next, the obstacle position estimating unit 14 calculates the moving direction of the representative point using the above equation (3) (step S22). Next, the obstacle position estimating section 14 estimates the obstacle position μ1 by the above equation (4) (step S23).

4 and 5, if the joint 202A on the tip side of the contact link is moving when contact with the obstacles 400 and 401 is detected, the joint 202A on the tip side of the contact link is used as a representative point, When the joint 202A on the distal side of the contact link is not moving and the joint 202B on the proximal side is moving, the joint 202B on the proximal side of the contact link is set as the representative point. The method is not limited. Depending on the situation, the proximal side may be set as the representative point even if the distal joint 202A of the contact link is moving, or even if the distal joint 202A of the contact link is not moving. , the joint 202A on the distal end side may be set as a representative point. For example, both a process of estimating the obstacle position μ using the joint 202A on the distal side of the contact link as a representative point and a process of estimating the obstacle position μ using the joint 202B on the proximal side of the contact link as a representative point are performed, As a result of multiple times of obstacle position estimation processing, an obstacle position μ estimated in a more probable direction is selected as the obstacle position, and the representative point in that case is used for subsequent contact detection for the same contact link. It may be determined as a representative point.

Return to FIG. After estimating the obstacle position μ by the process of FIG. 3, the obstacle position estimation unit 14 records the estimated obstacle position μ in the storage unit 15 (step S15). FIG. 6 shows an example of the estimated obstacle position μ. Line 300 in FIG. 6 and other similar lines show the history of arm 20 posture during various movements of arm 20 . Obstacle positions μ2 to μ5 indicate obstacle positions estimated by the process of step S14 when contact with the obstacle 402 is detected while the posture of the arm 20 is being varied. The obstacle position estimating unit 14 records in the storage unit 15 a plurality of obstacle positions .mu.2 to .mu.5 estimated while the arm 20 is operating.

Further, the obstacle position estimating section 14 records the obstacle position μ in the storage section 15 and outputs the obstacle position μ to the arm control section 12 . The arm control unit 12 executes post-contact processing based on the obstacle position μ (step S16). For example, the arm control unit 12 may perform collision mitigation control such as stopping the arm 20 or returning the arm 20 to an original position where contact with an obstacle does not occur. Furthermore, when the obstacle position μ can be grasped, the arm 20 can be made to move positively. For example, based on the obstacle position μ, the arm control unit 12 may avoid the obstacle and move the tip of the arm 20 closer to the target position. For the control of the arm 20 at that time, known techniques may be used for the method of reaching the target position and the method of avoiding obstacles. For example, the potential method is known as an example of a method for avoiding an obstacle when the obstacle position μ is known. The arm controller 12 may apply the potential method to control the arm 20 to avoid obstacles. As for the method of reaching the target position, the arm control section 12 may control the arm 20 using a known technique for reaching the target position of the manipulator. By acquiring the estimation result of the obstacle position μ, the arm control unit 12 can select the operation of the arm 20 after contacting the obstacle.

As described above with reference to FIGS. 2 to 6, according to the first embodiment, the positions of obstacles can be estimated based on the positions and velocities of representative points set on contact links. Known obstacle avoidance techniques can be applied by recognizing the position of the obstacle, and the arm 20 can perform operations other than rewinding and stopping after contact. As a result, in the conventional technology, when the arm 20 comes into contact with an obstacle, it is necessary to adjust the trajectory of the arm 20 after mitigating the collision such as by stopping. Since it is possible to automate the execution of the target motion while moving, the work efficiency using the manipulator 100 is improved. In addition, by switching the setting of the representative point according to the operation state of the arm 20 (whether or not the wrist joint 202A is moved, etc.), the position of the obstacle can be estimated more accurately. Further, according to the present embodiment, even if the manipulator 100 is not provided with an external sensor (a contact sensor, a camera, etc.) for contact detection and obstacle position estimation, contact with an obstacle can be detected, and the obstacle position can be determined. can be estimated. Since an external sensor is not required, even when the arm 20 is operated in a narrow space, the working space is not narrowed by the sensors, and there is no need to worry about failure or breakage of the sensors. For example, the control method of this embodiment can be applied to control a manipulator in an environment where a camera cannot be installed. Moreover, cost, weight, etc. can be reduced by not requiring an external sensor.

<Second embodiment>
The second embodiment relates to processing for estimating the shape and range of an obstacle based on the obstacle position estimated by the method of the first embodiment. A second embodiment will be described with reference to FIG.

FIG. 7 is a diagram illustrating processing for estimating the shape of an obstacle according to the second embodiment.
As described in step S15 in FIG. 2, when the arm 20 continues to operate, a plurality of obstacle positions μ (for example, obstacle positions μ2 to μ5) are recorded in the storage unit 15. FIG. The obstacle shape estimator 16 estimates the shape and range of the obstacle 402 from a plurality of obstacle positions μ, and outputs the estimation result. A left diagram 701 in FIG. 7 is a diagram plotting the obstacle positions μ2 to μ5 recorded in the storage unit 15 based on the position information. The obstacle shape estimator 16 estimates the shape of the obstacle based on the obstacle positions μ2 to μ5. For example, the obstacle shape estimating unit 16 includes all obstacle positions μ2 to μ5, calculates a rectangular parallelepiped or a cylinder whose volume is the smallest, and determines the shape of the calculated rectangular parallelepiped as the shape of the obstacle. The space occupied is assumed to be the range in which the obstacle exists. Further, the obstacle shape estimating unit 16 outputs the estimated rectangular parallelepiped and the positional relationship thereof to a display device, an electronic file, or the like. The result of obstacle shape estimation by the obstacle shape estimation unit 16 and an output example thereof are shown in the right diagram 702 of FIG. An obstacle u6 in the right diagram 702 indicates the shape and range of the obstacle estimated by the obstacle shape estimation unit 16 . By displaying the estimation result, the positional relationship between the obstacle shape based on the actual contact history with the obstacle and the manipulator 100 can be easily grasped. Further, by passing the position information of the obstacle range estimated by the obstacle shape estimation unit 16 to the arm control unit 12, the arm control unit 12 can perform control to avoid obstacles efficiently.

In the second embodiment, each of the plurality of obstacle positions μ estimated by the obstacle position estimating unit 14 is considered to be a part (point) of the entire obstacle, and these are integrated to obtain the shape of the entire obstacle to estimate As a result, it is possible to determine whether or not the position, which has not been estimated as the obstacle position μ, is part of the obstacle. As a result, the arm 20 can be operated while predicting and avoiding a position that may be part of the obstacle, so that the arm 20 can be smoothly operated toward the target position.

In addition, in order to improve the efficiency of control of the arm 20, the arm 20 is moved in various directions (for example, comprehensively in all directions) at the start of work to intentionally come into contact with an obstacle, and the position of the obstacle is determined. The estimating unit 14 may perform obstacle position estimation and record position information, and the obstacle shape estimating unit 16 may perform processing for estimating the shape of the obstacle. By grasping the shape and range of the obstacle in the first stage of the work, subsequent control of the manipulator 100 can be made more efficient.

8 is a diagram illustrating an example of a hardware configuration of a control device according to the embodiment; FIG.
A computer 900 includes a CPU 901 , a main memory device 902 , an auxiliary memory device 903 , an input/output interface 904 and a communication interface 905 .
The control device 10 described above is implemented in a computer 900 . Each function described above (sensor information acquisition unit 11, arm control unit 12, contact detection unit 13, obstacle position estimation unit 14, μ16) is stored in the auxiliary storage device 903 in the form of a program. The CPU 901 reads out the program from the auxiliary storage device 903, develops it in the main storage device 902, and executes the above processing according to the program. Also, the CPU 901 secures a storage area in the main storage device 902 according to the program. In addition, the CPU 901 secures a storage area for storing data being processed in the auxiliary storage device 903 according to the program.

A program for realizing all or part of the functions of the control device 10 is recorded on a computer-readable recording medium, and the program recorded on this recording medium is read by a computer system and executed. Processing by the functional unit may be performed. The "computer system" here includes hardware such as an OS and peripheral devices. The "computer system" also includes the home page providing environment (or display environment) if the WWW system is used. The term "computer-readable recording medium" refers to portable media such as CDs, DVDs, and USBs, and storage devices such as hard disks incorporated in computer systems. Further, when this program is distributed to the computer 900 via a communication line, the computer 900 receiving the distribution may develop the program in the main storage device 902 and execute the above process. Further, the above program may be for realizing a part of the functions described above, or may be capable of realizing the functions described above in combination with a program already recorded in the computer system. .

Although several embodiments of the present disclosure have been described above, all these embodiments are provided by way of example 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 modifications can be made without departing from the scope of the invention. These embodiments and their modifications are included in the scope and spirit of the invention, as well as the scope of the invention described in the claims and equivalents thereof.

<Appendix>
The control device 10, the position estimation method, and the program described in each embodiment are grasped, for example, as follows.

(1) The control device 10 according to the first aspect includes a contact detection unit 13 that detects that the arm 20 of the manipulator 100 has come into contact with obstacles 400, 401, etc., and Obstacle for estimating the position of the obstacle (μ0 to μ5) based on the moving direction (equation (3)) of the representative point set on the link and the distance d from the representative point to the obstacle and a position estimation unit 14 .
Thereby, contact with an obstacle can be detected and its position can be estimated without providing a sensor for detecting the external environment of the manipulator 100 . Since the position of the obstacle can be estimated, the position information can be used to arbitrarily control the operation of the manipulator 100 after detecting contact with the obstacle.

(2) The control device 10 according to the second aspect is the control device 10 of (1), wherein the obstacle position estimating section 14 determines the representative set a point.
Accordingly, by setting the representative point according to how the arm 20 is moved, the position of the obstacle can be accurately estimated.

(3) The control device 10 according to the third aspect is the control device 10 of (2), in which the tip end side of the joint 202 connected to the link (201B in FIG. 4) in which the contact is detected When contact with the obstacle 400 is detected while the joint 202A is moving, the obstacle position estimating section 14 sets the joint 202A on the tip side of the link as the representative point.
In the first embodiment of the present disclosure, it is assumed that there is an obstacle in the movement direction of the contact link. No disturbance torque is detected in the link beyond the distal joint), and the movement direction of the distal joint of the contact link is more closely related to the obstacle position than the movement direction of the proximal side (more appropriately, the obstacle represents the object position), and the joint on the tip side of the contact link is set as the representative point. Thereby, the obstacle position is properly estimated.

(4) The control device 10 according to the fourth aspect is the control device 10 of (2) to (3), wherein out of the joints 202 connected to the link (201B in FIG. 5) in which the contact is detected, , when contact with the obstacle 401 is detected while only the joint 202B on the proximal side is being moved, the obstacle position estimating section 14 uses the joint 202B on the proximal side of the link as the representative point. set.
In the first embodiment of the present disclosure, it is assumed that there is an obstacle in the movement direction of the contact link. If contact occurs due to the movement of the joint on the proximal side of the link, the direction of movement of the proximal side of the contact link is more closely related to the position of the obstacle than the direction of movement of the distal side. Set joints as representative points. Thereby, the obstacle position is properly estimated.

(5) A control device 10 according to a fifth aspect is the control device 10 of (1) to (4), wherein the contact detection unit detects contact with the obstacle in the link that constitutes the arm. Among the links for which is detected, the link closest to the leading end is identified as the link that has contacted the obstacle.
When the arm comes into contact with an obstacle, disturbance torque is observed in the contact link and the link on the base end side (base side). Therefore, by identifying the link closest to the leading end of the links where the disturbance torque is observed as the contacting link, the probability of correctly identifying the link that was actually in contact increases, thereby estimating the position of the obstacle. Accuracy can be ensured.

(6) A control device 10 according to a sixth aspect is the control device 10 of (1) to (5), wherein the distance from the representative point to the obstacle is the length of the radius of the link that constitutes the arm. set the
Although the distance from the representative point to the obstacle is unknown, it is possible to estimate the position of the obstacle by setting the link radius as the distance as a value that does not differ greatly.

(7) A control device 10 according to a seventh aspect is the control device 10 of (1) to (6), wherein the position estimation section records the estimated position of the obstacle in the storage section.
By recording the position of the obstacle, basic data for estimating the shape and range of the entire obstacle can be accumulated.

(8) A control device 10 according to an eighth aspect is the control device 10 of (7), in which the shape of the obstacle is estimated based on the position of the obstacle recorded in the storage unit. and an estimator.
Obstacle avoidance becomes possible by estimating the shape of the obstacle. Thereby, the working efficiency of the manipulator 100 can be improved.

(9) A position estimation method according to a ninth aspect includes the step of detecting that an arm of a manipulator contacts an obstacle; and estimating the position of the obstacle based on the direction of movement and the distance from the representative point to the obstacle.

(10) The program according to the tenth aspect causes the computer 900 to detect that the arm of the manipulator has come into contact with an obstacle, and to set a representative point on the link of the arm where the contact has been detected. A process of estimating the position of the obstacle based on the direction of movement and the distance from the representative point to the obstacle is executed.

DESCRIPTION OF SYMBOLS 10... Control apparatus 11... Sensor information acquisition part 12... Arm control part 13... Contact detection part 14... Obstacle position estimation part 15... Storage part 16... Obstacle shape Estimation unit 20 Arm 100 Manipulators 201, 201A, 201B, 201C, 201D Links 202, 202A, 202B, 202C Joint 203 Motor 204 Angle sensor 900・Computer 901 CPU
902 Main storage device 903 Auxiliary storage device 904 Input/output interface 905 Communication interface

Claims (10)

a contact detection unit that detects that the arm of the manipulator has come into contact with an obstacle;
A position estimating unit for estimating the position of the obstacle based on the moving direction of the representative point set on the link of the arm where the contact is detected and the distance from the representative point to the obstacle. When,
A control device comprising: The position estimating unit sets the representative point according to the motion state of the joints of the arm.
A control device according to claim 1 . When contact with the obstacle is detected while moving the joint on the distal end side among the joints connected to the link where the contact is detected,
The position estimation unit sets the joint on the tip side of the link as the representative point.
3. A control device according to claim 2. When contact with the obstacle is detected while only the joint on the base end side is moving among the joints connected to the link where the contact is detected,
The position estimation unit sets the joint on the proximal side of the link as the representative point.
4. The control device according to claim 2 or 3. The contact detection unit identifies the link closest to the tip of the links that have detected contact with the obstacle among the links that constitute the arm as the link that has contacted the obstacle.
The control device according to any one of claims 1 to 4. setting the length of the radius of the link that constitutes the arm to the distance from the representative point to the obstacle;
The control device according to any one of claims 1 to 5. The position estimation unit records the estimated position of the obstacle in a storage unit.
The control device according to any one of claims 1 to 6. a shape estimation unit that estimates the shape of the obstacle based on the position of the obstacle recorded in the storage unit;
8. The controller of claim 7, further comprising: Detecting that the manipulator arm has come into contact with an obstacle,
estimating the position of the obstacle based on the moving direction of the representative point set on the link of the arm where the contact is detected and the distance from the representative point to the obstacle;
Location estimation method. to the computer,
Detecting that the manipulator arm has come into contact with an obstacle,
A process of estimating the position of the obstacle based on the moving direction of the representative point set on the link of the arm where the contact is detected and the distance from the representative point to the obstacle;
program to run.

Images