JP6076873B2 - robot - Google Patents

Description

  The present invention relates to an arm robot or a robot including an arm.

  The body of the body (base body) for executing tasks involving handshake with humans on the robot arm body (arm) while stabilizing the posture of the robot by controlling the movement of the robot body. Technical techniques for determining posture have been proposed. Specifically, based on the result of detecting or estimating the external force acting on the arm body when executing the robot arm task task, the optimal posture of the body is generated by this technique, and the leg motion Is controlled (see Patent Document 1).

Japanese Patent No. 4384221

  However, by controlling the operation of the robot according to a strict kinematics model, the position / posture of the waist is directly determined according to the posture of the arm body. For this reason, the movement of the robot becomes hard and its posture may become unstable.

  Therefore, the present invention provides a robot capable of reducing the influence of external force on the leg and stabilizing the posture by changing the posture of the base body while flexibly changing the posture of the arm according to the external force. To do.

  The present invention includes a base body, a support mechanism that supports one or both of the position and posture of the base body in a controllable manner, a plurality of links and a plurality of joint mechanisms that connect the plurality of links. And a control device configured to control operations of the support mechanism and the joint mechanism.

In the robot of the present invention, the control device controls the operation of the arm by giving a torque command value to each of the plurality of joint mechanisms, and measures the joint angles of the plurality of joint mechanisms. A set value of the position and posture of the base body according to a whole body kinematics model based on the value, a restraint force model that defines a virtual mutual restraint force between the base body and the arm, and a position of the arm relative to the base body Based on the posture, the restraining force acting on the base is calculated, and then the set values are corrected according to the restraining force to generate command values for the position and orientation of the base, and according to the command values The operation of the support mechanism is controlled.

  According to the robot of the present invention, the arm operation is controlled according to the torque command value, not the command value such as the angle or angular velocity of each joint mechanism, so that the posture of the arm can be flexibly changed according to the external force. . Further, the operation of the support mechanism can be controlled so that the position and posture of the base body change so as to absorb the movement of the arm according to the virtual restraint force calculated according to the restraint model.

  Therefore, the posture of the robot can be stabilized while flexibly changing the posture of the arm according to various forms of external force. When the support mechanism has a joint mechanism, the operation of the support mechanism may be controlled according to the command value of the joint angle or the joint angular velocity instead of the torque command value of the joint mechanism, unlike the operation of the arm.

  The control device defines the mutual restraining force by a virtual elastic force according to the interval between the end points defined for each of the base body and the arm and a virtual damping force according to the changing speed of the interval between the end points. It is preferable to control the operation of the support mechanism using a constraint model.

  The control device may control the operation of the arm by giving a torque command value including a gravity compensation torque for compensating gravity acting on each of the plurality of links to each of the plurality of joint mechanisms. preferable.

  According to the robot having such a configuration, the posture of the arm can be maintained as it is without changing due to its own weight when no external force is applied to the arm.

The structure explanatory drawing of the robot of this invention. FIG. 3 is a configuration explanatory diagram of a control device of the present invention. FIG. Explanatory drawing regarding the moment calculation method. The conceptual explanatory drawing regarding the operation control of a robot. 1 is a configuration explanatory diagram of a legged mobile robot which is an embodiment of a robot. FIG. Explanatory drawing regarding a restraint model. Explanatory drawing regarding the 1st verification result of operation | movement of a robot. Explanatory drawing regarding the 2nd verification result of operation | movement of a robot.

(Robot configuration)
The robot 1 shown in FIG. 1 includes a base B1, a support mechanism B2 that supports the base B1, an arm A extending from the base B1, and an actuator (not shown) that is a driving source of the robot. And a control device 2 for controlling. The arm A includes a plurality of joint mechanisms J _i (i = 1 to N) and a plurality of links L _i connected via the plurality of joint mechanisms J _i . The number of joint mechanisms and the degrees of freedom (1-3) of each joint mechanism can be arbitrarily designed.

