JP5770067B2 - Robot arm - Google Patents

Description

  The present invention relates to a robot arm having redundancy.

  As a control method of the robot arm, a method for determining the driving amount of each joint angle using a Jacobian matrix by specifying an element for determining redundancy in addition to six elements for determining the position and posture is proposed ( Patent Document 1). According to this technique, the amount of change in the load applied to the drive source of each joint mechanism is reduced by designating “0” as the amount of change in the inertia ratio selected as an element for determining the redundancy.

Japanese Patent Publication No. 04-006003

  However, when the robot arm is supported by an unstable pedestal whose position or posture easily changes, when trying to make the position and posture of the designated position of the robot arm follow the target trajectory according to the control method, While the change of the load applied to each joint mechanism drive source can be suppressed, the operation of the robot arm may become unstable.

  Therefore, the present invention solves the problem of providing a robot arm that can stabilize the operation while keeping the position and posture of the designated location following the target trajectory even in a situation where the robot is supported by an unstable pedestal. Let it be an issue.

A robot arm according to the present invention for solving the above-described problem includes a plurality of joint mechanisms and a control device that controls each joint angle so that each joint angle changes according to a command value, and the position and posture change. It is a robot arm having redundancy supported by a pedestal that can be . The control device performs minute changes in the positions and postures of the designated portions with respect to minute changes in angles of the joint mechanisms in order to designate six elements that determine the amount of change in the positions and postures of the designated portions of the robot arm. Calculating the first command value by multiplying the pseudo inverse matrix of the first Jacobian matrix expressed by the minute change in the position and orientation of the designated location, and the position of the entire center of gravity of the robot arm as an element for determining the redundancy In order to specify a change amount of 0 as a value within a specified range, a pseudo inverse matrix of a second Jacobian matrix representing a minute change in the overall center of gravity position of the robot arm with respect to a minute change in the angle of the plurality of joint mechanisms And a result obtained by multiplying a small change in the center of gravity position of the robot arm calculated based on the first command value according to the forward kinematics model of the robot arm. Is calculated on the null space of the first Jacobian matrix, and the command value is the sum of the first command value and the result obtained by multiplying the second command value by the designated coefficient r. The designated coefficient r is a value in the range of 0 <r <1 .

  In the robot arm of the present invention, it is preferable that the designation coefficient r is a value that decreases as the sampling period increases, and increases as the sampling period decreases.

The structure explanatory view of the robot arm as one embodiment of the present invention. FIG. 2 is a configuration explanatory diagram of a control device for the robot arm of FIG. Explanatory drawing regarding the operation | movement form of a robot arm.

(Robot arm configuration)
A robot arm 1 as an embodiment of the present invention shown in FIG. 1 is supported by a base 2. The pedestal 2 is constituted by a part of a moving device such as a cart that can be remotely operated, or a lifter attached to the cart. The pedestal 2 or a device such as a moving device including the pedestal 2 may be a component of the robot arm 1. An end effector 3 is attached to the tip of the robot arm 1. The end effector 3 has an appropriate structure for engaging with a handle of the valve, for example, in order to execute a task of opening / closing the valve. The end effector 3 may be a component of the robot arm 1.

The robot arm 1 is connected to nine joint mechanisms J _i (i = Bz, By, Sz, Sy, Sx, Ey, Wx, Wy, Wz) and the nine joint mechanisms J _i in turn. Link member L _k (k = 1 to 9). That is, the degree of freedom q of the robot arm 1 of the present embodiment is “9”, and the redundancy p thereof is “3 (= 9−q)”. Each joint mechanism J _i is provided with a joint angle sensor such as a rotary encoder that outputs a signal corresponding to the joint angle θ _i .

  An imaging device is attached to at least one of the robot arm 1, the pedestal 2, and the end effector 3, and an image captured by the imaging device is displayed on an image device installed at a location away from the robot arm 1. . The operator can remotely control the operation of the robot arm 1 and the end effector 3 in addition to the operation of the moving device constituting the pedestal 2 by operating the remote operation device while viewing this image.

