JP6595206B2 - Multi-joint arm mechanism operation control apparatus and robot apparatus - Google Patents

Description

  Embodiments described herein relate generally to an operation control apparatus and a robot apparatus for an articulated arm mechanism.

  In recent years, an environment in which a robot is in the same space as a user has increased. It is expected that the situation of working alongside workers with industrial robots as well as nursing robots will expand in the future. Many such robots have a vertical articulated arm mechanism. The vertical articulated arm mechanism is required to have three degrees of freedom (x, y, z) with respect to position and three degrees of freedom (α, β, γ) with respect to the posture. This is realized by J2 and J3 and rotary joint portions J4, J5 and J6 called three wrist axes. For example, a torsional joint is applied to the joints J1, J4, and J6, and a bending joint is applied to the joints J2, J3, and J5. A tip effector (end effector) such as a hand is provided at the tip of the arm.

  There are various methods for controlling the movement of the hand position and posture. As an example, the trajectory from the current position to the final target position is calculated, and the unit time (control cycle) such as 10 ms on the trajectory is calculated. Find individual target positions.

  Algebraic or geometrical solutions are preferred in real-time control to obtain the joint angles of multiple joints from the target position by inverse kinematics using a homogeneous transformation matrix, but the arm mechanisms to which it can be applied are limited. In recent years, the Jacobian inverse matrix method with few such restrictions is generally used.

  In this method, the joint angular velocities of each of the plurality of joints are calculated from the target position using the Jacobian inverse matrix, and the joint angular velocities are output to the motor driver as command values. The motor driver drives the actuator (motor) using the speed of each joint as a target value.

  In this method, the actual hand position may deviate from the calculated position due to an inertia error or the like accompanying the start of movement or speed change, and feedback control is often employed to correct the error.

  However, in the feedback control, the correction operation is followed, and the error correction of the position is added to the next command value. Therefore, the position error cannot be completely eliminated, and the moving speed becomes unstable.

  The purpose is to eliminate or reduce the position error in the movement control of the robot arm.

The motion control device for a multi-joint arm mechanism according to the present embodiment outputs a plurality of command values to a plurality of motor drivers respectively corresponding to actuators of a plurality of joint portions constituting the multi-joint arm mechanism. In the command value calculation process, first, the current position calculation unit calculates the current position of the point of interest on the multi-joint arm mechanism from the current plurality of joint variables respectively corresponding to the plurality of joint parts. The trajectory calculation unit calculates the trajectory of the point of interest from the current position to the final target position. A target time width required for the point of interest to move from the current position to the final target position and a control period for controlling the motor driver are set by the time setting unit in accordance with a user instruction. The target position calculation unit calculates a plurality of target positions on the trajectory calculated based on the set target time width and the set control cycle. The moving speed calculation unit calculates the moving speed of the target point between the target positions calculated based on the calculated distance between the target positions and the control cycle . The calculated moving speed is converted into a plurality of joint speeds respectively corresponding to the plurality of joint parts by the joint speed conversion unit by a Jacobian inverse matrix corresponding to the structure of the multi-joint arm mechanism. The amount of displacement of each of the plurality of joints in the control cycle is calculated by the change amount calculation unit for each target position from the calculated plurality of joint velocities . By adding the displacement amount of each of the plurality of joints to the joint variable of each of the plurality of joints corresponding to each target position, the joint variable of each of the plurality of joints is set as the target value of each of the plurality of joints. The command value calculation unit calculates each position . It forces out command value output unit for each control cycle the instruction value of each of the plurality of joint portions in a plurality of motor drivers.