Depending on the operation of the actuator which constitutes the arm A, by about the axis of the joint angle q _i corresponding to the rotational degrees of freedom of each joint mechanism J _i is controlled, the i for the i-1 link L _i-1 The relative posture of the link L _i is adjusted. The robot 1 further includes a joint angle sensor (not shown) that outputs a signal corresponding to each joint angle q _i , and the control device 2 measures each joint angle q _i based on an output signal from each joint angle sensor.

By controlling the operation of the actuator constituting the support mechanism B2, the position P _B1 and the posture θ _B1 of the base B1 in the global coordinate system are controlled. The control device 2 is configured by an electronic control unit (configured by a CPU, ROM, RAM, I / O circuit, etc.) or a computer.

The values of the external force and the like in the i-th link coordinate system (local coordinate system) use the i-th link attitude matrix T (i) = Π _{k = 1 to i} T (k) (“Π” represents a product). Are converted to values in the global coordinate system. The coordinate values of the reference points P _Lk and P _B1 of the constraint force model (described later) defined in each link local coordinate system are converted into the coordinate values of the global coordinate system.

(Configuration of control device)
The control device 2 shown in FIG. 2 includes an arm control element 21, a whole body motion generation element 22, a restraint force calculation element 23, a position / posture command element 24, and a support mechanism control element 25. . Each component of the control device 2 includes a memory and an arithmetic processing unit (CPU) that executes necessary arithmetic processing to be described later after reading necessary data and programs from the memory.

When the robot 1 is a joint-driven robot in which the joint angle q _i of each joint mechanism J _i is controlled by the joint angle command value qcmd, the control device 2 includes a torque converter (US Pat. No. 7,986). , 118B2). The torque command value trq_cmd of each joint mechanism is converted into the joint angle command value qcmd or the joint angular velocity command value dqcmd / dt by the torque converter. Thereby, the robot 1 can be controlled as a pseudo torque command type robot.

(Robot and its control device functions)
(Function of arm control element)
(Torque command in joint space)
The first torque command value (torque command value in the joint space) trq_joint_cmd≡trq1_cmd = [trq1_cmd (1), ..trq1_cmd (i) according to the relational expression (21) representing the dynamic model of the arm A by the arm control element 21 ), ..trq1_cmd (N)] is set.

A (q) (d ² q / dt ² ) + B (q, (dq / dt)) + g (q) = trq1_cmd .. (21).

Angle q = [q (1), .q (i), ..q (N)] of each joint mechanism, joint angular velocity (dq / dt), joint angular acceleration (d ² q / dt ² ), inertia matrix A Each of (q), centrifugal force / Coriolis force B (q, (dq / dt)) and gravity compensation torque g (q) is determined based on an angular trajectory that defines a time-series variation of the angle q. .

According to equation (21), if the joint angle command value qcmd is not output, in response to an external force acting on the arm A at a location other than the control point P _C, the like is passively actuated the arm A 1 Torque command value trq1_cmd is output. Therefore, the link L _i moves passively according to the external force.

The gravity acting on each link L _i is compensated by the gravity compensation term g (q) on the left side of the relational expression (21) regardless of the posture of the robot 1. Therefore, according to the external force F acting on each link L _i, the robot 1 may be passively operated while compensating for the influence of the gravitational force acting on each link L _i. Robot 1 when an external force F is zero, the position and posture of each link L _i, can be maintained without directly changing the position and orientation of the thus arm A.

(Torque command in operation space)
Simultaneously with the torque control in the joint space, the force control in the operation space is simultaneously executed. The control point P _C in the operation space is defined at an arbitrary position during operation. Based on the force Fcmd generated in the arm A at the control point P _C and the inertia matrix Λ _C projected onto the operation space, the force Fcmd ^* generated in the operation space is converted according to the relational expression (22).

Fcmd ^* = Λ _C Fcmd .. (22).
According to the Jacobian matrix J _C defined with respect to the base B1 for an arbitrary control point P _C , the force Fcmd ^* generated in the arm A at the control point P _C is the torque of each joint according to the relational expression (23). It is distributed as command value trq_task_cmd≡trq2_cmd = [trq2_cmd (1), ..trq2_cmd (i), ..trq2_cmd (N)]. “ ^T ” represents transposition.