(Configuration of control device)
The robot arm 1 includes a control device 4 shown in FIG. The control device 4 is configured by a programmable computer. The target motion trajectory of the arm 1 is input to the control device 4 from the remote control device. The “trajectory” of a variable means a time-series variable value representing a time change mode of the variable. The control device 4 includes a first command value calculation element 41 and a second command value calculation element 42.

A signal output from the joint angle sensor is input to the control device 4. The control device 4 is configured to execute a control process of controlling the operation of the drive source of each joint mechanism J _i according to the operation control command of each joint mechanism J _i of the arm 1 based on the input signal. Yes.

  Here, the control device 4 is configured to execute arithmetic processing when the CPU (central processing processing) constituting the control device 4 reads necessary software and data from a memory (storage device), and It means that it is programmed to execute the arithmetic processing according to software.

(Control processing)
The control process of the robot arm 1 executed by the control device 4 having the above configuration will be described.

Deviation between the current target value Q _H _des and the previous command value Q _H _cmd_pre of the position and posture (hereinafter referred to as “hand position / posture”) of the tip, which is the designated portion of the robot arm 1, by the first command value calculation element 41 Based on the above, the first command value Δθ1_cmd is calculated according to the relational expression (01). The first command value Δθ1_cmd matches the hand position / posture Q _H = (P _H , θ _H ) = (x _H , y _H , z _H , θ _Hx , θ _Hy , θ _Hz ) with the current target value Q _H _des The amount of change Δθ = (Δθ _Bz , Δθ _By , Δθ _Sz , Δθ _Sy , Δθ _Sx , Δθ _Ey , Δθ _Wx , Δθ _Wy , Δθ _Wz ) for each joint mechanism J _i is determined. That is, the first command value Δθ1_cmd is for designating x _H , y _H , z _H , θ _Hx , θ _Hy and θ _Hz that are the six elements that determine the amount of change in the hand position / posture Q _H.

The previous command value Q _H _cmd_pre of the hand position / posture is calculated according to the forward kinematics model of the robot arm 1 based on the previous command value θ_cmd_pre as a result of accumulating the change command values Δθ_cmd up to the previous time of each joint angle. .

Δθ1_cmd = J _H ^# (Q _H _des-Q _H _cmd_pre) .. (01).

“ ^# ” Represents a pseudo inverse matrix. “J _H ” is a hand for a minute change Δθ = (Δθ _Bz , Δθ _By , Δθ _Sz , Δθ _Sy , Δθ _Sx , Δθ _Ey , Δθ _Wx , Δθ _Wy , Δθ _Wz ) of each joint mechanism J _i. it is a Jacobian matrix representing the minute change Delta] Q _H of the position and orientation Q _H (first Jacobian). The Jacobian matrix J _H and its pseudo inverse matrix J _H ^# are calculated based on the previous command value θ_cmd_pre for each joint angle and the previous command value Q _H _cmd_pre for the hand position / posture.

Based on the first command value Δθ1_cmd, the second command value calculation element 42 calculates the estimated change amount ΔP _gc _est of the total center-of-gravity position according to the forward kinematic model. Then, the second command value Δθ2_cmd is calculated according to the relational expression (02) based on the estimated change amount ΔP _gc _est of the overall center of gravity position. The second command value Δθ2_cmd is used to maintain the overall center of gravity position P _gc = (x _gc , y _gc , z _gc ) of the robot arm 1 so as not to change the current target value Q _H _des of the hand position / posture. The amount of change in the joint angle of the mechanism J _i is determined. That is. The second command value Δθ2_cmd is for designating the change amount of the overall center of gravity position P _gc of the robot arm 1 as an element for determining the redundancy p (= 3) to 0 or a value within the designated range.