FIG. 1 is an external perspective view of a robot arm mechanism of the robot apparatus according to the present embodiment. FIG. 2 is a perspective view showing the internal structure of the robot arm mechanism of FIG. FIG. 3 is a diagram showing the robot arm mechanism of FIG. FIG. 4 is a block diagram of the robot apparatus according to the present embodiment. FIG. 5 is a flowchart of operation control by the operation control apparatus of FIG. FIG. 6 is a flowchart of step S14 in FIG. FIG. 7 is a diagram showing a temporal change in the joint angle of the first joint portion in order to explain the effect of the operation control (position command) of FIG. 5 in comparison with the case of the speed command.

  Hereinafter, the robot apparatus according to the present embodiment will be described with reference to the drawings. The robot apparatus according to this embodiment includes an operation control apparatus. The motion control device can be incorporated into a robot device having a multi-joint arm mechanism as a single device that controls a motor driver of each joint of the multi-joint arm mechanism equipped in the robot device. In the present embodiment, a robot apparatus having a multi-joint arm mechanism in which one of a plurality of joint portions is a linear motion telescopic joint will be described. In the following description, components having substantially the same function and configuration are denoted by the same reference numerals, and redundant description will be given only when necessary.

  FIG. 1 is an external perspective view of a robot arm mechanism 200 of the robot apparatus according to the present embodiment. The robot arm mechanism 200 includes a substantially cylindrical base 1, an arm 2 connected to the base 1, and a wrist 4 attached to the tip of the arm 2. The wrist portion 4 is provided with an adapter (not shown). For example, the adapter is provided in a rotating portion of a sixth rotating shaft RA6 described later. A robot hand corresponding to the application is attached to the adapter provided on the wrist portion 4.

  The robot arm mechanism 200 has a plurality of joint portions J1, J2, J3, J4, J5, and J6 in this case. The plurality of joint portions J1, J2, J3, J4, J5, and J6 are sequentially arranged from the base portion 1. In general, the first, second, and third joints J1, J2, and J3 are called the root three axes, and the fourth, fifth, and sixth joints J4, J5, and J6 change the posture of the robot hand 3. Called wrist 3 axis. The wrist 4 has fourth, fifth, and sixth joints J4, J5, and J6. At least one of the joint portions J1, J2, and J3 constituting the base three axes is a linear motion expansion / contraction joint. Here, the third joint portion J3 is configured as a linear motion expansion / contraction joint portion, particularly a joint portion having a relatively long expansion / contraction distance. The arm part 2 represents the expansion / contraction part of the linear motion expansion / contraction joint part J3 (third joint part J3).

  The first joint portion J1 is a torsion joint centered on the first rotation axis RA1 that is supported, for example, perpendicularly to the base surface. The second joint portion J2 is a bending joint centered on the second rotation axis RA2 arranged perpendicular to the first rotation axis RA1. The third joint portion J3 is a joint in which the arm portion 2 expands and contracts linearly around a third axis (moving axis) RA3 arranged perpendicular to the second rotation axis RA2.

  The fourth joint portion J4 is a torsion joint centered on the fourth rotation axis RA4. The fourth rotation axis RA4 substantially coincides with the third movement axis RA3 when a later-described seventh joint portion J7 is not rotating, that is, when the entire arm portion 2 is linear. The fifth joint J5 is a bending joint centered on a fifth rotation axis RA5 orthogonal to the fourth rotation axis RA4. The sixth joint portion J6 is a bending joint centered on the sixth rotation axis RA6 that is perpendicular to the fourth rotation axis RA4 and perpendicular to the fifth rotation axis RA5.

  The arm support body (first support body) 11a that forms the base portion 1 has a cylindrical hollow structure formed around the first rotation axis RA1 of the first joint portion J1. The first joint portion J1 is attached to a fixed base (not shown). When the first joint portion J1 rotates, the arm portion 2 pivots left and right along with the shaft rotation of the first support 11a. The first support 11a may be fixed to the ground plane. In that case, the arm part 2 is provided in a structure that turns independently of the first support 11a. A second support part 11b is connected to the upper part of the first support 11a.

  The second support portion 11b has a hollow structure that is continuous with the first support portion 11a. One end of the second support portion 11b is attached to the rotating portion of the first joint portion J1. The other end of the second support portion 11b is opened, and the third support portion 11c is fitted so as to be rotatable on the second rotation axis RA2 of the second joint portion J2. The 3rd support part 11c has a hollow structure which consists of a scale-like exterior which is connected to the 1st support part 11a and the 2nd support part. The third support portion 11c is accommodated in the second support portion 11b and sent out as the second joint portion J2 is bent and rotated. The rear part of the arm part 2 that constitutes the linear motion expansion / contraction joint part J3 (third joint part J3) of the robot arm mechanism 200 is housed in the hollow structure in which the first support part 11a and the second support part 11b are continuous by contraction. Is done.

  The third support portion 11c is fitted to the lower end portion of the second support portion 11b so as to be rotatable about the second rotation axis RA2 with respect to the open end lower portion of the second support portion 11b. Thereby, a second joint portion J2 as a bending joint portion around the second rotation axis RA2 is configured. When the second joint portion J2 rotates, the arm portion 2 rotates in a vertical direction around the second rotation axis RA2, that is, performs a undulation operation.

  The fourth joint portion J4 is a torsional joint having an arm central axis along the expansion / contraction direction of the arm portion 2, that is, a fourth rotation axis RA4 that typically contacts the third movement axis RA3 of the third joint portion J3. When the fourth joint portion J4 rotates, the wrist portion 4 and the robot hand attached to the wrist portion 4 rotate about the fourth rotation axis RA4. The fifth joint J5 is a bending joint having a fifth rotation axis RA5 orthogonal to the fourth rotation axis RA4 of the fourth joint J4. When the fifth joint portion J5 rotates, the fifth joint portion J5 rotates up and down (vertical direction around the fifth rotation axis RA5) together with the robot hand from the fifth joint portion J5 to the tip. The sixth joint J6 is a bending joint having a sixth rotation axis RA6 perpendicular to the fourth rotation axis RA4 of the fourth joint J4 and perpendicular to the fifth rotation axis RA5 of the fifth joint J5. When the sixth joint portion J6 rotates, the robot hand turns left and right.

  As described above, the robot hand attached to the adapter of the wrist portion 4 includes the first, second, and third joint portions J1. J2. It is moved to an arbitrary position by J3, and is arranged in an arbitrary posture by the fourth, fifth, and sixth joint portions J4, J5, and J6. In particular, the length of the extension / contraction distance of the arm portion 2 of the third joint portion J3 enables the robot hand to reach a wide range of objects from the proximity position of the base portion 1 to the remote position. The third joint portion J3 is characterized by a linear expansion / contraction operation realized by a linear motion expansion / contraction mechanism constituting the third joint portion J3 and the length of the expansion / contraction distance.