trq2_cmd = Jc ^T Fcmd ^* .. (23).

When the control point P _C is defined in the Nth link L _N as the end effector, the Jacobian matrix J _C defined with reference to the coordinate system of the base B1 is defined according to the relational expression (221). This is a general Jacobian matrix.

Jc = (J1, ..Ji, ..Jn), Ji = (Ji1, Ji2, Ji3, Ji4, Ji5, Ji6) ^T .. (221).

On the other hand, the Jacobian matrix J _C when the control point P _C is at an arbitrary position on the i-th link L _i (i <N) is defined according to the relational expression (222).

  Jc = (J1, ..Ji, 0, ..0) .. (222).

A vector P _{J (i)} representing the position in the global coordinate system of the i-th joint mechanism J (i) for driving the i-th link L (i) where the control point P _C is defined, and a matrix Θ _{J (i} representing the attitude ₎ Is calculated. It is defined by a vector Pc_offset representing the offset of the control point P _C with respect to the i-th joint mechanism J (i) in the link coordinate system (local coordinate system) whose position and orientation are fixed with respect to the i-th link L (i). . Thereby, the control point P _C = P _{J (i)} + Θ _{J (i)} Pc_offset in the global coordinate system is defined.

As a result, the force command value Fcmd generated in the arm A at the control point P _C on the i-th link L _i becomes the i-th joint mechanism J _i and the joint mechanisms J _{1 to} J _i− closer to the base B1 than this. ₁ is converted into each second torque command value trq2_cmd (k) (k = 1 to i).

(Composition of torque command in joint space and operation space)
A projection matrix N _C ^T onto the zero space represented by the relational expression (231) is used, and a final torque command value trq_cmd is set according to the relational expression (232). Accordingly, the second torque command value trq2_cmd for generating a force on the arm A in the control point P _C, the first torque command value of the joint mechanism J _k of the side close to the substrate B1 than the control point P _C It can be avoided that trq1_cmd (k) interferes with each other.

Nc ^T = I-Jc ^T Jc * ^T , Jc * ^T = ΛcJcA ⁻¹ , Λc ⁻¹ = JcA ⁻¹ Jc ^T .. (231).

trq_cmd = Nc ^T trq1_cmd + trq2_cmd,
trq1_cmd = (trq1_cmd (1), ..trq1_cmd (i), trq1_cmd (i + 1), ..trq1_cmd (N)),
trq2_cmd = (trq2_cmd (1), ..trq2_cmd (i), 0, ..0) .. (232).

Therefore, the external force F acting on the arm A at the control point P _C is the torque command of the joint mechanisms J _{1 to} J _i-1 that are closer to the base B1 than the i-th link L _i and the i-th link L _i. It is reflected in the values trq_cmd (1) to trq_cmd (i). On the other hand, the force F is not reflected in the torque command values trq_cmd (i + 1) to trq_cmd (N) of the joint mechanisms J _{i + 1 to} J _N that are further to the base B1 than the i-th link L _i. .

(Function of whole body movement generation element)
The whole body motion generation element 22 calculates the whole body motion of the robot 1 according to the whole body kinematic model based on the measured value qact of each joint angle q. Thereby, the position P _{B1 [XYZ]} and the posture θ _{B1 [XYZ]} of the base B1 are set.

(Function of binding force calculation element)
Restraining force calculating element 23, by using the virtual force model (VFM) as binding model, virtual restraining force F _CFM acting against the substrate B1 and each link _{L i (k) (k =} 1~M ) Is calculated. According to the virtual force model, by the M virtual restraining elements CFM (k), the substrate B1 and the mutual binding of each link L _i is defined. The number of constraint elements defined for one link L _i need not be limited to 1 and may be plural. The number of links and the number of constraint elements do not need to match. The restraining element CFM (k) is configured by any combination of an arbitrary number of virtual elastic elements (spring elements) and damping elements (damper elements). The configuration of each constraint element CFM (k) may be the same or different.

  For example, as shown in FIG. 3A, the constraining element CFM (k) is composed of a virtual elastic element (elastic coefficient K) and a damping element (damping coefficient D) connected in parallel. Yes. In addition, as shown in FIG. 3B, the constraining element CFM (k) includes a virtual elastic element (elastic coefficient K1) and a damping element (damping coefficient D) connected in parallel. You may be comprised by the further elastic element (elastic coefficient K2) connected in series.

According to restraining force calculating element 23, the distance L (k) and its change rate dL (k substrates B1 end point P _{B1_CFM} of restraining elements CFM (k) (k) and the arm A side end point P _{A_CFM} (k) ) / Dt is calculated.

When the kth constraint element CFM (k) is defined by a virtual elastic element and a damping element connected in parallel (see FIG. 3A), the base B1 by the kth constraint element CFM (k) and the corresponding link L _i mutual restraining force F _CFM (k) is based on the elastic modulus of the elastic element damping coefficient (spring coefficient) K (k) and a damping element (damper coefficient) D (k), the equation (233) Therefore, it is calculated.

F _CFM (k) = K (k) × L (k) + D (k) × (dL (k) / dt) (233).

Instead of the virtual force model, a contact force sensor model (CSM) may be adopted as the restraint force model. In this case, the contact force F _CSM in the global coordinate system (k) is defined as a constraint force F _CFM (k) acting on the i-th link L _i.

(Function of position / posture command element)
The position / posture command element 24 _calculates a position command value P _{B1_cmd [XYZ]} and a posture command value θ _{B1_cmd [XYZ]} of the base B1.

Specifically, the center of gravity P _ICFM = Σ _{k = 1 to M} P _{B1_CFM} (k) of the end point P _{B1_CFM} (k) on the base B1 side of each constraint element CFM (k) is used as a starting point, and each constraint force F _CFM (k ) _Is a total binding force (vector) F _ICFM = Σ _{k = 1 to M} F _CFM (k) (see FIG. 4A). By applying the limit process represented by the relational expression (240) to the total restraining force F _ICFM (magnitude), the primary total restraining force F _ICFM * (vector) is obtained (see FIG. 4B). ).

