JP2000079582A - Controlling method for articulated robot arm - Google Patents

Abstract

PROBLEM TO BE SOLVED: To obtain a controlling method for a five-axis articulated robot arm for freely controlling the said arm by position attitude information from a position sensor. SOLUTION: This controlling method has a detection process: for detecting the position attitude information of a movable body in a space, an arithmetic process: for operating the joint angle of a articulated robot arm 5 that is a control object based on this detected said information, and a setting process: for setting an action range restriction value based on the joint angle at the time of the actuation of the said robot along the said information.

Description

DETAILED DESCRIPTION OF THE INVENTION

[0001]

BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention relates to a method for controlling an articulated robot arm that can freely operate a robot arm based on position and orientation information detected by a position sensor installed on a fingertip or an arm.

[0002]

2. Description of the Related Art (1) Description of Configuration of Prior Art Position and orientation of space (distance in X, Y, and Z directions, roll,
Obtaining the joint angle of the robot arm from the pitch and yaw angle) is called reverse motion. FIG. 12 shows a side view of a five-axis articulated robot arm. The length and angle of each arm are defined as follows.

40 Zh: Shoulder center height 41 Ls: Shoulder length 42 Le: Elbow length 43 Lw: Wrist length 44 Lt: Tool length 45 Lh: Shoulder center offset 46 θw: Waist angle (rotation with respect to horizontal plane 55 Angle, clockwise direction is positive, range is + 100 °, -100 °) 47 θs: shoulder angle (elevation angle from horizontal plane 55, upward is positive, range is + 100 °, -30 °) 48 θe : Elbow angle (elevation angle from horizontal plane 55, upward is positive, range is + 0 °, -110 °) 49 θp: Pitch angle (elevation angle from horizontal plane 55, upward is positive, range is + 90 °, 50 θr: Roll angle (Relative angle to pitch angle 49, positive clockwise when viewed from the outside, range + 180 °, -180 °)

The hand tip position / posture 54 is (x, y,
z, p, r). Here, x, y, and z are the position coordinates of the tip of the hand, p (pitch), and r (roll) are the hand posture.

(2) Description of the operation and operation of the prior art Next, the operation and operation of the prior art will be described with reference to the side view of the robot arm shown in FIG. The hand tip position / posture 54 is (x, y, z, p, r), the wrist center position 53 is (x1, y1, z1), and the shoulder center position 52 is (x2, y2, z2).

From the definition of the hand tip attitude, pitch angle 49 θp = p roll angle 50 θr = r

FIG. 13 is a diagram when the wrist unit 53 is viewed from the top, and FIG. 14 is a diagram when the wrist unit 53 is viewed from the side. From FIG. 13, the waist angle 46 θw = tan ⁻¹ (x /
y)

As a preparation before obtaining θs and θe, a list center position 53 (x1, y1, z1) is first determined. FIG.
L1 60 of 4 is the length projected from the wrist center 53 to the hand tip 54 on the XY plane. L1 = (Lw + L
t) · cos θp From FIGS. 13 and 14, x1 = x−L1 · sin θw y1 = y−L1 · cos θw z1 = Z− (Lw + Lt) · sinθp

Next, the shoulder center position 52 (x2, y
2, z2). FIG. 15 shows a side view of a shoulder / elbow shaft portion. x2 = Lh · sin θw y2 = Lh · cos θw z2 = Zh

Regarding the angles θs and θe of the shoulder axis and the elbow axis, the shoulder center 52 is used as the origin,
If the robot is considered to be a two-axis robot with an elbow axis, it can be solved by the same concept as a horizontal joint type robot. θs = θ1 θe = θ1 + θ2 where θ2 and θ1 are θ2 = θ2'-π θ1 = θ3 + θ4

[0011] From the shoulder center 52 to the wrist center 53
Is projected onto the XY plane, the length R1 can be expressed by the following equation.

[0012] R1 = √ ((x1-x2 ) 2 + (y1-y2) 2) R = √ (R1 2 + (z1-Zh) 2) cosθ2 than the cosine theorem ^{'= (Ls 2 + Le 2} -R 2) / 2 · Ls · Le θ2 ′ = cos ⁻¹ ((Ls ² + Le ² −R ² ) / 2 · Ls · L
^{e) θ3 = tan -1 ((} z1-Zh) / R1) θ4 = cos -1 ((Ls 2 + R 2 -Le 2) / 2 · Ls ·
R)

The shoulder angle 47 is obtained from the following equation.

[0014] θs = θ1 = θ3 + θ4 = tan -1 ((z1-Zh) / R1) + cos -1 ((Ls 2 + R 2 -Le 2) / 2 · Ls · R)

The elbow angle 48 is obtained from the following equation.

Θe = θ1 + θ2 = θ1 + (θ2′−π) = tan ⁻¹ ((z1−Zh) / R1) + cos ⁻¹ ((Ls ² + R ² −Le ² ) / 2 · Ls · R) + (cos ^-1 ((Ls ² + Le ² −R ² ) / 2 · Ls · Le) −π From the above calculation method, the waist angle 46, the shoulder angle 47,
Each of the elbow angle 48, the pitch angle 49, and the roll angle 50 is obtained.

[0017]

The roll angle output of the position sensor is ± 180 °.
When the position sensor is rotated by more than ± 180 °, the sign of the angle is reversed and the hand of the robot arm is rapidly reversely rotated. The waist axis of the robot arm is ± 100
Because it moves in the range of 異 な っ, the formula for calculating the waist angle differs depending on the position.

Further, when the waist angle is between 80 ° and 100 ° and between -80 ° and -100 °, there are two solutions at the front end of the hand when viewed from the shoulder center of the robot arm. Therefore, the waist angle cannot be specified by the conventional calculation method.

The calculation method of the shoulder angle must be changed between the case where the wrist center is the front and the case where the wrist center is the back as viewed from the shoulder center. Furthermore, in order to control the robot arm with the position and orientation information of the position sensor, error processing must be performed, such as limiting the input range of the position and orientation information, limiting the range of calculation angles, and limiting the operation range of the robot arm. There was a problem.

The present invention has been made to solve such a problem, and provides a control method of an articulated robot arm that can freely operate a robot arm based on position and orientation information of a position sensor. It is.

[0021]

According to a first aspect of the present invention, there is provided a method for controlling an articulated robot arm, comprising the steps of: detecting a position and orientation information of a movable body in a space by a position sensor;
A calculating step of calculating a joint angle of a multi-joint robot arm to be controlled based on the detected position and orientation information by a calculating means, and limiting an operation range when operating the multi-joint robot according to the position and orientation information Setting a value based on the joint angle.

A method for controlling an articulated robot arm according to a second aspect of the present invention includes a roll angle calculating step of calculating a roll angle from a predetermined angle threshold, a current roll angle, and a previous roll angle.

According to a third aspect of the present invention, there is provided a method for controlling an articulated robot arm, wherein the direction of the robot arm is determined based on a predetermined angle threshold value, the current hand tip position, and the previous determination of the waist angle, and the waist angle is calculated. It has an angle calculation step.

A control method for an articulated robot arm according to a fourth aspect of the present invention includes a shoulder angle calculating step of calculating a wrist center position and calculating a shoulder angle based on the wrist center position.

According to a fifth aspect of the invention, there is provided a method for controlling an articulated robot arm, wherein a hand tip position and a wrist tip position are calculated from the calculated joint angles so that the robot arm does not exceed the set safe operation range. Is set.

[0026]

DESCRIPTION OF THE PREFERRED EMBODIMENTS (1) Detailed Description of Embodiment Configuration Embodiment 1 FIG. 1 is an overall configuration diagram of an embodiment of a control method for a five-axis articulated robot arm according to the present invention. In FIG. 1, reference numeral 1 denotes a position sensor capable of detecting the position and orientation (distance in the X, Y, and Z directions, roll, pitch, and yaw angle) of, for example, a fingertip, an arm, and the like, and 2 controls the position sensor 1; A computer that sends position and orientation information to the control device, 3 receives a position and orientation information from the computer 2, and a control device that controls a robot arm to be controlled from the position and orientation information. The arithmetic unit 5 for calculating the joint angle is a five-axis articulated robot arm (hereinafter, referred to as a robot arm) whose operation is controlled by the control device 3 according to the calculated joint angle.

With the above configuration, the calculation unit 4 inputs the position and orientation information of the position sensor 1 to calculate the joint angle of the robot arm 5 and limits the range of the position and orientation information.
The robot arm 5 based on a certain angle threshold value and the present roll angle, the roll angle based on the result of the previous roll angle determination, the certain angle threshold value and the current hand tip position, the previous result on the waist angle determination
Is determined, the shoulder angle is calculated based on the waist angle and the calculated list center position, and the hand tip position and the list tip position are calculated from the calculated joint angles to exceed the operation range set by the robot arm 5. Limit the operating range so that there is no Note that the conditions of each axis and each arm length of the robot arm 5 are the same as those in the configuration of the related art.

(2) Detailed Description of Operation and Operation of the Embodiment Next, the operation of the first embodiment will be described with reference to FIGS. FIG. 2 is a diagram showing the movable range of the waist angle when the robot arm 5 is viewed from above, FIGS. 3 and 4 are diagrams showing the shoulder angle when the robot arm 5 is viewed from the side, and FIG. FIGS. 6 to 11 are flowcharts showing processing for calculating the joint angle by limiting the position and orientation information from the position sensor 1 to the operation range of the robot arm 5.

In the configuration shown in FIG.
Is attached to the operator's hand, and the robot arm 5 makes the same movement as the operator's arm. At this time, the position and orientation information is sent from the computer 2 to the calculation unit 4 of the control device 3, and the calculation unit 4 calculates the joint angle, and operates the robot arm 5 in the same manner as the operator's arm at this joint angle.

Next, the processing of the arithmetic unit 4 in the control device 3 will be described. First, in blocks 10 to 12 of the initial setting flowchart shown in FIG. 5, sensor input limit values (sensor X, Y, Z, sensor pitch angle, maximum value and minimum value of sensor roll angle), arm control angle limit value (The maximum value and the minimum value of the waist angle, the shoulder angle, the elbow angle, the roll angle, and the pitch angle), and the safe range limit values (the safe ranges X, Y, and Z of the arm operation) are initialized.

That is, in the block 10, the robot arm 5
The maximum and minimum values for the position sensor input are set from the maximum operation range of. This is determined by the length of each arm of the robot arm 5. In block 11, the maximum and minimum values of the control angle to the robot arm 5 are set. This is determined by the movable range of each axis of the robot arm 5. In block 12, a safe range in which the robot arm 5 operates safely is set. This is determined by the height of the robot arm 5 and the floor, the surrounding environment, and the like.

Next, the calculation processing of the joint angle will be described with reference to the initial setting flowcharts of FIGS. In block 13 in FIG. 6, the X coordinate and Y coordinate of the movable body input from the position sensor 1 to
It is checked whether the position and orientation information of the Z coordinate, the roll angle, and the pitch angle are within the range of the values set in blocks 10 to 12 in the calculation flowchart of FIG. Set to the maximum or minimum value of.

That is, it is determined whether or not the detected value of the sensor X is larger than the initially set sensor maximum value, and if it is larger, the value of the sensor X at that time is set as the sensor maximum value. But,
If smaller, it is determined whether the value of the sensor X is smaller than the initially set sensor minimum value. If smaller, the value of the sensor X at that time is set as the sensor minimum value.

The above processing is performed after the sensor X and the sensors Y,
This is also performed for Z. The sensor roll angle and the sensor pitch angle are also compared with the initially set maximum value and minimum value of each angle, and if each angle is outside the initially set range,
The sensor angle at that time is set to the initially set maximum value or minimum value.

Next, a method of calculating each joint angle and error processing will be described with reference to a calculation flowchart of FIG. Calculation of roll angle, error handling. The roll angle output of the position sensor is in the range of ± 180 °. Therefore, for example, from + 160 ° to +3
If it changes by 0 °, it is output as -170 ° instead of + 190 °. If this value is used as it is, robot arm 5
Is suddenly reversed.

The same phenomenon occurs when the angle exceeds -180 °. In order to prevent this, the block 14 uses a fact that the position and orientation information changes continuously, and compares a certain angle threshold value with the current roll angle and the previous roll angle. And +1
When it is determined that the angle has changed in the plus direction beyond 80 °, the roll angle is set to the maximum value (+ 180 °), and when it is determined that the angle has changed in the minus direction beyond −180 °, the roll angle is set to the minimum value (−180 °).゜).

Calculation of waist angle, error processing. Block 15 is a process when the robot arm 5 is on the Y axis. If it is on the Y axis, it is either + 90 ° or −90 ° but cannot be determined. Therefore, if the previous waist angle is plus, the waist angle is set to + 90 °, and if it is minus, −90 °. Block 16 is processing when the robot arm 5 is on the X axis. When it is on the X axis, the waist angle is set to 0 °.

The waist axis of the robot arm 5 is ± 10
Since the hand operates in the range of 0 °, when the hand tip is in front of the shoulder center, A, B, C,
There are four areas of D.

Also, when the tip of the hand is behind the shoulder center, there are four areas A, B, C and D. Therefore, when the tip of the hand is in the range of + 80 ° to + 100 ° and -80 ° to -100 °, there are two solutions of the waist angle to be obtained.

In blocks 17 to 20 in the calculation flowcharts of FIGS. 7 and 8, the position and orientation information is continuous, and the robot arm is compared by comparing the area where the tip of the hand is located with the previous waist angle and a certain angle threshold. 5
Judge the direction and determine the waist angle.

In block 17, the hand tip 54
Is in area A, and the previous waist angle was large.
If it is 5, the waist angle is tan ^-1 (sensor Y / sensor X) / PI, but if it is not in area A and the previous waist angle is not equal to -45, the area A and the previous waist angle are -45. It is determined whether it is small, and if so, the waist angle is tan ^-1 (sensor Y / sensor X)
Error processing of the waist angle area A is performed as / PI-180.

In block 18, the hand tip 54
Is in area B and the last time the waist angle was small 45
If so, the waist angle is set to tan ^-1 (sensor Y / sensor X) / PI, but if not in area B and the previous waist angle is not equal to -45, it is in area B and the previous waist angle is greater than 45 Is determined, and if so, the waist angle is set to tan ⁻¹ (sensor Y / sensor X) / PI + 180, and error processing of the waist angle area B is performed.

In block 19, if the tip of the hand is in area C and the previous waist angle is large and equal 45, the waist angle is set to tan ^-1 (sensor Y / sensor X) /
PI, however, if it is not in the area C and the previous waist angle is not equal to 45, it is determined whether it is in the area C and the previous waist angle is smaller than 45, and if so, the waist angle is set to tan ⁻¹ (sensor Y / Sensor X) / PI + 18
Error processing is performed on the waist angle area C as 0.

In block 20, if the tip of the hand is in the area D and the previous waist angle is equal to -45, the waist angle is tan ^-1 (sensor Y / sensor X).
/ PI, but if it is not in area D and the previous waist angle is not equal to ^minus 45, it is determined whether it is in area D and the previous waist angle is larger than -45, and if so, the waist angle is tan ^-1 ( Sensor Y / Sensor X) / PI
Error processing of the waist angle area C is performed as +180.

Block 21 in the calculation flowchart of FIG. 8 is an error process when the calculated waist angle exceeds the movable range of the waist axis. That is, when the calculated waist angle is out of the range of the initially set minimum value or maximum value, the waist angle is set to the set minimum value or maximum value.

Block 22 in the calculation flowchart of FIG. 9 is a process for setting the argument in the range of -1 to +1 so that no error occurs during the arc cosine calculation.

Calculation of shoulder angle, error processing: In calculation of the shoulder angle, the wrist center 53 of the robot arm 5 may be located behind the shoulder center 52 in some cases. FIG. 3 shows a shoulder angle when the wrist center 53 of the robot arm 5 is located behind the shoulder center 52, and FIG.
Shows the shoulder angle when the wrist center 53 of the robot arm 5 is located in front of the shoulder center 52.

In the case of FIG. 3 and the case of FIG. 4, the shoulder angle cannot be obtained by the same calculation formula depending on the position of the list center 53. In blocks 23 to 26 in the calculation flowcharts of FIGS. 9 and 10, the area at the tip of the hand and the area of the wrist center are determined to determine the shoulder angle. The definition of the area is the same as the waist angle in FIG.

In block 23, if R1 (displacement of the same vertical line between the shoulder center 52 and the wrist center 53) shown in FIG. 2 or 3 is 0, the shoulder angle is set to 90 + cos.
^-1 (ss) / PI, if not 0, hand tip 5
4 is in the area A, and it is determined whether the list center 53 is in the area A. If each of them is determined to be in the area A, the shoulder angle is set to tan ⁻¹ ((Z1
−Z2) / R1) + cos ⁻¹ (ss) / PI.

However, if it is not determined that there is, the hand tip 54 is in the area A, and the wrist center 5
It is determined whether 3 is in area C. If it is determined that there is, the shoulder angle is calculated as 90 + tan ^-1 ((Z1-Z2)
/ R1) + cos ^-1 (ss) / PI. Thus, the error processing of the hand tip area A is performed.

If it is not determined that the hand tip 54 is in the area A and the wrist center 53 is in the area C,
Hand tip 54 is in area B and wrist center 53
Is located in the area B. If determined, the shoulder angle is calculated as tan ⁻¹ ((Z1−Z2) / R1) + co
It is determined from s ⁻¹ (ss) / PI. If it is not determined that the hand tip 54 is in the area B and the wrist center 43 is in the area B, it is determined whether the hand tip 54 is in the area B and the wrist center 53 is in the area D. If it is determined, the shoulder angle is set to 90 + tan ^-1 ((Z1-
Z2) / R1) + cos ^-1 (ss) / PI. Thus, the error processing of the hand tip area B is performed.

If it is not determined that the hand tip 54 is in the area B and the wrist center 53 is in the area D, it is determined whether the hand tip 54 is in the area C and the wrist center 53 is in the area C. If it is determined, the shoulder angle is calculated as tan ^-1 ((Z1-Z2) / R1) + cos ^-1
(Ss) / PI.

If it is not determined that the hand tip 54 is in the area C and the wrist center 53 is in the area C, it is determined whether the hand tip 54 is in the area C and the wrist center 53 is in the area A. If it is determined, the shoulder angle is calculated as 90 + tan ^-1 ((Z1-Z2) / R1) + co
It is determined from s ⁻¹ (ss) / PI. Thus, the error processing of the hand tip area C is performed.

If it is not determined that the hand tip 54 is in the area C and the wrist center 53 is in the area A, it is determined whether the hand tip 54 is in the area D and the wrist center 53 is in the area D. If it is determined, the shoulder angle is calculated as tan ^-1 ((Z1-Z2) / R1) + cos ^-1
(Ss) / PI.

If it is not determined that the hand tip 54 is in the area D and the wrist center 53 is in the area D, it is determined whether the hand tip 54 is in the area D and the wrist center 53 are in the area B. If determined, the shoulder angle is 90
+ Tan ^-1 ((Z1-Z2) / R1) + cos
^-1 (ss) / PI. Thus, the error processing of the hand tip area D is performed.

In the calculation flowchart of FIG. 10, block 27 is a shoulder angle limit error process when the shoulder angle calculated value obtained in blocks 23 to 26 exceeds the movable range of the shoulder axis. The calculated shoulder angle value is initialized. If the shoulder angle exceeds the minimum or maximum value, the shoulder angle is set to the initially set minimum or maximum value. Block 28 is an elbow angle limit error process when the elbow angle calculation value exceeds the elbow axis movable range.
The elbow angle is calculated as (cos ^-1 (ee) -PI · 180) / P
After obtaining from I, the elbow angle limit error processing is performed as in block 27.

A block 29 is a pitch angle limit error process when the calculated pitch angle exceeds the movable range of the pitch axis. After the pitch angle is obtained from the sensor pitch angle−shoulder angle−elbow angle, as in block 27, Perform pitch angle limit error processing.

The calculation of the waist angle, the shoulder angle, the elbow angle, the pitch angle, and the roll angle is completed up to the above block 29. The tip position is
It may be different from the actual position of the position sensor. Further, there is a risk that the arm collides with the robot body or collides with the floor depending on the position of the position sensor.

Then, the forward motion is calculated from the calculated joint angle to obtain the hand tip position and the wrist center position.
The arm operates within a preset operation range. In the calculation flowchart of FIG.
At 0, the forward end position and the wrist center position are obtained from the joint angles by forward motion calculation.

First, the hand tip 54 is obtained from the shoulder center 52 shown in FIG.

L = Ls · cos (shoulder angle · PI) +
Le · cos ((shoulder angle + elbow angle) · PI) +
(Lw + Lt) · cos ((shoulder angle + elbow angle + pitch angle) · PI)

Next, the X and Y coordinate values of the hand tip 54 are obtained. The Z coordinate value of the hand tip 54 is obtained from the following equation.

Hand tip Z = Ls · sin (shoulder angle · PI) + Le · sin ((shoulder angle + elbow angle) ·
PI) + (Lw + Lt) · sin ((shoulder angle + elbow angle + pitch angle) · PI)

The list center 53 calculates from Ls · sin (shoulder angle · PI) + Le · sin ((shoulder angle + elbow angle) · PI).

Block 31 is a process for preventing the robot arm 5 from operating when the hand tip position exceeds the safe operation range or when the wrist center position collides with the floor.

That is, if it is determined that the X, Y, Z coordinate values of the hand tip 54 and the Z coordinate value of the wrist center 53 calculated in the block 30 are out of the respective safe ranges, the waist angle, The shoulder angle, elbow angle, roll angle, and pitch angle are the previous waist angle, shoulder angle, elbow angle, roll angle, and pitch angle. If not outside the safe range, the calculation is terminated.

(3) Description of another embodiment, description of an example of diverting to another use Embodiment 2 In the above embodiment, the calculation device calculates the joint angle for controlling the robot arm from the position and orientation information.However, the calculation is performed by the computer that controls the position sensor. The movement may be simulated.

Embodiment 3 Although the first embodiment is a method of controlling the five-axis articulated robot arm 5, it can be applied to the control of a six-axis articulated robot arm. In this case, although the calculation formulas are partially different, the concept of error processing is the same.

[0069]

According to the present invention as described above, the position and orientation information of the position sensor is subjected to various error processings by the arithmetic unit of the control device so as to control the robot arm. A position sensor is attached to the robot arm 5 to accurately perform the same movement as that of the pilot's arm.
This has the effect of being able to be reflected in

[Brief description of the drawings]

FIG. 1 is an overall configuration diagram of an embodiment of a control method of an articulated robot arm according to the present invention.

FIG. 2 is a diagram showing a waist angle movable range according to the present invention.

FIG. 3 is a diagram showing calculation of a shoulder angle (pattern 1) according to the present invention.

FIG. 4 is a diagram showing calculation of a shoulder angle (pattern 2) according to the present invention.

FIG. 5 is a flowchart showing a calculation initial setting process according to the present invention.

FIG. 6 is a flowchart showing a calculation process according to the present invention.

FIG. 7 is a flowchart showing a calculation process according to the present invention.

FIG. 8 is a flowchart showing a calculation process according to the present invention.

FIG. 9 is a flowchart showing a calculation process according to the present invention.

FIG. 10 is a flowchart showing a calculation process according to the present invention.

FIG. 11 is a flowchart showing a calculation process according to the present invention.

FIG. 12 is a side view of a robot arm for explaining a conventional analysis.

FIG. 13 is a top view of a corner portion of a wrist in a robot arm for explaining a conventional analysis.

FIG. 14 is a side view of a wrist corner of a robot arm for explaining a conventional analysis.

FIG. 15 is a side view of a shoulder / elbow corner of a robot arm for explaining a conventional analysis.

[Explanation of symbols]

1 position sensor, 2 computer, 3 control device, 4 arithmetic unit, 5 5-axis articulated robot arm,
10 Sensor input limit value processing, 11 Arm control angle limit value processing, 12 Safe range limit value processing,
13 Sensor input limit error processing, 14 Roll angle error processing, 15 Y axis processing, 16 X axis processing, 17 Waist angle area A error processing, 1
8 Waist angle area B error processing, 19 Waist angle area C error processing, 20 Waist angle area D error processing, 21 Waist angle limit error processing,
22 cos ^-1 operation error processing, 23 hand tip area A error processing, 24 hand tip area B
Error processing, 25 Hand tip area C error processing,
26 Hand tip area D error processing, 27 Shoulder angle limit error processing, 28 Elbow angle limit error processing, 29 Pitch angle limit error processing,
30 Hand tip position calculation processing, 31 Operation range error processing, 46 θw: Waist angle, 47 θs
: Shoulder angle, 48 θe: Elbow angle, 49 θp
: Pitch angle, 50 θr: Roll angle, 52 Shoulder center, 53 Wrist center, 54 Hand tip, 55 Horizontal plane, 60L1.

 ──────────────────────────────────────────────────続 き Continued on the front page F term (reference) 3F059 BA04 BC01 CA08 EA01 FC13 3F061 DD00

Claims (5)

[Claims] 1. A detecting step of detecting position and orientation information of a movable body in a space by a position sensor, and calculating a joint angle of a multi-joint robot arm to be controlled based on the detected position and orientation information by a calculating means. A method for controlling an articulated robot arm, comprising: a calculating step; and a setting step of setting an operation range limit value for operating the articulated robot in accordance with the position and orientation information based on the joint angle. 2. The control of an articulated robot arm according to claim 1, further comprising a roll angle calculating step of calculating a roll angle from a predetermined angle threshold, a current roll angle, and a previous roll angle. Method. 3. A waist angle calculating step of determining a direction of the robot arm from a predetermined angle threshold value, a current hand tip position, and a previous waist angle, and calculating a waist angle. 3. The method for controlling an articulated robot arm according to item 1. 4. The control method for an articulated robot arm according to claim 1, further comprising a shoulder angle calculation step of calculating a wrist center position and calculating a shoulder angle from the wrist center position. 5. A setting step of calculating a hand tip position and a wrist tip position from the calculated joint angles and limiting an operation range so as not to exceed a safe operation range set by the robot arm. Item 2. The method for controlling an articulated robot arm according to Item 1.