WO2016175157A1 - 多関節アーム機構の動作制御装置及びロボット装置 - Google Patents
多関節アーム機構の動作制御装置及びロボット装置 Download PDFInfo
- Publication number
- WO2016175157A1 WO2016175157A1 PCT/JP2016/062862 JP2016062862W WO2016175157A1 WO 2016175157 A1 WO2016175157 A1 WO 2016175157A1 JP 2016062862 W JP2016062862 W JP 2016062862W WO 2016175157 A1 WO2016175157 A1 WO 2016175157A1
- Authority
- WO
- WIPO (PCT)
- Prior art keywords
- joint
- arm mechanism
- control device
- unit
- hand
- Prior art date
Links
Images
Classifications
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/10—Programme-controlled manipulators characterised by positioning means for manipulator elements
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J18/00—Arms
- B25J18/02—Arms extensible
Definitions
- Embodiments of the present invention relate to an operation control apparatus and a robot apparatus for an 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.
- 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.
- 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.
- the joint angular velocities of each of the plurality of joints are calculated from the target position by 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.
- 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.
- the purpose is to eliminate or reduce the position error in the movement control of the robot arm.
- the motion control device for the multi-joint arm mechanism outputs a plurality of command values to a plurality of motor drivers respectively corresponding to the actuators of the plurality of joint portions constituting the multi-joint arm mechanism.
- 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.
- the command value calculation unit calculates the moving speed of the point of interest based on the distance and unit time from each of a plurality of target positions per unit time on the orbit to the next target position.
- a plurality of joint angular velocities corresponding to a plurality of joints are converted into a plurality of joint angular velocities respectively corresponding to a plurality of joints by a Jacobian inverse matrix corresponding to the structure of the arm mechanism, and a plurality of joints corresponding to the next target position from the calculated joint angular velocities and unit times.
- a variable is calculated as multiple command values.
- a plurality of command values are output from a command value output unit to 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.
- 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.
- 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.
- 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 part 4 is provided with an adapter (not shown).
- 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, here, six joint portions J1, J2, J3, J4, J5, and J6.
- the plurality of joint portions J1, J2, J3, J4, J5, and J6 are sequentially arranged from the base portion 1.
- the first, second, and third joints J1, J2, and J3 are called the root three axes
- the fourth, fifth, and sixth joints J4, J5, and J6 change the posture of the robot hand 3.
- 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.
- 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 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 (first support) 11a forming the base 1 has a cylindrical hollow structure formed around the first rotation axis RA1 of the first joint J1.
- the first joint portion J1 is attached to a fixed base (not shown).
- 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.
- 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 at the lower end portion of the second support portion 11b.
- a second joint portion J2 as a bending joint portion around the second rotation axis RA2 is configured.
- 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 a fourth rotation axis RA4 that is typically in contact with the arm central axis along the expansion / contraction direction of the arm portion 2, that is, the third movement axis RA3 of the third joint portion J3.
- 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.
- 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.
- the robot hand turns left and right.
- the robot hand attached to the adapter of the wrist part 4 has the first, second and third joint parts 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.
- the length of the extension / contraction distance of the arm part 2 of the third joint part J3 enables the robot hand to reach a wide range of objects from the proximity position of the base 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 a length of the expansion / contraction distance.
- 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.
- 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.
- 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.
- 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.
- 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.
- 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.
- 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.
- the bending of the first and second connection frame rows 21 and 22 is constrained to each other.
- 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.
- 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 are surrounded by a top plate, a bottom plate, and both side plates, and the front end and the rear end are open.
- 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.
- 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.
- 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.
- FIG. 3 is a diagram showing the robot arm mechanism 200 of FIG.
- 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.
- 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.
- 1st joint part J1 is arrange
- 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.
- 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.
- 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.
- 3rd joint part J3 is comprised as a linear motion expansion-contraction joint centering on movement axis RA3.
- 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.
- 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.
- 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.
- 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.
- the robot arm mechanism 200 of the robot apparatus eliminates the singularity posture structurally.
- FIG. 4 is a block diagram of the robot apparatus according to the present embodiment.
- 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.
- 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 joint portions J1-J6, and includes a 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.
- 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, 211 each 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. .
- 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.
- 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.
- the position of the hand reference point is defined as the center position between the two finger tips.
- 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.
- 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) ⁇ .
- 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).
- 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,
- ⁇ 0-J6 the joint variable vector of the current robot arm mechanism 200 - ⁇ 0 ( ⁇ 0- J1, ⁇ 0-J2, L 0-J3, ⁇ 0-J4, ⁇ 0-J5
- ⁇ 0-J6 By substituting ⁇ 0-J6 ) into the homogeneous transformation matrix K, the current position x 0 (x 0 , y 0 , z 0 ) of the hand reference point viewed from the robot coordinate system ⁇ b and the hand posture ⁇ 0 ( ⁇ 0 , ⁇ 0 , ⁇ 0 ).
- 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 ⁇ 0 , the final target position X 1 of the hand, and the final target posture ⁇ 1 as parameters.
- a hand trajectory hereinafter referred to as a 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 target position x 1 is a parameter that gives both the hand reference point position x 1 and the hand posture ⁇ 1 .
- Target time T 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 0 and the hand of the hand it may be automatically determined in accordance with the distance between X 1.
- the hand trajectory is calculated as a straight line trajectory connecting the final target position X 1 of the current position x 0 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, and 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 0 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 ( ⁇ 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 ⁇ 0 ( ⁇ 0 , ⁇ 0 , ⁇ 0 ) are calculated.
- the trajectory calculation unit 105 calculates the hand trajectory from the current hand position x 0 and hand posture ⁇ 0 to the final target position X 1 and hand posture ⁇ 1 of the hand,
- 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).
- a variable n is used.
- 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 Calculation of hand speed - x n + 1 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.
- 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
- the variable n is the number of repetitions (m ⁇ 1)
- the calculation process by the command value calculation unit 106 is terminated.
- 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.
- 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.
- 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.
- 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.
- the motor driver 201 receives pulses necessary for the joint angle of the first joint portion J1 to reach the joint angle ⁇ 1 -J1 at time t 1 after the unit time ⁇ t has elapsed from time t 0. Is supplied to the stepping motor.
- the joint angle of the first joint portion J1 reaches the joint angle ⁇ 1 -J1 at time t 1 after the unit time ⁇ t has elapsed from time t 0 .
- What is important here is that even if the joint angle of the first joint portion J1 does not rotate to the joint angle ⁇ 1 -J1 at the time t 1 after the unit time ⁇ t has elapsed from the time t 0 for some reason, The error between the scheduled joint angle ⁇ 1 -J1 and the actual joint angle is that it does not accumulate thereafter.
- the motor driver 201 does not reach the joint angle ⁇ 1 -J1 at the time t 1 after the unit time ⁇ t has elapsed since the time t 0, but the time t 2 after the unit time ⁇ t has elapsed since the next time t 1.
- 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.
- 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.
- 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.
- 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,
- 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.
- 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.
- 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.
- the Jacobian inverse matrix is a linear operation, the calculation process is very simple, and such a problem does not occur.
- 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. .
Landscapes
- Engineering & Computer Science (AREA)
- Robotics (AREA)
- Mechanical Engineering (AREA)
- Manipulator (AREA)
Abstract
ロボットアームの移動制御において位置誤差を解消又は減少させる。 ロボット装置の動作制御装置(100)は、複数の関節部(J1-J6)にそれぞれ対応する現在の複数の関節変数から注目点の現在位置を計算する現在位置・姿勢計算部(104)と、現在位置から最終目標位置までの軌道を計算する軌道計算部(105)と、軌道上の単位時間ごとの複数の目標位置各々から次の目標位置までの距離と単位時間とに基づいて移動速度を計算し、計算された移動速度をロボットアーム機構(200)の構造に応じたヤコビアン逆行列により複数の関節部(J1-J6)の複数の関節角速度に変換し、複数の関節角速度と単位時間とから次の目標位置に対応する複数の関節変数を複数の指令値として計算する指令値計算部(106)と、複数の指令値を複数のモータドライバ(201、203,205、207、209、211)にそれぞれ出力するドライバ制御部(103)とを具備する。
Description
本発明の実施形態は、多関節アーム機構の動作制御装置及びロボット装置に関する。
近年ロボットがユーザと同一空間にいる環境が多くなってきている。介護用ロボットはもちろん産業用ロボットでも作業者と並んで作業を行なう状況が今後拡大していくものと考えられる。そのようなロボットの多くは垂直多関節アーム機構を備えている。垂直多関節アーム機構には位置に関して3自由度(x,y,z)、姿勢に関して3自由度(α,β,γ)が要求され、一般的には根元3軸と呼ばれる回転関節部J1,J2,J3と手首3軸と呼ばれる回転関節部J4,J5,J6とからそれを実現している。例えば関節部J1,J4,J6にはねじり関節部、関節部J2,J3,J5には曲げ関節部が適用される。アーム先端にはハンド等の手先効果器(エンドエフェクタ)が装備される。
その手先の位置、姿勢の動作制御としては様々な方式があるが、その一例として現在位置から最終目標位置までの軌道を計算し、その軌道上での10ms等の単位時間(制御周期)ごとの個々の目標位置を求める。
目標位置から複数の関節部それぞれの関節角度を同次変換行列を用いた逆運動学により求めるには代数的又は幾何学的解法が実時間制御では好ましいが、それを適用できるアーム機構が制限されており、近年ではそのような制限が少ないヤコビアン逆行列による方法が一般的によく用いられている。
この方法では目標位置からヤコビアン逆行列により複数の関節部それぞれの関節角速度を計算し、その関節角速度を指令値としてモータドライバに出力する。モータドライバは各関節部の速度を目標値としてアクチュエータ(モータ)を駆動する。
この方式では移動開始時や速度変化に伴う慣性誤差等により実際の手先の位置が計算上の位置から外れてしまう事があり、その誤差を修正するためにフィードバック制御が採用されることが多い。
しかしフィードバック制御では修正動作が後追いとなりその位置の誤差修正は次の指令値に加えることとなるので位置誤差を完全に解消することはできず、また移動速度が不安定になる事態も生じる。
目的は、ロボットアームの移動制御において位置誤差を解消又は減少させることにある。
本実施形態に係る多関節アーム機構の動作制御装置は、多関節アーム機構を構成する複数の関節部のアクチュエータにそれぞれ対応する複数のモータドライバに複数の指令値をそれぞれ出力する。指令値の計算処理では、まず現在位置計算部で複数の関節部にそれぞれ対応する現在の複数の関節変数から多関節アーム機構上の注目点の現在位置を計算する。現在位置から最終目標位置までの注目点の軌道を軌道計算部で計算する。指令値計算部では、軌道上の単位時間ごとの複数の目標位置各々から次の目標位置までの距離と単位時間とに基づいて注目点の移動速度を計算し、計算された移動速度を多関節アーム機構の構造に応じたヤコビアン逆行列により複数の関節部にそれぞれ対応する複数の関節角速度に変換するとともに、計算された複数の関節角速度と単位時間とから次の目標位置に対応する複数の関節変数を複数の指令値として計算する。複数の指令値を複数のモータドライバに指令値出力部から出力する。
以下、図面を参照しながら本実施形態に係るロボット装置を説明する。本実施形態に係るロボット装置は動作制御装置を備える。動作制御装置は、ロボット装置に装備されている多関節アーム機構の各関節のモータドライバを制御する単独の装置として、多関節アーム機構を備えるロボット装置に組み込むことができる。本実施形態では、複数の関節部のうち一が直動伸縮関節の多関節アーム機構を有するロボット装置を説明する。以下の説明において、略同一の機能及び構成を有する構成要素については、同一符号を付し、重複説明は必要な場合にのみ行う。
図1は、本実施形態に係るロボット装置のロボットアーム機構200の外観斜視図である。ロボットアーム機構200は、略円筒形状の基部1と基部1に接続されるアーム部2とアーム部2の先端に取り付けられる手首部4とを有する。手首部4には図示しないアダプタが設けられている。例えば、アダプタは後述の第6回転軸RA6の回転部に設けられる。手首部4に設けられたアダプタには、用途に応じたロボットハンドが取り付けられる。
ロボットアーム機構200は、複数、ここでは6つの関節部J1,J2,J3,J4,J5,J6を有する。複数の関節部J1,J2,J3,J4,J5,J6は基部1から順番に配設される。一般的に、第1、第2、第3関節部J1,J2,J3は根元3軸と呼ばれ、第4、第5、第6関節部J4,J5,J6はロボットハンド3の姿勢を変化させる手首3軸と呼ばれる。手首部4は第4、第5、第6関節部J4,J5,J6を有する。根元3軸を構成する関節部J1,J2,J3の少なくとも一つは直動伸縮関節である。ここでは第3関節部J3が直動伸縮関節部、特に伸縮距離の比較的長い関節部として構成される。アーム部2は直動伸縮関節部J3(第3関節部J3)の伸縮部分を表している。
第1関節部J1は基台面に対して例えば垂直に支持される第1回転軸RA1を中心としたねじり関節である。第2関節部J2は第1回転軸RA1に対して垂直に配置される第2回転軸RA2を中心とした曲げ関節である。第3関節部J3は、第2回転軸RA2に対して垂直に配置される第3軸(移動軸)RA3を中心として直線的にアーム部2が伸縮する関節である。
第4関節部J4は、第4回転軸RA4を中心としたねじり関節である。第4回転軸RA4は、後述の第7関節部J7が回転していないとき、つまりアーム部2の全体が直線形状にあるとき、第3移動軸RA3と略一致する。第5関節部J5は第4回転軸RA4に対して直交する第5回転軸RA5を中心とした曲げ関節である。第6関節部J6は第4回転軸RA4に対して直交し、第5回転軸RA5に対して垂直に配置される第6回転軸RA6を中心とした曲げ関節である。
基部1を成すアーム支持体(第1支持体)11aは、第1関節部J1の第1回転軸RA1を中心に形成される円筒形状の中空構造を有する。第1関節部J1は図示しない固定台に取り付けられる。第1関節部J1が回転するとき、アーム部2は第1支持体11aの軸回転とともに左右に旋回する。なお、第1支持体11aが接地面に固定されていてもよい。その場合、第1支持体11aとは独立してアーム部2が旋回する構造に設けられる。第1支持体11aの上部には第2支持部11bが接続される。
第2支持部11bは第1支持部11aに連続する中空構造を有する。第2支持部11bの一端は第1関節部J1の回転部に取り付けられる。第2支持部11bの他端は開放され、第3支持部11cが第2関節部J2の第2回転軸RA2において回動自在に嵌め込まれる。第3支持部11cは第1支持部11a及び第2支持部に連通する鱗状の外装からなる中空構造を有する。第3支持部11cは、第2関節部J2の曲げ回転に伴ってその後部が第2支持部11bに収容され、また送出される。ロボットアーム機構200の直動伸縮関節部J3(第3関節部J3)を構成するアーム部2の後部はその収縮により第1支持部11aと第2支持部11bの連続する中空構造の内部に収納される。
第3支持部11cはその後端下部において第2支持部11bの開放端下部に対して第2回転軸RA2を中心として回動自在に嵌め込まれる。それにより第2回転軸RA2を中心とした曲げ関節部としての第2関節部J2が構成される。第2関節部J2が回動するとき、アーム部2は第2回転軸RA2を中心に垂直方向に回動、つまり起伏動作をする。
第4関節部J4は、アーム部2の伸縮方向に沿ったアーム中心軸、つまり第3関節部J3の第3移動軸RA3に典型的には接する第4回転軸RA4を有するねじり関節である。第4関節部J4が回転すると、手首部4及び手首部4に取り付けられたロボットハンドは第4回転軸RA4を中心に回転する。第5関節部J5は、第4関節部J4の第4回転軸RA4に対して直交する第5回転軸RA5を有する曲げ関節部である。第5関節部J5が回転すると、第5関節部J5から先端にかけてロボットハンドとともに上下(第5回転軸RA5を中心に垂直方向)に回動する。第6関節部J6は、第4関節部J4の第4回転軸RA4に直交し、第5関節部J5の第5回転軸RA5に垂直な第6回転軸RA6を有する曲げ関節である。第6関節部J6が回転すると、ロボットハンドは左右に旋回する。
上記の通り手首部4のアダプタに取り付けられたロボットハンドは、第1、第2、第3関節部J1.J2.J3により任意位置に移動され、第4、第5、第6関節部J4、J5、J6により任意姿勢に配置される。特に第3関節部J3のアーム部2の伸縮距離の長さは、基部1の近接位置から遠隔位置までの広範囲の対象にロボットハンドを到達させることを可能にする。第3関節部J3はそれを構成する直動伸縮機構により実現される直線的な伸縮動作とその伸縮距離の長さとが特徴的である。
(内部構造の説明)
図2は、図1のロボットアーム機構200の内部構造を示す斜視図である。直動伸縮機構はアーム部2と射出部30とを有する。アーム部2は第1連結コマ列21と第2連結コマ列22とを有する。第1連結コマ列21は複数の第1連結コマ23からなる。第1連結コマ23は略平板形に構成される。前後の第1連結コマ23は、互いの端部箇所においてピンにより屈曲自在に列状に連結される。第1連結コマ列21は内側や外側に自在に屈曲できる。
図2は、図1のロボットアーム機構200の内部構造を示す斜視図である。直動伸縮機構はアーム部2と射出部30とを有する。アーム部2は第1連結コマ列21と第2連結コマ列22とを有する。第1連結コマ列21は複数の第1連結コマ23からなる。第1連結コマ23は略平板形に構成される。前後の第1連結コマ23は、互いの端部箇所においてピンにより屈曲自在に列状に連結される。第1連結コマ列21は内側や外側に自在に屈曲できる。
第2連結コマ列22は複数の第2連結コマ24からなる。第2連結コマ24は横断面コ字形状の短溝状体に構成される。前後の第2連結コマ24は、互いの底面端部箇所においてピンにより屈曲自在に列状に連結される。第2連結コマ列22は内側に屈曲できる。第2連結コマ24の断面はコ字形状であるので、第2連結コマ列22は、隣り合う第2連結コマ24の側板同士が衝突して、外側には屈曲しない。なお、第1、第2連結コマ23、24の第2回転軸RA2に向いた面を内面、その反対側の面を外面というものとする。第1連結コマ列21のうち先頭の第1連結コマ23と、第2連結コマ列22のうち先頭の第2連結コマ24とは結合コマ27により接続される。例えば、結合コマ27は第2連結コマ24と第1連結コマ23とを合成した形状を有している。
射出部30は、複数の上部ローラ31と複数の下部ローラ32とが角筒形状のフレーム35に支持されてなる。例えば、複数の上部ローラ31は第1連結コマ23の長さと略等価な間隔を隔ててアーム中心軸に沿って配列される。同様に、複数の下部ローラ32は第2連結コマ24の長さと略等価な間隔を隔ててアーム中心軸に沿って配列される。射出部30の後方には、ガイドローラ40とドライブギア50とが第1連結コマ列21を挟んで対向するように設けられる。ドライブギア50は図示しない減速器を介してモータ55に接続される。第1連結コマ23の内面には連結方向に沿ってリニアギアが形成されている。複数の第1連結コマ23が直線状に整列されたときに互いのリニアギアは直線状につながって、長いリニアギアを構成する。ドライブギア50は、直線状のリニアギアにかみ合わされる。直線状につながったリニアギアはドライブギア50とともにラックアンドピニオン機構を構成する。
(接合構造の説明)
アーム伸長時、モータ55が駆動し、ドライブギア50が順回転すると、第1連結コマ列21はガイドローラ40により、アーム中心軸と平行な姿勢となって、上部ローラ31と下部ローラ32との間に誘導される。第1連結コマ列21の移動に伴い、第2連結コマ列22は射出部30の後方に配置された図示しないガイドレールにより射出部30の上部ローラ31と下部ローラ32との間に誘導される。上部ローラ31と下部ローラ32との間に誘導された第1、第2連結コマ列21,22は互いに押圧される。これにより、第1、第2連結コマ列21,22による柱状体が構成される。射出部30は、第1、第2連結コマ列21,22を接合して柱状体を構成するとともに、その柱状体を上下左右に支持する。第1、第2連結コマ列21、22の接合による柱状体が射出部30により堅持されることで、第1、第2連結コマ列21,22の接合状態が保持される。第1、第2連結コマ列21、22の接合状態が維持されているとき、第1、第2連結コマ列21,22の屈曲は互いに拘束される。それにより第1、第2連結コマ列21、22は、一定の剛性を備えた柱状体を構成する。柱状体とは、第2連結コマ列22に第1連結コマ列21が接合されてなる柱状の棒体を言う。この柱状体は第2連結コマ24が第1連結コマ23とともに全体として様々な断面形状の筒状体に構成される。筒状体とは上下左右が天板、底板及び両側板で囲まれ、前端部と後端部とが開放された形状として定義される。第1、第2連結コマ列21、22の接合による柱状体は、結合コマ27が始端となって、第3移動軸RA3に沿って直線的に第3支持部11cの開口から外に向かって送り出される。
アーム伸長時、モータ55が駆動し、ドライブギア50が順回転すると、第1連結コマ列21はガイドローラ40により、アーム中心軸と平行な姿勢となって、上部ローラ31と下部ローラ32との間に誘導される。第1連結コマ列21の移動に伴い、第2連結コマ列22は射出部30の後方に配置された図示しないガイドレールにより射出部30の上部ローラ31と下部ローラ32との間に誘導される。上部ローラ31と下部ローラ32との間に誘導された第1、第2連結コマ列21,22は互いに押圧される。これにより、第1、第2連結コマ列21,22による柱状体が構成される。射出部30は、第1、第2連結コマ列21,22を接合して柱状体を構成するとともに、その柱状体を上下左右に支持する。第1、第2連結コマ列21、22の接合による柱状体が射出部30により堅持されることで、第1、第2連結コマ列21,22の接合状態が保持される。第1、第2連結コマ列21、22の接合状態が維持されているとき、第1、第2連結コマ列21,22の屈曲は互いに拘束される。それにより第1、第2連結コマ列21、22は、一定の剛性を備えた柱状体を構成する。柱状体とは、第2連結コマ列22に第1連結コマ列21が接合されてなる柱状の棒体を言う。この柱状体は第2連結コマ24が第1連結コマ23とともに全体として様々な断面形状の筒状体に構成される。筒状体とは上下左右が天板、底板及び両側板で囲まれ、前端部と後端部とが開放された形状として定義される。第1、第2連結コマ列21、22の接合による柱状体は、結合コマ27が始端となって、第3移動軸RA3に沿って直線的に第3支持部11cの開口から外に向かって送り出される。
アーム収縮時、モータ55が駆動し、ドライブギア50が逆回転されると、ドライブギア50と係合している第1連結コマ列21が第1支持体11a内に引き戻される。第1連結コマ列の移動に伴って、柱状体が第3支持体11c内に引き戻される。引き戻された柱状体は射出部30後方で分離される。例えば、柱状体を構成する第1連結コマ列21はガイドローラ40とドライブギア50とに挟まれ、柱状体を構成する第2連結コマ列22は重力により下方に引かれ、それにより第2連結コマ列22と第1連結コマ列21とは互いに離反される。離反された第1、第2連結コマ列21,22はそれぞれ屈曲可能な状態に復帰し、個々に屈曲され、第1支持体11aの内部に格納される。
(座標系の定義)
図3は、図1のロボットアーム機構200を図記号表現により示す図である。ロボットアーム機構200において、根元3軸を構成する第1関節部J1と第2関節部J2と第3関節部J3とにより3つの位置自由度が実現される。また、手首3軸を構成する第4関節部J4と第5関節部J5と第6関節部J6とにより3つの姿勢自由度が実現される。
図3は、図1のロボットアーム機構200を図記号表現により示す図である。ロボットアーム機構200において、根元3軸を構成する第1関節部J1と第2関節部J2と第3関節部J3とにより3つの位置自由度が実現される。また、手首3軸を構成する第4関節部J4と第5関節部J5と第6関節部J6とにより3つの姿勢自由度が実現される。
第1関節部J1は、第1支持部11aと第2支持部11bとの間に配設されており、回転軸RA1を中心としたねじり関節として構成されている。回転軸RA1は第1関節部J1の固定部が設置される基台の基準面BPに垂直に配置される。回転軸RA1に平行にZ軸を規定する。Z軸を中心とした直交3軸のロボット座標系Σb(Xb,Yb,Zb)を規定する。
第2関節部J2は回転軸RA2を中心とした曲げ関節として構成される。第2関節部J2の回転軸RA2は空間座標系上のXb軸に平行に設けられる。第2関節部J2の回転軸RA2は第1関節部J1の回転軸RA1に対して垂直な向きに設けられる。さらに第2関節部J2は、第1関節部J1に対して、第1回転軸RA1の方向(Zb軸方向)と第1回転軸RA1に垂直なYb軸方向との2方向に関してオフセットされる。第2関節部J2が第1関節部J1に対して上記2方向にオフセットされるように、第2支持体11bは第1支持体11aに取り付けられる。第1関節部J1に第2関節部J2を接続する仮想的なアームロッド部分(リンク部分)は、先端が直角に曲がった2つの鈎形状体が組み合わされたクランク形状を有している。この仮想的なアームロッド部分は、中空構造を有する第1、第2支持体11a、11bにより構成される。
第3関節部J3は移動軸RA3を中心とした直動伸縮関節として構成される。第3関節部J3の移動軸RA3は第2関節部J2の回転軸RA2に対して垂直な向きに設けられる。第2関節部J2の回転角がゼロ度、つまりアーム部2の起伏角がゼロ度であってアーム部2が水平な基準姿勢においては、第3関節部J3の移動軸RA3は、第2関節部J2の回転軸RA2とともに第1関節部J1の回転軸RA1にも垂直な方向に設けられる。空間座標系上では、第3関節部J3の移動軸RA3はXb軸及びZb軸に対して垂直なYb軸に平行に設けられる。さらに、第3関節部J3は、第2関節部J2に対して、その回転軸RA2の方向(Yb軸方向)と、移動軸RA3に直交するZb軸の方向との2方向に関してオフセットされる。第3関節部J3が第2関節部J2に対して上記2方向にオフセットされるように、第3支持体11cは第2支持体11bに取り付けられる。第2関節部J2に第3関節部J3を接続する仮想的なアームロッド部分(リンク部分)は、先端が垂直に曲がった鈎形状体を有している。この仮想的なアームロッド部分は、第2、第3支持体11b、11cにより構成される。
第4関節部J4は回転軸RA4を中心としたねじり関節として構成される。第4関節部J4の回転軸RA4は第3関節部J3の移動軸RA3に略一致するよう配置される。
第5関節部J5は回転軸RA5を中心とした曲げ関節として構成される。第5関節部J5の回転軸RA5は第3関節部J3の移動軸RA3及び第4関節部J4の回転軸RA4に略直交するよう配置される。
第6関節部J6は回転軸RA6を中心としたねじり関節として構成される。第6関節部J6の回転軸RA6は第4関節部J4の回転軸RA4及び第5関節部J5の回転軸RA5に略直交するよう配置される。第6関節部J6は手先効果器としてのロボットハンドを左右に旋回するために設けられている。なお、第6関節部J6は、その回転軸RA6が第4関節部J4の回転軸RA4及び第5関節部J5の回転軸RA5に略直交する曲げ関節として構成されてもよい。
第5関節部J5は回転軸RA5を中心とした曲げ関節として構成される。第5関節部J5の回転軸RA5は第3関節部J3の移動軸RA3及び第4関節部J4の回転軸RA4に略直交するよう配置される。
第6関節部J6は回転軸RA6を中心としたねじり関節として構成される。第6関節部J6の回転軸RA6は第4関節部J4の回転軸RA4及び第5関節部J5の回転軸RA5に略直交するよう配置される。第6関節部J6は手先効果器としてのロボットハンドを左右に旋回するために設けられている。なお、第6関節部J6は、その回転軸RA6が第4関節部J4の回転軸RA4及び第5関節部J5の回転軸RA5に略直交する曲げ関節として構成されてもよい。
このように複数の関節部J1-J6の根元3軸のうちの一つの曲げ関節部を直動伸縮関節部に換装し、第1関節部J1に対して第2関節部J2を2方向にオフセットさせ、第2関節部J2に対して第3関節部J3を2方向にオフセットさせることにより、本実施形態に係るロボット装置のロボットアーム機構200は、特異点姿勢を構造上解消している。
(ブロック構成)
図4は、本実施形態に係るロボット装置のブロック図である。なお、本実施形態に係るロボットアーム機構200の関節部J1,J2,J3,J4,J5、J6には、アクチュエータとして、例えばステッピングモータが設けられている。これらステッピングモータには、モータドライバ201、203,205、207,209、211がそれぞれ電気的に接続されている。また、これらステッピングモータ201、203,205、207,209、211のドライブシャフトには、一定の回転角ごとに出力されるパルスをカウンタで加減算して角位置を測定するためのロータリーエンコーダ202,204,206,208,210,212がそれぞれ接続されている。
図4は、本実施形態に係るロボット装置のブロック図である。なお、本実施形態に係るロボットアーム機構200の関節部J1,J2,J3,J4,J5、J6には、アクチュエータとして、例えばステッピングモータが設けられている。これらステッピングモータには、モータドライバ201、203,205、207,209、211がそれぞれ電気的に接続されている。また、これらステッピングモータ201、203,205、207,209、211のドライブシャフトには、一定の回転角ごとに出力されるパルスをカウンタで加減算して角位置を測定するためのロータリーエンコーダ202,204,206,208,210,212がそれぞれ接続されている。
動作制御装置100は、関節部J1-J6の動作制御することにより手先等の注目点の移動を制御するために設けられ、システム制御部101と、操作部インターフェース(I/F)102と、ドライバ制御部103と、現在位置・姿勢計算部104、軌道計算部105と、指令値計算部106とを有する。
システム制御部101は、CPU(Central Processing Unit)と半導体メモリ等を有し、動作制御装置100を統括して制御する。システム制御部101には、制御/データバス109を介して各部が接続されている。
システム制御部101は、CPU(Central Processing Unit)と半導体メモリ等を有し、動作制御装置100を統括して制御する。システム制御部101には、制御/データバス109を介して各部が接続されている。
動作制御装置100には、操作部インターフェース102を介して操作部300が接続されている。操作部300は、手首部4に取り付けられたロボットハンド(手先効果器)の移動や姿勢の変更をユーザが入力するための入力インターフェースとして機能する。動作制御装置100では例えばロボットハンドの把持中心点に注目してその点(手先基準点)を制御点(手先座標系Σhの原点)として移動・姿勢変更等の計算処理を実行する。動作制御装置100には、操作部300を介して、手先の最終目標位置等が入力される。操作部300は、例えば、スイッチ、マウス、キーボード、トラックボールおよびタッチパネル等の入力デバイスを有する。
ドライバ制御部103は、モータドライバ201、203,205、207,209、211を統括して制御する。ドライバ制御部103は、後述の指令値計算部106で関節部J1-J6ごとに計算された指令値に応じた制御信号をモータドライバ201、203,205、207,209、211各々に対して送信する。
現在位置・姿勢計算部104は、関節部J1,J2,J3,J4,J5、J6各々の関節変数に基づいて、アーム構造のリンクパラメータに応じて既定された同次変換行列に従って順運動学によりロボット座標系から見た手先基準点の位置と手先姿勢とを計算する。なお関節変数とは、関節部J1,J2,J4,J5、J6では基準位置からの正負の回転角度であり、関節部J3であれば最も収縮した状態からの伸張距離(直動変位)である。
図3に示すように、ロボット座標系Σbは第1関節部J1の第1回転軸RA1上の任意位置を原点とした座標系である。ロボット座標系Σbにおいて、直交3軸(Xb、Yb,Zb)が規定されている。Zb軸は第1回転軸RA1に平行な軸である。Xb軸とYb軸とは互いに直交し、且つZb軸に直交する軸である。手先座標系Σhは、手首部4に取り付けられたロボットハンドの任意位置(手先基準点)を原点とした座標系である。例えば、ロボットハンドが2指ハンドのとき、手先基準点の位置は2指先間中央位置に規定される。手先座標系Σhにおいて、直交3軸(Xh、Yh,Zh)が規定されている。Zh軸は第6回転軸RA6に平行な軸である。Xh軸とYh軸とは互いに直交し、且つZh軸に直交する軸である。例えば、Xh軸は、ロボットハンドの前後方向に平行な軸である。手先姿勢とは、手先座標系Σhのロボット座標系Σbに対する直交3軸各々周りの回転角(Xh軸周りの回転角(ヨウ角)α、Yh軸周りの回転角(ピッチ角)β、Zh軸周りの回転角(ロール角)γとして与えられる。
現在位置・姿勢計算部104は、エンコーダ202,204,206,208,210,212のエンコーダパルスに基づいて、ロボットアーム機構200の関節変数ベクトル-θを計算する。関節変数ベクトル-θは回転関節部J1,J2,J4、J5,J6の関節角度θJ1,θJ2,θJ4、θJ5、θJ6と直動伸縮関節部J3の伸縮長LJ3とで与えられる関節変数の組(θJ1,θJ2,LJ3、θJ4、θJ5、θJ6)である。現在位置・姿勢計算部104は、同次変換行列K(パラメータ(θJ1,θJ2,LJ3、θJ4、θJ5、θJ6))により、ロボット座標系Σbから見た手先基準点の位置(x、y、z)と手先姿勢(α、β、γ)とを計算する。同次変換行列Kは、手先座標系Σhとロボット座標系Σbとの関係を定義する行列式である。同次変換行列Kは、ロボットアーム機構200を構成するリンク間の関係(リンク長とリンクのねじれ角)と関節部の軸間の関係(リンク間距離とリンクの間角度)とで決まる。例えば、現在位置・姿勢計算部104は、現在のロボットアーム機構200の関節変数ベクトル-θ0(θ0-J1,θ0-J2,L0-J3、θ0-J4、θ0-J5、θ0-J6)を同次変換行列Kに代入することにより、ロボット座標系Σbから見た手先基準点の現在位置x0(x0、y0、z0)と手先姿勢φ0(α0、β0、γ0)とを計算する。
軌道計算部105は、手先基準点(以下単に手先という)の現在位置・姿勢と手先の最終目標位置・姿勢とに基づいて、単位時間(制御周期、例えば10ms)毎の手先の目標位置を計算する。目標位置を与えることで、目標位置で規定された位置と姿勢をとって、手先を移動させることができる。手先の現在位置・姿勢は、現在位置・姿勢計算部104による計算処理から得られる。手先の最終目標位置・姿勢は、例えば、操作部300を介してユーザにより入力される。
軌道計算部105は、予めプリセットされている、手先の現在位置x0、現在姿勢φ0、手先の最終目標位置X1、最終目標姿勢Φ1をパラメータとする多項式に、各パラメータを代入することにより手先の軌道(以下、手先軌道という)を計算し、その手先軌道上に単位時間Δt毎の目標位置を計算する。軌道計算方法としては任意の方法が採用される。単位時間Δtは制御周期として固定値であり、ユーザにより、例えば10msに設定されている。軌道計算部105は、手先を現在位置から最終目標位置への移動に要する目標時間幅Tと単位時間Δtとに基づいて、目標位置の数m(=T/Δt)を決定し、単位時間Δt毎の手先軌道上の手先の目標位置(x1、x2・・・xm(m=t1/Δt))を計算する。例えば、目標位置x1は、手先基準点の位置x1と手先姿勢φ1との両方を与えるパラメータとする。目標時間Tは、例えば、ユーザにより直接入力されてもよいし、ユーザが操作部300を操作している期間に応じて決定されてもよいし、手先の現在位置x0と手先の最終目標位置X1との距離に応じて自動的に決定されてもよい。例えば、手先軌道は、手先の現在位置x0と手先の最終目標位置X1とを結ぶ直線軌道として計算される。
指令値計算部106は、手先の現在位置と手先の最終目標位置との間の複数の目標位置に関する複数の関節変数ベクトルを計算する。なお、関節変数ベクトルとは関節部J1-J6の6つの関節変数、つまり回転関節部J1、J2、J4-J6の回転角と直動伸縮関節部J3のアーム伸縮長とからなる。指令値計算部106の計算処理については後述する。図5は、図4の動作制御装置100による動作制御の流れ図である。
(工程S11)現在の関節変数ベクトル-θ0を計算
システム制御部101の制御に従って、現在位置・姿勢計算部104により、エンコーダ202,204,206,208,210,212から入力されたエンコーダパルスに基づいて、現在の関節変数ベクトル-θ0(θ0-J1、θ0-J2、L0-J3、θ0-J4、θ0-J5、θ0-J6)が計算される。
(工程S12)手先の現在位置x0を計算
システム制御部101の制御に従って、現在位置・姿勢計算部104により、現在の関節変数ベクトル-θ0に基づいて、ロボット座標系Σb上の手先の現在位置x0(x0、y0、z0)と手先姿勢φ0(α0、β0、γ0)が計算される。
システム制御部101の制御に従って、現在位置・姿勢計算部104により、エンコーダ202,204,206,208,210,212から入力されたエンコーダパルスに基づいて、現在の関節変数ベクトル-θ0(θ0-J1、θ0-J2、L0-J3、θ0-J4、θ0-J5、θ0-J6)が計算される。
(工程S12)手先の現在位置x0を計算
システム制御部101の制御に従って、現在位置・姿勢計算部104により、現在の関節変数ベクトル-θ0に基づいて、ロボット座標系Σb上の手先の現在位置x0(x0、y0、z0)と手先姿勢φ0(α0、β0、γ0)が計算される。
(工程S13)手先の目標位置(x1、x2、・・・xm(=X1))を計算
システム制御部101の制御に従って、軌道計算部105により、手先の現在位置x0・手先姿勢φ0から手先の最終目標位置X1・手先姿勢Φ1までの手先軌道が計算され、その手先軌道上に単位時間Δt毎の手先の目標位置(x1、x2、・・・xm(=X1))が計算される。
(工程S14)目標の関節変数ベクトル(-θ1、-θ2、・・・-θm)を計算
システム制御部101の制御に従って、指令値計算部106により、手先の目標位置(x1、x2、・・・xm(=X1))にそれぞれ対応する関節変数ベクトル(-θ1、-θ2、・・・-θm)が計算される。指令値計算部106による計算処理手順の詳細は後述する。
(工程S15)目標の関節変数ベクトル(-θ1、-θ2、・・・-θm)を出力
ドライバ制御部103から、モータドライバ201、203、205、207、209、211に対して、指令値として目標の関節変数ベクトル(-θ1、-θ2、・・・-θm)が所定の制御周期Δt(例えば、10ms)で順次出力される。
システム制御部101の制御に従って、軌道計算部105により、手先の現在位置x0・手先姿勢φ0から手先の最終目標位置X1・手先姿勢Φ1までの手先軌道が計算され、その手先軌道上に単位時間Δt毎の手先の目標位置(x1、x2、・・・xm(=X1))が計算される。
(工程S14)目標の関節変数ベクトル(-θ1、-θ2、・・・-θm)を計算
システム制御部101の制御に従って、指令値計算部106により、手先の目標位置(x1、x2、・・・xm(=X1))にそれぞれ対応する関節変数ベクトル(-θ1、-θ2、・・・-θm)が計算される。指令値計算部106による計算処理手順の詳細は後述する。
(工程S15)目標の関節変数ベクトル(-θ1、-θ2、・・・-θm)を出力
ドライバ制御部103から、モータドライバ201、203、205、207、209、211に対して、指令値として目標の関節変数ベクトル(-θ1、-θ2、・・・-θm)が所定の制御周期Δt(例えば、10ms)で順次出力される。
次に、工程S14の指令値計算部106による計算処理について図6を参照して説明する。図6は、図5の工程S14の流れ図である。
(工程S141)初期化(n=0)
ここでは説明の便宜上、変数nを用いる。まずこの変数nがゼロ値に初期化される。
(工程S142)ヤコビアン逆行列J-1(-θn)の計算
本実施形態のアーム機構はその構造上特異点が存在せず、従ってヤコビアン逆行列が常に存在する。ヤコビアン逆行列は、手先速度(手先位置・姿勢の微小変化)を関節角速度(関節角度・伸縮長の微小変化)に変換する行列である。ヤコビアン逆行列は、手先基準点の位置・手先姿勢を表すベクトルの関節変数による編微分で与えられる。指令値計算部106は、工程S11で現在位置・姿勢計算部104により計算された現在の関節変数ベクトル-θn(θn-J1、θn-J2、Ln-J3、θn-J4、θn-J5、θn-J6)により、アーム構造のリンクパラメータによるヤコビアン逆行列J-1(-θn)を計算する。
(工程S141)初期化(n=0)
ここでは説明の便宜上、変数nを用いる。まずこの変数nがゼロ値に初期化される。
(工程S142)ヤコビアン逆行列J-1(-θn)の計算
本実施形態のアーム機構はその構造上特異点が存在せず、従ってヤコビアン逆行列が常に存在する。ヤコビアン逆行列は、手先速度(手先位置・姿勢の微小変化)を関節角速度(関節角度・伸縮長の微小変化)に変換する行列である。ヤコビアン逆行列は、手先基準点の位置・手先姿勢を表すベクトルの関節変数による編微分で与えられる。指令値計算部106は、工程S11で現在位置・姿勢計算部104により計算された現在の関節変数ベクトル-θn(θn-J1、θn-J2、Ln-J3、θn-J4、θn-J5、θn-J6)により、アーム構造のリンクパラメータによるヤコビアン逆行列J-1(-θn)を計算する。
(工程S143)手先速度-xn+1の計算
現在の手先位置(現在の目標位置)xn、次の手先位置(単位時間Δt後の次の目標位置)xn+1、単位時間Δtに基づいて、手先速度・xn+1が計算される。
(工程S144)関節角速度-・θn+1の計算
工程S143で計算された手先速度-xn+1がヤコビアン逆行列J-1(-θn)により、関節角速度-・θn+1に変換される。
現在の手先位置(現在の目標位置)xn、次の手先位置(単位時間Δt後の次の目標位置)xn+1、単位時間Δtに基づいて、手先速度・xn+1が計算される。
(工程S144)関節角速度-・θn+1の計算
工程S143で計算された手先速度-xn+1がヤコビアン逆行列J-1(-θn)により、関節角速度-・θn+1に変換される。
(工程S145)目標の関節変数ベクトル-θn+1の計算
繰り返し回数が前の工程S145で計算された(変数n=0の場合は工程S11で計算された)関節変数ベクトル-θn、工程S144で計算された関節角速度-・θn+1、単位時間Δtに基づいて、次の関節変数ベクトル-θn+1が計算される。関節角速度-・θn+1に単位時間Δtを乗算することにより、単位時間Δtの間の各関節の変位量が計算される。移動直前の関節変数ベクトル-θnに関節の変位量を加算することにより、単位時間Δt経過後の関節変数ベクトル-θn+1が計算される。
繰り返し回数が前の工程S145で計算された(変数n=0の場合は工程S11で計算された)関節変数ベクトル-θn、工程S144で計算された関節角速度-・θn+1、単位時間Δtに基づいて、次の関節変数ベクトル-θn+1が計算される。関節角速度-・θn+1に単位時間Δtを乗算することにより、単位時間Δtの間の各関節の変位量が計算される。移動直前の関節変数ベクトル-θnに関節の変位量を加算することにより、単位時間Δt経過後の関節変数ベクトル-θn+1が計算される。
(工程S146)処理継続の判定
変数nが繰り返し回数(m-1)のとき、指令値計算部106による計算処理は終了される。一方、変数(n)が繰り返し回数(m-1)ではない場合、工程S147に処理が移行される。
変数nが繰り返し回数(m-1)のとき、指令値計算部106による計算処理は終了される。一方、変数(n)が繰り返し回数(m-1)ではない場合、工程S147に処理が移行される。
(工程S147)変数の繰り上げn←n+1
変数nを(n+1)に繰り上げ、工程S142に処理が戻る。
変数nを(n+1)に繰り上げ、工程S142に処理が戻る。
以上説明した指令値計算部106による計算処理により、手先の目標位置(x1、x2、・・・xm)にそれぞれ対応する関節変数ベクトル(-θ1、-θ2、・・・-θm)が計算される。
次に、本実施形態に係るロボット装置の動作制御装置100による動作制御の効果を図7を参照して説明する。本実施形態に係るロボット装置の動作制御装置100は、手先の現在位置と手先の次の目標位置とに基づいて、手先の目標位置に対応する関節変数(関節角度、伸張距離)θJ1,θJ2,LJ3、θJ4、θJ5、θJ6を計算し、その関節変数θJ1,θJ2,LJ3、θJ4、θJ5、θJ6をそれぞれ指令値としてモータドライバ201、203,205、207,209、211に対して入力する。モータドライバ201、203,205、207,209、211は、各関節部の関節角度、伸張距離を目標値としてアクチュエータ(ステッピングモータ)を駆動する。
図5の動作制御(位置指令)による効果を速度指令の場合と比較して説明するために第1関節部J1の関節角度の時間変化を示す図である。図7では、一例として第1関節部J1の関節角度θJ1に関して、モータドライバ201に対して位置(関節角度)を指令値としたときの時間変化を、速度(関節角速度)を指令値としたときの時間変化とともに示している。第1関節部J1のモータドライバ201は、ドライバ制御部103から時刻t0、t1、t2、・・・・tm-1に、それぞれ指令値として関節角度θ1-J1、θ2-J1、θ3-J1、・・・θm-J1が与えられる。図7に示すように、モータドライバ201は、時刻t0から単位時間Δt経過後の時刻t1に、第1関節部J1の関節角度が関節角度θ1-J1に到達するために必要なパルスをステッピングモータに供給する。これにより、第1関節部J1の関節角度は、時刻t0から単位時間Δt経過後の時刻t1に関節角度θ1-J1に到達する。ここで重要なのは、何らかの要因で、第1関節部J1の関節角度が、時刻t0から単位時間Δt経過後の時刻t1に関節角度θ1-J1に回転していなくても、計算上の予定した関節角度θ1-J1と実際の関節角度との間の誤差がその後に累積しない点にある。すなわち、モータドライバ201は、時刻t0から単位時間Δt経過後の時刻t1に関節角度θ1-J1に到達していなくても、次の時刻t1から単位時間Δt経過後の時刻t2に、第1関節部J1の関節角度が関節角度θ2-J1に到達するために必要なパルスをステッピングモータに供給する。これにより、第1関節部J1の関節角度は、時刻t1に予定した関節角度θ1-J1まで回転していなくても、時刻t1における実際の関節角度から、単位時間Δt経過後の時刻t2に関節角度θ1-J1に到達する。
図5の動作制御(位置指令)による効果を速度指令の場合と比較して説明するために第1関節部J1の関節角度の時間変化を示す図である。図7では、一例として第1関節部J1の関節角度θJ1に関して、モータドライバ201に対して位置(関節角度)を指令値としたときの時間変化を、速度(関節角速度)を指令値としたときの時間変化とともに示している。第1関節部J1のモータドライバ201は、ドライバ制御部103から時刻t0、t1、t2、・・・・tm-1に、それぞれ指令値として関節角度θ1-J1、θ2-J1、θ3-J1、・・・θm-J1が与えられる。図7に示すように、モータドライバ201は、時刻t0から単位時間Δt経過後の時刻t1に、第1関節部J1の関節角度が関節角度θ1-J1に到達するために必要なパルスをステッピングモータに供給する。これにより、第1関節部J1の関節角度は、時刻t0から単位時間Δt経過後の時刻t1に関節角度θ1-J1に到達する。ここで重要なのは、何らかの要因で、第1関節部J1の関節角度が、時刻t0から単位時間Δt経過後の時刻t1に関節角度θ1-J1に回転していなくても、計算上の予定した関節角度θ1-J1と実際の関節角度との間の誤差がその後に累積しない点にある。すなわち、モータドライバ201は、時刻t0から単位時間Δt経過後の時刻t1に関節角度θ1-J1に到達していなくても、次の時刻t1から単位時間Δt経過後の時刻t2に、第1関節部J1の関節角度が関節角度θ2-J1に到達するために必要なパルスをステッピングモータに供給する。これにより、第1関節部J1の関節角度は、時刻t1に予定した関節角度θ1-J1まで回転していなくても、時刻t1における実際の関節角度から、単位時間Δt経過後の時刻t2に関節角度θ1-J1に到達する。
一方、モータドライバ201に指令値として関節角速度を入力したとき、移動開始時や速度変化に伴う慣性誤差が累積し、第1関節部J1の実際の関節角度は、計算上の予定した関節角度から外れてしまうことがある。一方、本実施形態に係るロボット装置の動作制御装置100のように、モータドライバ201に指令値として関節角度を入力することで、モータドライバ201は、単位時間Δtで第1関節部J1の関節角度を指令された関節角度まで移動するようにステッピングモータを駆動する。これにより、第1関節部J1の動作制御の途中で位置誤差が発生した場合であっても、その位置誤差は累積しない。したがって、本実施形態に係るロボット装置は、ロボットアームの移動制御において位置誤差を解消又は減少させることができる。
つまり、本実施形態では、目標位置間の手先速度を計算し、それをヤコビアン逆行列により関節角速度に変換し(従来ではこの関節角速度を指令値としていた)、その関節角速度から関節変数ベクトルを求め、関節変数ベクトルを指令値としてモータドライブに供給することにより、モータドライブでは、指令された関節変数ベクトルに単位時間で移動するようモータを駆動するので関節角速度を指令値とする場合と異なり位置誤差は原理的に生じない、又は非常に低下させることができ、フィードバック制御は不要になる。また位置誤差が生じないことから目標位置を順次進めながら次々と目標位置の関節変数ベクトルを計算し、つまり事前に全目標位置の関節変数ベクトルを計算させることも可能になる。また同次変換行列を用いた逆運動学により関節変数ベクトルを求めることができるが、その場合非線形演算であるがゆえに計算処理が非常に複雑になり、また解析的に解を一意に求める事ができないことがあるが、ヤコビアン逆行列は線形演算であり計算処理は非常に簡素であり、そのような問題は生じない。もっとも本実施形態の直動伸縮アーム機構には通常の回転関節を組み合わせたアーム構造とは異なり冗長性が存在しないので、無数解が生じる事がないのでヤコビアン逆行列で簡素に処理することができる。
本発明のいくつかの実施形態を説明したが、これらの実施形態は、例として提示したものであり、発明の範囲を限定することは意図していない。これら実施形態は、その他の様々な形態で実施されることが可能であり、発明の要旨を逸脱しない範囲で、種々の省略、置き換え、変更を行うことができる。これら実施形態やその変形は、発明の範囲や要旨に含まれると同様に、特許請求の範囲に記載された発明とその均等の範囲に含まれるものである。
100…動作制御装置、200…ロボットアーム機構、300…操作部、101…システム制御部、102…操作部I/F、103…ドライバ制御部、104…現在位置・姿勢計算部、105…軌道計算部、106…指令値計算部、109…制御/データバス、201、203、205、207、209、211…モータドライバ、202,204,206,208,210,212…エンコーダ
Claims (8)
- 多関節アーム機構を構成する複数の関節部のアクチュエータにそれぞれ対応する複数のモータドライバに複数の指令値をそれぞれ出力する多関節アーム機構の動作制御装置において、
前記複数の関節部にそれぞれ対応する現在の複数の関節変数から前記多関節アーム機構上の注目点の現在位置を計算する現在位置計算部と、
前記現在位置から最終目標位置までの前記注目点の軌道を計算する軌道計算部と、
前記軌道上の単位時間ごとの複数の目標位置各々から次の目標位置までの距離と前記単位時間とに基づいて前記注目点の移動速度を計算し、前記計算された移動速度を前記多関節アーム機構の構造に応じたヤコビアン逆行列により前記複数の関節部にそれぞれ対応する複数の関節角速度に変換するとともに、前記計算された複数の関節角速度と前記単位時間とから前記次の目標位置に対応する複数の関節変数を前記複数の指令値として計算する指令値計算部と、
前記複数の指令値を前記複数のモータドライバにそれぞれ出力する指令値出力部とを具備する多関節アーム機構の動作制御装置。 - 前記指令値計算部は、前記現在位置から前記最終目標位置に向かって順次連鎖しながら前記複数の目標位置各々について前記複数の関節変数を計算する請求項1記載の多関節アーム機構の動作制御装置。
- 前記指令値出力部は、前記単位時間経過ごとに前記次の目標位置に関する前記複数の指令値を前記モータドライバに順次出力する請求項1記載の多関節アーム機構の動作制御装置。
- 前記指令値出力部は、前記指令値計算部により前記複数の目標位置の全てに関する前記複数の関節変数の計算処理と並行して前記複数の指令値を前記モータドライバに順次出力する請求項3記載の多関節アーム機構の動作制御装置。
- 前記複数の関節部は回転関節である請求項1記載の多関節アーム機構の動作制御装置。
- 前記複数の関節部には肘関節部がなく前記複数の関節部の少なくとも一は直動伸縮関節部である請求項1記載の多関節アーム機構の動作制御装置。
- 前記直動伸縮関節部は、
屈曲可能に連結された断面コ字形状を有する複数の第1連結コマと、
屈曲可能に連結された平板形状を有する複数の第2連結コマと、前記複数の第2連結コマのうち先端の第2連結コマは前記複数の第1連結コマのうち先端の第1連結コマと接続される、前記第1連結コマはその上部において前記第2連結コマに接合されることにより屈曲が拘束されて柱状体が構成される、前記第1、第2連結コマの分離により前記柱状体が解除される、
前記第1連結コマを前記第2連結コマに接合し前記柱状体を構成するととともに前記柱状体を支持する射出部とを有する請求項6記載の多関節アーム機構の動作制御装置。 - 複数の関節部を備える多関節アーム機構と、
前記多関節アーム機構を構成する複数の関節部のアクチュエータにそれぞれ対応する複数のモータドライバに複数の指令値をそれぞれ出力する動作制御装置とを有するロボット装置において、
前記動作制御装置は、
前記複数の関節部にそれぞれ対応する現在の複数の関節変数から前記多関節アーム機構上の注目点の現在位置を計算する現在位置計算部と、
前記現在位置から最終目標位置までの前記注目点の軌道を計算する軌道計算部と、
前記軌道上の単位時間ごとの複数の目標位置各々から次の目標位置までの距離と前記単位時間とに基づいて前記注目点の移動速度を計算し、前記計算された移動速度を前記多関節アーム機構の構造に応じたヤコビアン逆行列により前記複数の関節部にそれぞれ対応する複数の関節角速度に変換するとともに、前記計算された複数の関節角速度と前記単位時間とから前記次の目標位置に対応する複数の関節変数を前記複数の指令値として計算する指令値計算部と、
前記複数の指令値を前記複数のモータドライバにそれぞれ出力する指令値出力部とを有するロボット装置。
Applications Claiming Priority (2)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP2015093438A JP6595206B2 (ja) | 2015-04-30 | 2015-04-30 | 多関節アーム機構の動作制御装置及びロボット装置 |
JP2015-093438 | 2015-04-30 |
Publications (1)
Publication Number | Publication Date |
---|---|
WO2016175157A1 true WO2016175157A1 (ja) | 2016-11-03 |
Family
ID=57198896
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
PCT/JP2016/062862 WO2016175157A1 (ja) | 2015-04-30 | 2016-04-24 | 多関節アーム機構の動作制御装置及びロボット装置 |
Country Status (3)
Country | Link |
---|---|
JP (1) | JP6595206B2 (ja) |
TW (1) | TW201641233A (ja) |
WO (1) | WO2016175157A1 (ja) |
Cited By (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
TWI608913B (zh) * | 2017-02-17 | 2017-12-21 | Hiwin Tech Corp | Guide groove gripper device |
CN108568808A (zh) * | 2017-03-09 | 2018-09-25 | 上银科技股份有限公司 | 导槽式夹爪装置 |
CN111052337A (zh) * | 2017-08-04 | 2020-04-21 | 川崎重工业株式会社 | 机器人的诊断方法 |
Families Citing this family (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
TWI662781B (zh) * | 2018-03-29 | 2019-06-11 | 東元電機股份有限公司 | 馬達控制系統及其方法 |
CN108393907A (zh) * | 2018-05-22 | 2018-08-14 | 蒋再男 | 基于位置跟踪器的机械臂遥操作运动映射方法 |
JP6806736B2 (ja) * | 2018-06-05 | 2021-01-06 | ファナック株式会社 | 位置情報表示システム |
Citations (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JPS63289606A (ja) * | 1987-05-21 | 1988-11-28 | Toshiba Corp | 冗長自由度形ロボット |
JP2010102409A (ja) * | 2008-10-21 | 2010-05-06 | Toshiba Corp | ロボット制御装置 |
WO2011152265A1 (ja) * | 2010-05-31 | 2011-12-08 | 独立行政法人産業技術総合研究所 | 直動伸縮アーム機構および当該直動伸縮アーム機構を備えたロボットアーム |
JP2013240847A (ja) * | 2012-05-18 | 2013-12-05 | Ihi Corp | ロボットハンド装置と制御方法 |
-
2015
- 2015-04-30 JP JP2015093438A patent/JP6595206B2/ja active Active
-
2016
- 2016-04-24 WO PCT/JP2016/062862 patent/WO2016175157A1/ja active Application Filing
- 2016-04-28 TW TW105113293A patent/TW201641233A/zh unknown
Patent Citations (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JPS63289606A (ja) * | 1987-05-21 | 1988-11-28 | Toshiba Corp | 冗長自由度形ロボット |
JP2010102409A (ja) * | 2008-10-21 | 2010-05-06 | Toshiba Corp | ロボット制御装置 |
WO2011152265A1 (ja) * | 2010-05-31 | 2011-12-08 | 独立行政法人産業技術総合研究所 | 直動伸縮アーム機構および当該直動伸縮アーム機構を備えたロボットアーム |
JP2013240847A (ja) * | 2012-05-18 | 2013-12-05 | Ihi Corp | ロボットハンド装置と制御方法 |
Cited By (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
TWI608913B (zh) * | 2017-02-17 | 2017-12-21 | Hiwin Tech Corp | Guide groove gripper device |
CN108568808A (zh) * | 2017-03-09 | 2018-09-25 | 上银科技股份有限公司 | 导槽式夹爪装置 |
CN108568808B (zh) * | 2017-03-09 | 2020-07-07 | 上银科技股份有限公司 | 导槽式夹爪装置 |
CN111052337A (zh) * | 2017-08-04 | 2020-04-21 | 川崎重工业株式会社 | 机器人的诊断方法 |
CN111052337B (zh) * | 2017-08-04 | 2023-08-22 | 川崎重工业株式会社 | 机器人的诊断方法 |
Also Published As
Publication number | Publication date |
---|---|
JP2016209937A (ja) | 2016-12-15 |
JP6595206B2 (ja) | 2019-10-23 |
TW201641233A (zh) | 2016-12-01 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
WO2016175157A1 (ja) | 多関節アーム機構の動作制御装置及びロボット装置 | |
JP6659238B2 (ja) | ロボット装置及びステッピングモータ制御装置 | |
JP6443875B2 (ja) | ロボットアーム機構 | |
WO2016204099A1 (ja) | ロボット装置及びモータ制御装置 | |
JP6468804B2 (ja) | ロボットアーム機構 | |
WO2016104807A1 (ja) | ロボットアーム機構及び直動伸縮機構 | |
US20180207803A1 (en) | Robot apparatus | |
WO2016084932A1 (ja) | ロボット装置及びロボット制御装置 | |
JP2016120586A (ja) | ロボットシステム及びロボット装置 | |
WO2016190141A1 (ja) | ロボット装置及びステッピングモータ制御装置 | |
JP4970492B2 (ja) | 多関節ロボット | |
JP2017052072A (ja) | ロボット装置 | |
TW201703949A (zh) | 機器人裝置 | |
JP2016101651A (ja) | ロボット装置 | |
JP6748179B2 (ja) | ロボットアーム機構 | |
JP2018195214A (ja) | 操作装置及びその運転方法 | |
JP2016168646A (ja) | 直動伸縮機構及びロボットアーム機構 | |
JP2005230952A (ja) | 制御方法および制御装置 | |
JP2004011831A (ja) | パラレルリンク機構 |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
121 | Ep: the epo has been informed by wipo that ep was designated in this application |
Ref document number: 16786434 Country of ref document: EP Kind code of ref document: A1 |
|
NENP | Non-entry into the national phase |
Ref country code: DE |
|
122 | Ep: pct application non-entry in european phase |
Ref document number: 16786434 Country of ref document: EP Kind code of ref document: A1 |