F _ICFM * = 0 (if F _ICFM <F _min ),
= F _ICFM (if F _{min ≤F ICFM ≤F max} ),
= F _ICFM (if F _max <F _ICFM ) .. (240).

The result of projecting the primary total restraining force F _ICFM * onto the base coordinate system is obtained as the secondary total restraining force F _ICFM ** (see FIG. 4B). The “base coordinate system” is a three-dimensional orthogonal coordinate system in which a representative point such as the center of gravity of the base B1 is the origin, and the trunk rotation center axis of the base B1 is the Z axis (yaw axis). Euler angles δθ _{ICFM [XYZ]} = (δθ _{ICFM [X]} , δθ _{ICFM [Y]} , δθ _{ICFM [Z]} ) are calculated from the secondary total binding force F _ICFM **. The Euler angles and the gain matrix _{M θ = Diag (M θX,} M θY, M θZ) using a relational expression (241) in accordance with angle variation .delta..theta _B1 around the axes _[XYZ] is computed.

_{Δθ B1 [XYZ]} = M _{θ Δθ ICFM [XYZ]} .. (241).

Then, according to the relational expression (242), the final posture command value θ _{B1_cmd [XYZ] of} the base B1 is calculated (see FIG. 5A).

θ _{B1 —} cmd _[XYZ] = δθ _{B1 [XYZ]} + θ _{B1 [XYZ]} .. (242).

A force Fact _[XYZ] acting on the root portion P _{B1_A} (for example, the first joint mechanism J ₁ ) of the arm A is measured based on an output signal of a force sensor (not shown) of the robot 1. Using the force measurement value and the compliance coefficient Kcmp _[XYZ] , the displacement is converted into the displacement amount δP _{B1 [XYZ]} in each axial direction according to the relational expression (243).