(Description of internal structure)
FIG. 2 is a perspective view showing the internal structure of the robot arm mechanism 200 of FIG. The linear motion expansion / contraction mechanism has an arm part 2 and an injection part 30. The arm unit 2 includes a first connection frame row 21 and a second connection frame row 22. The first connected frame row 21 includes a plurality of first connected frames 23. The first connection piece 23 is formed in a substantially flat plate shape. The front and rear first connecting pieces 23 are connected in a row so as to be freely bent by pins at the end portions of each other. The 1st connection top row | line | column 21 can be bent freely inside and outside.

  The second linked frame row 22 includes a plurality of second linked frames 24. The second connecting piece 24 is configured as a short groove having a U-shaped cross section. The front and rear second connecting pieces 24 are connected in a row so as to be freely bent by pins at the bottom end portions of each other. The second connecting frame row 22 can be bent inward. Since the cross section of the second connecting piece 24 is U-shaped, the second connecting piece row 22 does not bend outward because the side plates of the adjacent second connecting pieces 24 collide with each other. The surfaces of the first and second connecting pieces 23 and 24 facing the second rotation axis RA2 are referred to as inner surfaces, and the opposite surfaces are referred to as outer surfaces. The first first linked frame 23 in the first linked frame sequence 21 and the first second linked frame 24 in the second linked frame sequence 22 are connected by a linked frame 27. For example, the connecting piece 27 has a shape in which the second connecting piece 24 and the first connecting piece 23 are combined.

  The injection unit 30 includes a plurality of upper rollers 31 and a plurality of lower rollers 32 supported by a rectangular tube-shaped frame 35. For example, the plurality of upper rollers 31 are arranged along the arm central axis at an interval substantially equal to the length of the first connecting piece 23. Similarly, the plurality of lower rollers 32 are arranged along the arm central axis at an interval substantially equivalent to the length of the second connecting piece 24. A guide roller 40 and a drive gear 50 are provided behind the injection unit 30 so as to face each other with the first connecting piece row 21 interposed therebetween. The drive gear 50 is connected to the motor 55 via a speed reducer (not shown). A linear gear is formed on the inner surface of the first connecting piece 23 along the connecting direction. When the plurality of first connecting pieces 23 are arranged in a straight line, the linear gears are connected in a straight line to form a long linear gear. The drive gear 50 is meshed with a linear linear gear. The linear gear connected in a straight line forms a rack and pinion mechanism together with the drive gear 50.

(Description of bonding structure)
When the arm is extended and the motor 55 is driven and the drive gear 50 rotates forward, the first connecting piece row 21 is brought into a posture parallel to the arm central axis by the guide roller 40, so that the upper roller 31 and the lower roller 32 are in contact with each other. Guided in between. Along with the movement of the first connection piece row 21, the second connection piece row 22 is guided between the upper roller 31 and the lower roller 32 of the injection unit 30 by a guide rail (not shown) disposed behind the injection unit 30. . The first and second connecting frame rows 21 and 22 guided between the upper roller 31 and the lower roller 32 are pressed against each other. Thereby, the columnar body by the 1st, 2nd connection top row | line | columns 21 and 22 is comprised. The injection unit 30 joins the first and second connecting frame rows 21 and 22 to form a columnar body, and supports the columnar body vertically and horizontally. The columnar body formed by joining the first and second connecting piece rows 21 and 22 is firmly held by the injection unit 30, so that the joining state of the first and second connecting piece rows 21 and 22 is maintained. When the joined state of the first and second connection frame rows 21 and 22 is maintained, the bending of the first and second connection frame rows 21 and 22 is constrained to each other. Thereby, the 1st, 2nd connection top row | line | columns 21 and 22 comprise the columnar body provided with fixed rigidity. The columnar body refers to a columnar rod body in which the first connection frame row 21 is joined to the second connection frame row 22. In this columnar body, the second connecting piece 24 and the first connecting piece 23 are formed into cylindrical bodies having various cross-sectional shapes as a whole. The cylindrical body is defined as a shape in which the top, bottom, left, and right sides are surrounded by a top plate, a bottom plate, and both side plates, and a front end portion and a rear end portion are opened. The columnar body formed by joining the first and second connecting piece rows 21 and 22 starts from the connecting piece 27 and linearly extends from the opening of the third support portion 11c along the third movement axis RA3. Sent out.

  When the arm 55 is contracted and the motor 55 is driven and the drive gear 50 is rotated in the reverse direction, the first connecting piece row 21 engaged with the drive gear 50 is pulled back into the first support 11a. The columnar body is pulled back into the third support body 11c with the movement of the first connection frame row. The columnar body pulled back is separated behind the injection unit 30. For example, the first connecting piece row 21 constituting the columnar body is sandwiched between the guide roller 40 and the drive gear 50, and the second connecting piece row 22 constituting the columnar body is pulled downward by gravity, whereby the second connecting piece row 22 is drawn. The frame row 22 and the first linked frame row 21 are separated from each other. The separated first and second connecting frame rows 21 and 22 are returned to a bendable state, individually bent, and stored in the first support 11a.