^{_{Δθ2_cmd = (EJ H # J H}} ) J gc # ΔP gc _est .. (02).

"J _gc" is a relative small change Δθ in the joint angle of each joint mechanism J _i, Jacobian matrix representing the minute change [Delta] P _H of the overall center-of-gravity position P _gc (second Jacobian). The Jacobian matrix J _gc and its pseudo inverse matrix J _gc ^# are calculated based on the previous command value θ_cmd_pre for each joint angle and the previous command value P _gc _cmd_pre for the entire center of gravity position. “EJ _H ^# J _H ” means a null space of the Jacob matrix J _H.

The sum of the first command value Δθ1_cmd and the second command value Δθ2_cmd is calculated as the command value Δθ_cmd. In accordance with this command value Δθ_cmd, the operation of the drive source is controlled so that the joint angle θ _i of each joint mechanism J _i changes by Δθ_cmd_i .
The sum of the first command value Δθ1_cmd and the result obtained by multiplying the second command value Δθ2_cmd by a predetermined ratio r (0 <r <1) may be calculated as the command value Δθ_cmd. Based on the time constant T and the sampling period τ, the ratio is expressed by the relational expression (03).

  r = 1 / (1+ (τ / T)) .. (03).

According to the present invention, even when the robot arm 1 is supported by the unstable pedestal 2, the overall center of gravity position P of the robot arm 1 is maintained while the hand position / posture Q _H follows the target value Q _H _des. _Since the change of _gc is prevented or restricted, the operation of the robot arm 1 is stabilized.

For example, consider a situation where the pedestal 2 is unstable such that the movement of the pedestal 2 is represented by a model in contact with the floor via an elastic or spring element. In this situation, and FIG. 3 (b) the hand position and orientation Q _H as shown in the result of the follow the target value Q _H _des, if the overall center-of-gravity positions P _gc of the robot arm 1 is changed, the elastic element The position and posture of the pedestal 2 change due to the deformation. When each joint angle θ is controlled to compensate for the position and orientation change of the pedestal 2, at least a part of the joint angle θ may be oscillated, and the operation of the robot arm 1 may become unstable. .

However, according to the present invention, in this situation, even while tracking the tip position and orientation Q _H as shown in the target value Q _H _des in FIG. 3 (a), the overall center-of-gravity position P of the robot arm 1 Changes in _gc are prevented or restricted. For this reason, the change of the position and attitude of the base 2 due to the deformation of the elastic element is prevented or restricted, and the operation of the robot arm 1 is stably controlled.

Since the redundancy p of the robot arm 1 is “3”, the overall center-of-gravity position P _gc of the robot arm 1 can be adjusted in three orthogonal directions. A drive mechanism (XY stage or the like) that displaces the rear end (base end) of the robot arm 1 with respect to the base 2 is provided, and the three-axis directions of the total center of gravity position P _gc in the base coordinate system of the robot arm 1 are provided. When some of the components can be controlled by the drive mechanism, the total number of joint mechanisms J _i is set to “7” or “8” so that the redundancy p of the robot arm 1 is “1” or “2”. It may be changed.

1 ‥ robot arm, 2 ‥ pedestal, 3 ‥ end effector, 4 ‥ controller, J ₁ ~J ₉ ‥ joint mechanism.

Claims (2)

A robot arm having a plurality of joint mechanisms and a control unit that controls each joint angle so that each joint angle changes according to a command value, and is supported by a base that can change its position and posture. Because
The controller is
A first Jacobian matrix representing a minute change in the position and orientation of the designated portion with respect to a minute change in angle of the plurality of joint mechanisms in order to designate six elements that define the amount of change in the position and posture of the designated position of the robot arm The first command value is calculated by multiplying the pseudo inverse matrix of
In order to designate the amount of change in the overall center of gravity position of the robot arm as an element for determining the redundancy as 0 or a value within a designated range, the overall center of gravity of the robot arm with respect to minute changes in the angles of the plurality of joint mechanisms A pseudo inverse matrix of the second Jacobian matrix representing a minute change in position is multiplied by a minute change in the center of gravity position of the robot arm calculated based on the first command value according to the forward kinematics model of the robot arm. Calculating a second command value by projecting the result onto the null space of the first Jacobian matrix; and
A sum of a result of multiplying the first command value and the second command value by a designated coefficient r is calculated as the command value ;
The robot arm according to claim 1, wherein the designation coefficient r is a value in a range of 0 <r <1 .   The robot arm according to claim 1,
  The robot arm according to claim 1, wherein the specified coefficient r is a value that decreases as the sampling period increases and increases as the sampling period decreases.

Images