δP _{B1 [XYZ]} = Fact _[XYZ] / Kcmp _[XYZ] .. (243).

Then, the final position command value P _{B1 —} cmd _{[XYZ] of} the base B1 is calculated according to the relational expression (244).

P _{B1_} cmd _[XYZ] = _{δP B1 [XYZ]} + P _{B1 [XYZ]} .. (244).

The support mechanism B2 may be inclined with respect to the floor surface due to the influence of the external force F. Therefore, the inclination angle θ _{B2 [XY]} and the angular velocity dθ _{B2 [XY]} / dt of the support mechanism B2 are estimated by an inclination sensor (such as a gyro sensor) of the robot 1 (see FIG. 5). Based on the estimation result, it is converted into the second posture change amount δθ _{B2 —} cmd _[XY] around the X axis and the Y axis according to the relational expression (245). In this case, the final posture command value θ _{B1_cmd [XYZ]} of the base body B1 is calculated in a form in which the second posture change amount δθ _{B2_cmd [XY]} is added (see FIG. 5B).

δθ _{B2 —} cmd _[XY] = K _P — _{B2 [XY]} θ _{B2 [XY]} + K _{V — B2 [XY]} (dθ _{B2 [XY]} / dt) .. (245).

(Function of support mechanism control element)
By controlling the operation of the support mechanism B2 by the support mechanism control element 25, the position P _B1 and the posture θ _B1 of the base body _B1 are controlled. At this time, in order to stabilize the posture of the robot 1, the operation of the support mechanism B2 is controlled so that the ZMP of the robot 1 is included in the support stable region defined by the ground contact region. For example, the position P _B1 and the posture θ _{B1 of the} base body B1 may be controlled by the operation of the support mechanism B2 while maintaining the grounding state (see solid lines in FIGS. 5A and 5B). The position P _B1 and the posture θ _{B1 of the} base body B1 may be controlled by the operation of the support mechanism B2 that changes the contact position (see the broken lines and the hatched arrows in FIGS. 5A and 5B). Further, the support mechanism B2 is controlled not as a torque command type but as a joint angle command type control target.