(Definition of coordinate system)
FIG. 3 is a diagram showing the robot arm mechanism 200 of FIG. In the robot arm mechanism 200, three position degrees of freedom are realized by the first joint portion J1, the second joint portion J2, and the third joint portion J3 that form the three base axes. In addition, three posture degrees of freedom are realized by the fourth joint portion J4, the fifth joint portion J5, and the sixth joint portion J6 constituting the wrist three axes.

  The first joint portion J1 is disposed between the first support portion 11a and the second support portion 11b, and is configured as a torsion joint with the rotation axis RA1 as the center. The rotation axis RA1 is arranged perpendicular to the reference plane BP of the base on which the fixing portion of the first joint portion J1 is installed. A Z axis is defined parallel to the rotation axis RA1. An orthogonal three-axis robot coordinate system Σb (Xb, Yb, Zb) centering on the Z axis is defined.

  The 2nd joint part J2 is comprised as a bending joint centering on rotating shaft RA2. The rotation axis RA2 of the second joint portion J2 is provided in parallel to the Xb axis on the spatial coordinate system. The rotation axis RA2 of the second joint portion J2 is provided in a direction perpendicular to the rotation axis RA1 of the first joint portion J1. Further, the second joint portion J2 is offset with respect to the first joint portion J1 in two directions, that is, the direction of the first rotation axis RA1 (Zb axis direction) and the Yb axis direction perpendicular to the first rotation axis RA1. The second support 11b is attached to the first support 11a so that the second joint J2 is offset in the two directions with respect to the first joint J1. A virtual arm rod portion (link portion) that connects the second joint portion J2 to the first joint portion J1 has a crank shape in which two hook-shaped bodies whose tips are bent at right angles are combined. This virtual arm rod part is comprised by the 1st, 2nd support bodies 11a and 11b which have a hollow structure.

  The third joint portion J3 is configured as a linear motion telescopic joint with the movement axis RA3 as the center. The movement axis RA3 of the third joint portion J3 is provided in a direction perpendicular to the rotation axis RA2 of the second joint portion J2. In the reference posture in which the rotation angle of the second joint portion J2 is zero degrees, that is, the undulation angle of the arm portion 2 is zero degrees and the arm portion 2 is horizontal, the movement axis RA3 of the third joint portion J3 is the second joint The rotation axis RA2 of the part J2 and the rotation axis RA1 of the first joint part J1 are provided in a direction perpendicular to the rotation axis RA2. On the spatial coordinate system, the movement axis RA3 of the third joint portion J3 is provided in parallel to the Yb axis perpendicular to the Xb axis and the Zb axis. Further, the third joint portion J3 is offset with respect to the second joint portion J2 in two directions, that is, the direction of the rotation axis RA2 (Yb axis direction) and the direction of the Zb axis orthogonal to the movement axis RA3. The third support 11c is attached to the second support 11b so that the third joint J3 is offset in the two directions with respect to the second joint J2. The virtual arm rod portion (link portion) that connects the third joint portion J3 to the second joint portion J2 has a hook-shaped body whose tip is bent vertically. This virtual arm rod portion is constituted by the second and third supports 11b and 11c.

The fourth joint portion J4 is configured as a torsion joint with the rotation axis RA4 as the center. The rotation axis RA4 of the fourth joint part J4 is arranged to substantially coincide with the movement axis RA3 of the third joint part J3.
The fifth joint J5 is configured as a bending joint with the rotation axis RA5 as the center. The rotation axis RA5 of the fifth joint portion J5 is disposed so as to be substantially orthogonal to the movement axis RA3 of the third joint portion J3 and the rotation axis RA4 of the fourth joint portion J4.
The sixth joint portion J6 is configured as a torsion joint with the rotation axis RA6 as the center. The rotation axis RA6 of the sixth joint portion J6 is disposed so as to be substantially orthogonal to the rotation axis RA4 of the fourth joint portion J4 and the rotation axis RA5 of the fifth joint portion J5. The sixth joint J6 is provided to turn the robot hand as a hand effector left and right. The sixth joint portion J6 may be configured as a bending joint whose rotation axis RA6 is substantially orthogonal to the rotation axis RA4 of the fourth joint portion J4 and the rotation axis RA5 of the fifth joint portion J5.

  In this way, one bending joint portion of the base three axes of the plurality of joint portions J1-J6 is replaced with a linear motion expansion / contraction joint portion, and the second joint portion J2 is offset in two directions with respect to the first joint portion J1. Then, by offsetting the third joint portion J3 in two directions with respect to the second joint portion J2, the robot arm mechanism 200 of the robot apparatus according to the present embodiment eliminates the singularity posture structurally.

(Block configuration)
FIG. 4 is a block diagram of the robot apparatus according to the present embodiment. Note that, for example, stepping motors are provided as actuators at the joints J1, J2, J3, J4, J5, and J6 of the robot arm mechanism 200 according to the present embodiment. Motor drivers 201, 203, 205, 207, 209, and 211 are electrically connected to these stepping motors, respectively. Further, on the drive shafts of these stepping motors 201, 203, 205, 207, 209, 211, rotary encoders 202, 204 for measuring angular positions by adding / subtracting pulses output at fixed rotation angles with a counter. , 206, 208, 210, 212 are connected to each other.

The motion control device 100 is provided to control the movement of a point of interest such as a hand by controlling the motion of the joints J1-J6. The system control unit 101, an operation unit interface (I / F) 102, a driver It has a control unit 103, a current position / attitude calculation unit 104, a trajectory calculation unit 105, and a command value calculation unit 106.
The system control unit 101 includes a CPU (Central Processing Unit), a semiconductor memory, and the like, and controls the operation control apparatus 100 in an integrated manner. Each unit is connected to the system control unit 101 via a control / data bus 109.

  An operation unit 300 is connected to the operation control apparatus 100 via an operation unit interface 102. The operation unit 300 functions as an input interface for the user to input movement and posture change of a robot hand (hand effector) attached to the wrist unit 4. In the motion control apparatus 100, for example, paying attention to the grip center point of the robot hand, the calculation process such as movement / posture change is executed using the point (hand reference point) as a control point (origin of the hand coordinate system Σh). The final target position of the hand is input to the motion control device 100 via the operation unit 300. The operation unit 300 includes input devices such as a switch, a mouse, a keyboard, a trackball, and a touch panel, for example.

  The driver control unit 103 controls the motor drivers 201, 203, 205, 207, 209, and 211 in an integrated manner. The driver control unit 103 transmits to the motor drivers 201, 203, 205, 207, 209, and 211 a control signal corresponding to the command value calculated for each joint portion J1-J6 by the command value calculation unit 106 described later. To do.

  The current position / posture calculation unit 104 performs forward kinematics based on the joint transformation matrix defined according to the link parameters of the arm structure based on the joint variables of the joints J1, J2, J3, J4, J5, and J6. The position of the hand reference point and the hand posture viewed from the robot coordinate system are calculated. The joint variable is a positive or negative rotation angle from the reference position in the joint portions J1, J2, J4, J5, and J6, and in the joint portion J3, the extension distance (linear displacement) from the most contracted state. .

  As shown in FIG. 3, the robot coordinate system Σb is a coordinate system having an arbitrary position on the first rotation axis RA1 of the first joint portion J1 as the origin. In the robot coordinate system Σb, three orthogonal axes (Xb, Yb, Zb) are defined. The Zb axis is an axis parallel to the first rotation axis RA1. The Xb axis and the Yb axis are orthogonal to each other and orthogonal to the Zb axis. The hand coordinate system Σh is a coordinate system having an origin at an arbitrary position (hand reference point) of the robot hand attached to the wrist 4. For example, when the robot hand is a two-finger hand, the position of the hand reference point is defined as the center position between the two finger tips. In the hand coordinate system Σh, three orthogonal axes (Xh, Yh, Zh) are defined. The Zh axis is an axis parallel to the sixth rotation axis RA6. The Xh axis and the Yh axis are orthogonal to each other and orthogonal to the Zh axis. For example, the Xh axis is an axis parallel to the front-rear direction of the robot hand. The hand posture is a rotation angle around each of three orthogonal axes of the hand coordinate system Σh with respect to the robot coordinate system Σb (rotation angle around the Xh axis (yaw angle) α, rotation angle around the Yh axis (pitch angle) β, Zh axis It is given as the surrounding rotation angle (roll angle) γ.

Current position and orientation calculation unit 104, based on the encoder pulses from the encoder 202,204,206,208,210,212, joint variable vector of the robot arm mechanism 200 ^- computing the theta. Joint variable vector ^- theta is given by a rotating joint J1, J2, J4, J5, J6 of joint angle _{θ J1, θ J2, θ J4} , θ J5, θ J6 and telescopic length _{L J3} of linear expansion joints J3 Is a set of joint variables (θ _J1 , θ _J2 , L _J3 , θ _J4 , θ _J5 , θ _J6 ). The current position / posture calculation unit 104 uses the homogeneous transformation matrix K (parameters (θ _J1 , θ _J2 , L _J3 , θ _J4 , θ _J5 , θ _J6 )) to determine the position of the hand reference point viewed from the robot coordinate system Σb. Calculate (x, y, z) and hand posture (α, β, γ). The homogeneous transformation matrix K is a determinant that defines the relationship between the hand coordinate system Σh and the robot coordinate system Σb. The homogeneous transformation matrix K is determined by the relationship between the links constituting the robot arm mechanism 200 (link length and link torsion angle) and the relationship between the joint axes (distance between links and angle between links). For example, current position and orientation calculation unit 104, the joint variable vector of the current robot arm mechanism _{^{200 - θ 0 (θ 0-}} J1, θ 0-J2, L 0-J3, θ 0-J4, θ 0-J5, By substituting θ _0−J6 ) into the homogeneous transformation matrix K, the current position x ₀ (x ₀ , y ₀ , z ₀ ) of the hand reference point and the hand posture φ ₀ (α ₀ ) viewed from the robot coordinate system Σb. , Β ₀ , γ ₀ ).

  The trajectory calculation unit 105 calculates the target position of the hand for each unit time (control period, for example, 10 ms) based on the current position / posture of the hand reference point (hereinafter simply referred to as the hand) and the final target position / posture of the hand. To do. By giving the target position, it is possible to move the hand with the position and posture defined by the target position. The current position / posture of the hand is obtained from calculation processing by the current position / posture calculation unit 104. The final target position / posture of the hand is input by the user via the operation unit 300, for example.