Example 1
The operation of the robot was verified when the operator grabbed and moved the robot arm. As shown in FIG. 6, the robot is a legged mobile robot. Like a human, the robot is a base B1, a head B0 disposed above the base B1, and an upper part of the base B1 from both upper sides. There are provided left and right arms A that are extended and left and right legs (supporting mechanisms) B2 that are extended downward from the lower part of the base B1 (see Table 03/090978 and Table 03/090979). (See publications).

  The base B1 is composed of an upper part and a lower part that are connected vertically so as to be relatively rotatable about the yaw axis. The head B0 can move, such as rotating around the yaw axis with respect to the base B1.

  The arm A includes a first link L1 and a second link L2. A hand (third link) L3 is provided at the tip of each of the left and right arms A. The base B1 and the first link L1 are connected via a first joint mechanism (shoulder joint mechanism) J1, and the first link L1 and the second link L2 are connected via a second joint mechanism (elbow joint mechanism) J2. The second link L2 and the hand L3 are connected via a third joint mechanism (wrist joint mechanism) J3. The first joint mechanism J1 has a degree of freedom of rotation about the roll, pitch and yaw axes, the second joint mechanism J2 has a degree of freedom of rotation about the pitch axis, and the third joint mechanism J3 has a roll, pitch, It has a degree of freedom of rotation about the yaw axis.

  The leg B2 includes a first leg link BL1, a second leg link BL2, and a foot portion BL3. The base B1 and the first leg link BL1 are connected via a hip joint mechanism (first leg joint mechanism) BJ1, and the first leg link BL1 and the second leg link BL2 are connected to a knee joint mechanism (second leg joint). Mechanism) The second leg link BL2 and the foot BL3 are connected via the BJ2 and the foot joint mechanism (third leg joint mechanism) BJ3.

  The hip joint mechanism BJ1 has a degree of freedom of rotation about the roll, the pitch, and the roll axis, the knee joint mechanism BJ2 has a degree of freedom of rotation about the pitch axis, and the ankle joint mechanism BJ3 rotates about the roll and the pitch axis. Has a degree of freedom. The hip joint mechanism BJ1, the knee joint mechanism BJ2, and the ankle joint mechanism BJ3 constitute a “leg joint mechanism group”. The translational and rotational degrees of freedom of each joint mechanism included in the leg joint mechanism group may be changed as appropriate. In addition, any one of the hip joint mechanism BJ1, the knee joint mechanism BJ2, and the ankle joint mechanism BJ3 may be omitted, and the leg joint mechanism group may be configured by a combination of the remaining two joint mechanisms. . Further, when the leg B2 has a second leg joint mechanism different from the knee joint, the leg joint mechanism group may be configured to include the second leg joint mechanism. An elastic material is provided on the bottom of the foot portion BL3 to alleviate the impact at the time of landing (see Japanese Patent Application Laid-Open No. 2001-129774).

  The operation of the leg B2 of the robot 1 is controlled according to a posture stabilization technique described in Japanese Patent No. 3132156. The operation of arm A is controlled according to the passive joint drive mode in which only the gravitational torque of the present invention is compensated.

  In this embodiment, as shown in FIG. 7, two constraint elements having different end points are defined between the base B1 and each of the left and right first links L1 [R] and L1 [L]. Yes. In addition, one constraint element is defined between the base body B1 and each of the left and right second links L2 [R] and L2 [L]. Only the posture of the base B1 is controlled. The constraint condition for the reference position (waist) of the base B1 is not set.

  When an external force is applied to the right arm A [R] as shown by the black arrows in each of FIGS. 8A to 8C (for example, the operator holds the right arm A [R] The operation of the robot 1 was verified. As a result, as indicated by white arrows in FIGS. 8A to 8C, the external force is absorbed by the posture changes around the X axis, Y axis, and Z axis of the base B1. Thus, it was confirmed that the operation of the leg B2 was controlled.

(Example 2)
In the second embodiment, for the same legged mobile robot 1 (see FIG. 6) as in the first embodiment, as shown by the black arrows in FIGS. 9A and 9B, the right arm A [R ] And the operation of the robot 1 when an external force is applied to each of the left arm A [L]. As a result, the position of the foot portion BL3 as shown by the white arrow in each of FIGS. 9A and 9B is absorbed so that an external force component that cannot be absorbed only by the posture change of the base B1 is absorbed. It was confirmed that the operation of the leg B2 is controlled so that the change is made.

1 ‥ robot, 2 ‥ controller, A ‥ arm, B1 ‥ substrate, B2 ‥ support mechanism, J _i ‥ joint mechanism, L _i ‥ link.

Claims (3)

A base, a support mechanism that controllably supports one or both of the position and orientation of the base, and an arm that extends from the base and has a plurality of links and a plurality of joint mechanisms that connect the plurality of links A control device configured to control operations of the support mechanism and the joint mechanism,
The control device controls the operation of the arm by giving a torque command value to each of the plurality of joint mechanisms,
Based on the measured values of the joint angles of the plurality of joint mechanisms, the set values of the position and posture of the base body are determined according to a whole body kinematic model,
Based on a restraint force model that defines a virtual mutual restraint force between the base body and the arm and a posture of the arm with respect to the base body, a restraint force acting on the base body is calculated, and then the restraint force is determined. The robot generates a command value for the position and orientation of the base body by correcting the set value, and controls the operation of the support mechanism according to the command value. The robot according to claim 1, wherein
The control device defines the mutual restraining force by a virtual elastic force according to the interval between the end points defined for each of the base body and the arm and a virtual damping force according to the changing speed of the interval between the end points. A robot characterized by controlling the operation of the support mechanism using a constraint model. The robot according to claim 1 or 2,
The control device controls the operation of the arm by giving a torque command value including a gravity compensation torque for compensating the gravity acting on each of the plurality of links to each of the plurality of joint mechanisms. Characteristic robot.