The trajectory calculation unit 105 substitutes each parameter into a preset polynomial having the current position x _{0 of the} hand, the current posture φ ₀ , the final target position X ₁ of the hand, and the final target posture Φ ₁ as parameters. To calculate a hand trajectory (hereinafter referred to as a hand trajectory) and calculate a target position for each unit time Δt on the hand trajectory. An arbitrary method is adopted as the trajectory calculation method. The unit time Δt is a fixed value as a control cycle, and is set to 10 ms, for example, by the user. The trajectory calculation unit 105 determines the number m (= T / Δt) of target positions based on the target time width T and unit time Δt required for moving the hand from the current position to the final target position, and unit time Δt. The target position (x ₁ , x ₂ ... X _m (m = t1 / Δt)) of the hand on each hand trajectory is calculated. For example, the target position x ₁ is a parameter that gives both the hand reference point position x ₁ and the hand posture φ ₁ . Target time T, for example, may be input directly by the user, it may be determined in accordance with the period in which the user is operating the operation unit 300, the final target position of the current position x ₀ and the hand of the hand it may be automatically determined in accordance with the distance between X _1. For example, the hand trajectory is calculated as a straight line trajectory connecting the final target position X ₁ of the current position x ₀ and the hand hand.

  The command value calculation unit 106 calculates a plurality of joint variable vectors related to a plurality of target positions between the current position of the hand and the final target position of the hand. The joint variable vector includes six joint variables of the joint portions J1-J6, that is, the rotation angles of the rotary joint portions J1, J2, J4-J6 and the arm expansion / contraction length of the linear motion expansion / contraction joint portion J3. The calculation process of the command value calculation unit 106 will be described later. FIG. 5 is a flowchart of operation control by the operation control apparatus 100 of FIG.

(Step S11) The current joint variable vector ^- theta ₀ calculations
Under the control of the system control unit 101, the current position and orientation calculation unit 104, based on the encoder pulse input from the encoder 202,204,206,208,210,212, current joint variable vector ^{- θ} ₀ ^(θ _{0 -J1} , _{θ0 -J2} , _{L0 -J3} , θ0 _-J4 , θ0 _-J5 , θ0 _-J6 ) are calculated.
(Step S12) calculates a current position _{x 0} of the hand
Under the control of the system control unit 101, the current position and orientation calculation unit 104, the current joint variable vector ^- based on the theta _0, the current position of the hand on the robot coordinate system _{Σb x 0 (x 0, y} 0, z 0 ) And the hand posture φ ₀ (α ₀ , β ₀ , γ ₀ ) are calculated.

(Step S13) The target position (x ₁ , x ₂ ,... X _m (= X ₁ )) of the hand is calculated.
Under the control of the system control unit 101, the trajectory calculation unit 105 calculates the hand trajectory from the current hand position x ₀ and hand posture φ ₀ to the final target position X ₁ and hand posture φ ₁ of the hand, The target position (x ₁ , x ₂ ,... X _m (= X ₁ )) of the hand for each unit time Δt is calculated.
(Step S14) The goal of the joint variable vector ^{_{(- θ 1, - θ 2}} , ··· - θ m) calculated
Under the control of the system control unit 101, the command value calculation unit 106 causes the joint variable vectors ( ⁻ θ ₁ , ⁻ corresponding to the hand target positions (x ₁ , x ₂ ,... X _m (= X ₁ )), respectively. θ _2, ··· ^- θ _m) is calculated. Details of the calculation processing procedure by the command value calculation unit 106 will be described later.
(Step S15) The goal of the joint variable vector ^{_{(- θ 1, - θ 2}} , ··· - θ m) to the output
From the driver control unit 103, the motor driver 201,203,205,207,209,211, the target as a command value joint variable vector ^{_{(- θ 1, - θ 2}} , ··· - θ m) is given The signals are sequentially output at a control period Δt (for example, 10 ms).

Next, calculation processing by the command value calculation unit 106 in step S14 will be described with reference to FIG. FIG. 6 is a flowchart of step S14 in FIG.
(Step S141) Initialization (n = 0)
Here, for convenience of explanation, a variable n is used. First, this variable n is initialized to a zero value.
Calculation of ^{- (θ} _n) (step S142) Jacobian inverse matrix ^{J -1}
The arm mechanism of this embodiment does not have a singular point because of its structure, and therefore a Jacobian inverse matrix always exists. The Jacobian inverse matrix is a matrix that converts the hand speed (minor change in hand position / posture) to the joint angular velocity (minor change in joint angle / extension / contraction length). The Jacobian inverse matrix is given by a knitting differential based on a joint variable of a vector representing the position and hand posture of the hand reference point. Command value calculating section 106, the current joint variable vector calculated by the current position and orientation calculation unit 104 in step _{^{S11 - θ n (θ n-}} J1, θ n-J2, L n-J3, θ n-J4, the _{θ n-J5, θ n-} J6), Jacobian inverse matrix ^{J -1} by the link parameters of the arm structure ^(- θ _n) is calculated.

(Step S143) hand speed ^- x _{n + 1} of the calculation
Based on the current hand position (current target position) x _n , the next hand position (next target position after the unit time Δt) x _{n + 1} , and the unit time Δt, the hand speed ^· x _{n + 1} is calculated.
(Step S144) Calculation of joint angular velocity ^{− ·} θ _{n + 1}
Calculated hand speed in step S143 ^- x _{n + 1} is a Jacobian inverse matrix ^{J -1 -} by (θ _n), joint angular velocity ^- are converted to ^· θ _{n + 1.}

(Step S145) The goal of the joint variable vector ^{- θ} _{n + 1} of the calculation
Number of repetitions was calculated in the previous step S145 (in the case of the variable n = 0 calculated in step S11) joint variable vector ^- theta _n, calculated joint angular velocity in step S144 ^{- · θ} _{n + 1,} the unit time Δt based on the following joint variable vector ^{- θ} _{n + 1} is calculated. By multiplying the joint angular velocity ^{− ·} θ _{n + 1} by the unit time Δt, the displacement amount of each joint during the unit time Δt is calculated. Joint variable vector of the moving just before ^- by adding the displacement amount of the joint theta _n, the joint variable vector after the unit time Δt elapses ^- theta _{n + 1} is calculated.

(Step S146) Determination of processing continuation
When the variable n is the number of repetitions (m−1), the calculation process by the command value calculation unit 106 is terminated. On the other hand, when the variable (n) is not the number of repetitions (m−1), the process proceeds to step S147.

(Step S147) Raise variable n ← n + 1
The variable n is incremented to (n + 1), and the process returns to step S142.

Thus calculation processing by the command value calculating unit 106 as described, the target position of the hand _{(x 1, x 2, ···} x m) to the corresponding joint variable vector ^{_{(- θ 1, - θ 2}} , ··· - θ _m ) is calculated.

Next, the effect of motion control by the motion control device 100 of the robot apparatus according to the present embodiment will be described with reference to FIG. The motion control device 100 of the robot apparatus according to the present embodiment is based on the current position of the hand and the next target position of the hand, and joint variables (joint angle, extension distance) θ _J1 , θ corresponding to the target position of the hand. _J2 , L _J3 , θ _J4 , θ _J5 , θ _J6 are calculated, and motor drivers 201, 203, 205, with the joint variables θ _J1 , θ _J2 , L _J3 , θ _J4 , θ _J5 , θ _J6 as command values, respectively. Input to 207, 209, and 211. Motor drivers 201, 203, 205, 207, 209, and 211 drive actuators (stepping motors) with joint angles and extension distances of the joints as target values.
It is a figure which shows the time change of the joint angle of the 1st joint part J1, in order to demonstrate the effect by the operation control (position command) of FIG. 5 compared with the case of a speed command. In FIG. 7, as an example, regarding the joint angle θ _J1 of the first joint portion J1, the time change when the position (joint angle) is a command value for the motor driver 201, and the speed (joint angular velocity) is the command value. It shows with time. Motor driver 201 of the first joint portion J1, the time _t 0 from the driver control unit _103, t _1, t 2, to · · · · _{t m-1,} joint angle theta _1-J1 as respective command _values, theta _{2- J1,} θ3 _-J1,... Θm _-J1 are given. As shown in FIG. 7, the motor driver 201 receives a pulse necessary for the joint angle of the first joint portion _J1 to reach the joint angle θ ₁ -J1 at time t ₁ after the unit time Δt has elapsed from time t _0. Is supplied to the stepping motor. As a result, the joint angle of the first joint portion J1 reaches the joint angle θ ₁ -J1 at time t ₁ after the unit time Δt has elapsed from time t ₀ . What is important here is that even if the joint angle of the first joint portion J1 does not rotate to the joint angle θ ₁ -J1 at the time t ₁ after the lapse of the unit time Δt from the time t _{0 for} some reason, The difference between the scheduled joint angle θ _{1 -J1} and the actual joint angle is that it does not accumulate thereafter. That is, even if the motor driver 201 has not reached the joint angle θ ₁ -J1 at time t ₁ after elapse of the unit time Δt from time t ₀ , the time t ₂ after elapse of the unit time Δt from the next time t _1. In addition, a pulse necessary for the joint angle of the first joint portion J1 to reach the joint angle θ2 _-J1 is supplied to the stepping motor. Thus, the joint angle of the first joint portion J1, even if they do not rotate until the joint angle theta _1-J1 were scheduled at time t _1, the actual joint angle at time t _1, after the unit time Δt time elapsed it reaches the joint angle theta _1-J1 to t _2.

  On the other hand, when a joint angular velocity is input as a command value to the motor driver 201, an inertia error at the start of movement or a change in velocity accumulates, and the actual joint angle of the first joint portion J1 is calculated from the calculated joint angle. It may come off. On the other hand, like the operation control device 100 of the robot apparatus according to the present embodiment, the joint angle of the first joint portion J1 is obtained in unit time Δt by inputting the joint angle as a command value to the motor driver 201. The stepping motor is driven so as to move to the commanded joint angle. Thereby, even if a position error occurs during the operation control of the first joint portion J1, the position error is not accumulated. Therefore, the robot apparatus according to this embodiment can eliminate or reduce the position error in the movement control of the robot arm.

  That is, in the present embodiment, the hand speed between the target positions is calculated, converted into a joint angular speed using a Jacobian inverse matrix (previously, this joint angular speed was used as a command value), and a joint variable vector is obtained from the joint angular speed, By supplying the joint variable vector as a command value to the motor drive, the motor drive drives the motor to move to the commanded joint variable vector in unit time, so the position error is different from the case where the joint angular velocity is used as the command value. It does not occur in principle or can be greatly reduced, eliminating the need for feedback control. Further, since no position error occurs, it is possible to calculate the joint variable vectors of the target positions one after the other while sequentially moving the target positions, that is, to calculate the joint variable vectors of all the target positions in advance. In addition, joint variable vectors can be obtained by inverse kinematics using a homogeneous transformation matrix. In this case, however, the calculation process becomes very complicated due to the non-linear operation, and the solution can be obtained uniquely analytically. Although it may not be possible, the Jacobian inverse matrix is a linear operation, the calculation process is very simple, and such a problem does not occur. However, the linear motion telescopic arm mechanism of the present embodiment does not have redundancy unlike an arm structure that combines ordinary rotary joints, so that innumerable solutions do not occur and can be simply processed with the Jacobian inverse matrix. .

  Although several embodiments of the present invention have been described, these embodiments are presented 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 changes can be made without departing from the spirit 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 the equivalents thereof.

  DESCRIPTION OF SYMBOLS 100 ... Motion control apparatus, 200 ... Robot arm mechanism, 300 ... Operation part, 101 ... System control part, 102 ... Operation part I / F, 103 ... Driver control part, 104 ... Current position and attitude | position calculation part, 105 ... Trajectory calculation , 106 ... command value calculation unit, 109 ... control / data bus, 201, 203, 205, 207, 209, 211 ... motor driver, 202, 204, 206, 208, 210, 212 ... encoder

Claims (8)

In an operation control device for a multi-joint arm mechanism that outputs a plurality of command values to a plurality of motor drivers respectively corresponding to actuators of a plurality of joint portions constituting the multi-joint arm mechanism,
A current position calculation unit for calculating a current position of a point of interest on the multi-joint arm mechanism from a plurality of current joint variables respectively corresponding to the plurality of joint parts;
A trajectory calculation unit for calculating a trajectory of the attention point from the current position to the final target position;
A time setting unit for setting a target time width required for the point of interest to move from the current position to the final target position and a control cycle for controlling the motor driver according to a user instruction;
A target position calculator for calculating a plurality of target positions on the calculated trajectory based on the set target time width and the set control period;
A moving speed calculator that calculates a moving speed of the point of interest between the calculated target positions based on the calculated distance between the target positions and the control period ;
A joint speed conversion unit that converts the calculated movement speed into a plurality of joint speeds corresponding to the plurality of joint parts by a Jacobian inverse matrix corresponding to the structure of the multi-joint arm mechanism ;
A change amount calculation unit that calculates a displacement amount of each of the plurality of joint portions in the control cycle for each of the target positions from the calculated joint speeds ;
By adding the amount of displacement of each of the plurality of joints to the joint variable of each of the plurality of joints corresponding to each of the target positions, the joint variable of each of the plurality of joints is determined for each of the plurality of joints. A command value calculator for calculating each target position as a command value;
Operation control device of the articulated arm mechanism comprising a command value output unit for force out the control for each cycle a command value of each of the plurality of joint portions prior to liver Tadoraiba. The plurality of joints for each of the plurality of target positions while sequentially chaining from the current position to the final target position by the moving speed calculation unit, the joint speed conversion unit, the change amount calculation unit, and the command value calculation unit. The motion control device for an articulated arm mechanism according to claim 1, wherein the variable is calculated. 2. The motion control device for an articulated arm mechanism according to claim 1, wherein the command value output unit sequentially outputs the plurality of command values related to a target position next to each of the target positions to the motor driver every time the control cycle elapses. .   The command value output unit sequentially outputs the plurality of command values to the motor driver in parallel with the calculation processing of the plurality of joint variables related to all of the plurality of target positions by the command value calculation unit. Control device for multi-joint arm mechanism.   The motion control device for an articulated arm mechanism according to claim 1, wherein the plurality of joint portions are rotary joints.   2. The motion control device for a multi-joint arm mechanism according to claim 1, wherein the plurality of joint portions do not have elbow joint portions, and at least one of the plurality of joint portions is a linear motion expansion / contraction joint portion. The direct acting telescopic joint is
A plurality of first connecting pieces having a U-shaped cross-section connected to bendable;
A plurality of second connecting pieces having a flat plate shape connected so as to be bendable, and a second connecting piece at the tip of the plurality of second connecting pieces is a first connecting piece at the tip of the plurality of first connecting pieces. The first connecting piece to be connected is joined to the second connecting piece at an upper portion thereof to bend and thereby form a columnar body. The columnar body is formed by separating the first and second connecting pieces. Is released,
The motion control device for an articulated arm mechanism according to claim 6, further comprising: an injection unit that joins the first connection piece to the second connection piece to form the columnar body and supports the columnar body. An articulated arm mechanism comprising a plurality of joints;
In a robot apparatus having an operation control device that outputs a plurality of command values to a plurality of motor drivers respectively corresponding to actuators of a plurality of joint portions constituting the multi-joint arm mechanism,
The operation control device includes:
A current position calculation unit for calculating a current position of a point of interest on the multi-joint arm mechanism from a plurality of current joint variables respectively corresponding to the plurality of joint parts;
A trajectory calculation unit for calculating a trajectory of the attention point from the current position to the final target position;
A time setting unit for setting a target time width required for the point of interest to move from the current position to the final target position and a control cycle for controlling the motor driver according to a user instruction;
A target position calculator for calculating a plurality of target positions on the calculated trajectory based on the set target time width and the set control period;
A moving speed calculator that calculates a moving speed of the point of interest between the calculated target positions based on the calculated distance between the target positions and the control period ;
A joint speed conversion unit that converts the calculated movement speed into a plurality of joint speeds corresponding to the plurality of joint parts by a Jacobian inverse matrix corresponding to the structure of the multi-joint arm mechanism ;
A change amount calculation unit that calculates a displacement amount of each of the plurality of joint portions in the control cycle for each of the target positions from the calculated joint speeds ;
By adding the amount of displacement of each of the plurality of joints to the joint variable of each of the plurality of joints corresponding to each of the target positions, the joint variable of each of the plurality of joints is determined for each of the plurality of joints. A command value calculator for calculating each target position as a command value;
Robot apparatus having a command value output unit for force out the control for each cycle a command value of each of the plurality of joint portions prior to liver Tadoraiba.