JP2013000816A - Robot control device, and method and program for controlling the same - Google Patents

Abstract

PROBLEM TO BE SOLVED: To rapidly and stably perform convergence operation of robot control.SOLUTION: This robot control device includes: a command value generation means to generates a position command value of a predetermined part of a robot; a first deviation calculation means to calculate a deviation between the position command value generated by the command value generation means and a control position of the predetermined part; a force reference value calculation means to calculate a force reference value being a reference value of force generated in the predetermined part based on the deviation calculated by the first deviation calculation means; a joint angle calculation means to calculate each joint torque value based on the force reference value calculated by the force reference value calculation means and a transposition Jacobian matrix and calculate each joint angle based on each calculated joint torque value; and a forward kinematic arithmetic means to calculate a control position of the predetermined part by performing forward kinematic arithmetic based on each joint angle calculated by the joint angle calculation means.

Description

  The present invention relates to a robot control apparatus capable of performing convergence calculation for robot control at high speed and stably, a control method therefor, and a control program.

  For example, an inverse kinematic calculation method using a Newton-Raphson method is known as a method for obtaining a joint angle for determining a hand position or a foot position of a robot. When inverse kinematics calculation using this Newton-Raphson method is performed, because the inverse matrix of the Jacobian matrix is used, the component may become very large and the solution may diverge in a singular posture such as a humanoid robot. The calculation speed is also slowed down. On the other hand, the steepest descent method is known as an inverse kinematic calculation method that suppresses the divergence of such a solution and improves the calculation speed. For example, a control device that sets a target using the steepest descent method and controls an actuator is known (see Patent Document 1).

JP-A-6-106490

  However, since the control device shown in Patent Document 1 sets a target using the steepest descent method, it is stable even in a specific posture of a humanoid robot or the like, but on the other hand, its convergence speed is slow. Is present.

  The present invention has been made to solve such problems, and provides a robot control apparatus, a control method thereof, and a control program capable of performing convergence control of robot control at high speed and stably. Main purpose.

  One aspect of the present invention for achieving the above object is that a command value generating unit that generates a position command value of a predetermined part of a robot, the position command value generated by the command value generating unit, and a control of the predetermined unit A first deviation calculating unit that calculates a deviation from the position; and a force reference value that calculates a force reference value that is a reference value of a force generated in the predetermined portion based on the deviation calculated by the first deviation calculating unit. A joint for calculating each joint torque based on the force reference value and the transposed Jacobian matrix calculated by the calculating means and the force reference value calculating means, and calculating each joint angle based on the calculated joint torque An angle calculation unit; and a forward kinematics calculation unit that calculates a control position of the predetermined unit by performing a forward kinematics calculation based on each joint angle calculated by the joint angle calculation unit. Robo It is a door control device. According to this aspect, it is possible to perform robot control convergence calculation at high speed and stably.

In this aspect, a disturbance estimated value calculating unit that calculates a disturbance estimated value that is an estimated value of a disturbance related to the force generated in the predetermined portion, and the disturbance estimated value from the force reference value calculated by the force reference value calculating unit. Disturbance subtraction means for subtracting the disturbance estimated value calculated by the calculation means;
The disturbance estimated value calculating means calculates the disturbance estimated value based on the joint angular velocity calculated by the joint angle calculating means and the force reference value subtracted by the disturbance subtracting means. Also good. Thereby, the divergence which arises at the time of the convergence calculation of robot control can be suppressed effectively. Here, the disturbance is, for example, an error of a predetermined part with respect to the position command value in the convergence calculation due to discretization or the like, and the factor that causes the error not to converge due to the error is expressed in the force dimension. Means.

  In this one aspect, the disturbance estimated value calculating means calculates the speed of the predetermined part by adding the Jacobian matrix to the joint angular speed calculated by the joint angle calculating means, and differentiates the speed to differentiate the speed of the predetermined part. A force calculation unit that calculates acceleration and adds an inertia matrix to the acceleration to calculate a force of a predetermined part; a force of the predetermined part calculated by the force calculation part; and a disturbance estimated value by the disturbance subtraction unit A second deviation calculation unit that calculates a deviation of the force reference value, a low-pass filter unit that performs a low-pass filter process on the deviation calculated by the second deviation calculation unit, and calculates the estimated disturbance value; , May be included.

  In this aspect, the apparatus may further include a switching unit that switches the disturbance estimated value calculation unit and the disturbance subtraction unit between a connected state and a disconnected state. Thereby, it is possible to prevent the estimated disturbance value from deteriorating the stability in the robot's unique posture.

  In this aspect, the force reference value calculating means may calculate the force reference value by adding a predetermined control gain and inertia matrix to the deviation calculated by the first deviation calculating means.

  In this one aspect, the joint angle calculation means integrates the transposed Jacobian matrix and the reciprocal of the joint inertia to the force reference value calculated by the force reference value calculation means, and integrates the integration result. Each joint angular velocity and each joint angle may be calculated. As a result, it is possible to perform adjustment that achieves both high-speed convergence calculation and stability in robot control.

  In this aspect, the inertia of the joint may be a component corresponding to the joint among diagonal components of a nominal value or an inertia matrix.

  On the other hand, according to one aspect of the present invention for achieving the above object, a step of generating a position command value of a predetermined part of a robot and a deviation between the generated position command value and a control position of the predetermined part are calculated. Each joint torque based on the step, a step of calculating a force reference value that is a reference value of the force generated in the predetermined portion based on the calculated deviation, and the calculated force reference value and the transposed Jacobian matrix And calculating each joint angle based on each calculated joint torque, and calculating a control position of the predetermined unit by performing forward kinematics calculation based on each calculated joint angle And a control method for a robot control apparatus characterized by comprising:

  Further, according to one aspect of the present invention for achieving the above object, a process for generating a position command value of a predetermined part of a robot and a deviation between the generated position command value and a control position of the predetermined part are calculated. Each joint torque based on the process, a process of calculating a force reference value that is a reference value of the force generated in the predetermined part based on the calculated deviation, and the calculated force reference value and the transposed Jacobian matrix Processing for calculating each joint angle based on each calculated joint torque, and processing for calculating a control position of the predetermined unit by performing forward kinematics calculation based on each calculated joint angle And a control program for a robot control device that causes a computer to execute.

  ADVANTAGE OF THE INVENTION According to this invention, the robot control apparatus which can perform the convergence calculation of robot control stably stably at high speed, its control method, and a control program can be provided.

1 is a block diagram showing a schematic system configuration of a robot control apparatus according to a first embodiment of the present invention. It is a control block diagram of the robot control apparatus according to the first embodiment of the present invention. It is a flowchart which shows an example of the control processing flow of the control method by the robot control apparatus which concerns on Embodiment 1 of this invention. It is the simulation result which compared the convergence calculation by the robot control apparatus which concerns on Embodiment 1 of this invention, and the convergence calculation by the conventional steepest descent method. It is a control block diagram of the robot control apparatus according to the second embodiment of the present invention.

Embodiment 1.
Embodiments of the present invention will be described below with reference to the drawings. FIG. 1 is a block diagram showing a schematic system configuration of the robot control apparatus according to the first embodiment of the present invention. FIG. 2 is a control block diagram of the robot control apparatus according to the first embodiment. The robot control apparatus 1 according to the present embodiment controls the posture of a robot such as a humanoid robot having a plurality of joints that respectively connect a plurality of links.

  The robot control apparatus 1 includes a command value generation unit 2, a first deviation calculation unit 3, a force reference value calculation unit 4, a disturbance subtraction unit 5, a joint angle calculation unit 6, a forward kinematics calculation unit 7, A disturbance estimated value calculation unit 8. The robot control apparatus 1 includes, for example, a CPU (Central Processing Unit) that performs control processing, arithmetic processing, and the like, a ROM (Read Only Memory) that stores control programs and arithmetic programs executed by the CPU, and processing. A hardware configuration is mainly made up of a microcomputer having a RAM (Random Access Memory) for temporarily storing data and the like. The CPU, ROM, and RAM are connected to each other by a data bus.

The command value generation unit 2 is a specific example of command value generation means, and generates, for example, a position command value x _plan for a hand that is a predetermined unit such as a humanoid robot. A first deviation calculation unit 3 is connected to the command generation unit 2, and the command value generation unit 2 outputs the generated position command value x _plan to the first deviation calculation unit 3.

The first deviation calculation unit 3 is a specific example of the first deviation calculation unit, and includes a hand position command value x _plan generated by the command value generation unit 2 and a control position x where the hand is actually controlled. The deviation (x _plan −x) is calculated. A force reference value calculation unit 4 is connected to the first deviation calculation unit 3, and the first deviation calculation unit 3 outputs the calculated deviation to the force reference value calculation unit 4.

The force reference value calculation unit 4 is a specific example of a force reference value calculation unit. Based on the deviation calculated by the first deviation calculation unit 3, a force reference value f _ref that is a reference value of the force generated at the hand is obtained. calculate. For example, the force reference value calculating unit 4, the deviation calculated by the first deviation calculating section 3 by accumulating a predetermined control gain K _p to calculate the acceleration of the reference value of the hand, inertial reference value of the calculated acceleration The force reference value f _ref is calculated by integrating the matrix M (matrix determined by the posture of the robot). A disturbance subtraction unit 5 is connected to the force reference value calculation unit 4, and the force reference value calculation unit 4 outputs the calculated force reference value f _ref to the disturbance subtraction unit 5.

The disturbance subtracting unit 5 is a specific example of a disturbance subtracting unit, and a disturbance estimated value f _d calculated by a disturbance estimated value calculating unit 8 described later from the force reference value f _ref calculated by the force reference value calculating unit 4. Is subtracted. A joint angle calculation unit 6 and a disturbance estimated value calculation unit 8 are connected to the disturbance subtraction unit 5, and the disturbance subtraction unit 5 outputs the calculated deviation to the joint angle calculation unit 6 and the disturbance estimation value calculation unit 8. .

The joint angle calculation unit 6 is a specific example of a joint angle calculation unit, and calculates each joint torque τ based on the force reference value f _ref calculated by the force reference value calculation unit 4 and the transposed Jacobian matrix J ^T. Each joint angle θ is calculated based on each calculated joint torque τ.

For example, the joint angle calculation unit 6 calculates each joint torque τ by adding the transposed Jacobian matrix J ^T to the force reference value obtained by subtracting the estimated disturbance value by the disturbance subtraction unit 5, and calculates each joint torque τ for each calculated joint torque τ. Each joint angular acceleration is calculated by accumulating the reciprocal 1 / I of the inertia I of the joint, and each joint velocity is calculated by first-order integration (1 / s) of the calculated joint angular acceleration. Are further integrated by the first order (1 / s) to calculate each joint angle θ. Here, the inertia I of each joint is, for example, a nominal value (a constant value), but may be a component corresponding to each joint among the diagonal components of the inertia matrix M.

  In the conventional steepest descent method, the convergence calculation is speeded up by adjusting the step width. On the other hand, according to the robot control apparatus 1 according to the first embodiment, by performing the above calculation, it can be considered to be replaced with the concept of physical quantity. Therefore, it is easy to use existing control theory frameworks and tools. It is possible to make adjustments that achieve both high-speed convergence calculation and stability in robot control.

  The joint angle calculation unit 6 is connected to a disturbance estimated value calculation unit 8 and a forward kinematics calculation unit 7, and the joint angle calculation unit 6 outputs the calculated joint angular velocities to the disturbance estimated value calculation unit 8. Then, each calculated joint angle θ is output to the forward kinematics calculation unit 7. Further, the joint angle calculation unit 6 outputs the calculated joint angles θ to, for example, a servo motor provided at each joint. Each servo motor rotationally drives each joint so that each joint angle θ from the joint angle calculation unit 6 becomes the same.

  The forward kinematics calculation unit 7 is a specific example of the forward kinematics calculation means, and performs forward kinematics calculation (DK: Direct Kinematics) based on each joint angle θ calculated by the joint angle calculation unit 6. The control position x is calculated. The forward kinematics calculator 7 feeds back the calculated joint angles θ to the first deviation calculator 3.

The disturbance estimated value calculation unit 8 is a specific example of the disturbance estimated value calculation unit, and is based on the joint angular velocity calculated by the joint angle calculation unit 6 and is a disturbance estimated value f that is an estimated value of disturbance related to the force generated at the hand. _d is calculated.

The disturbance estimated value calculation unit 8 calculates the hand speed by adding the Jacobian matrix J to the joint angular velocity calculated by the joint angle calculation unit 6, and calculates the hand acceleration by differentiating the calculated hand speed. The force calculation unit 81 that calculates the hand force by adding the inertia matrix M to the calculated hand acceleration, and the hand force calculated by the force calculation unit 81 and the disturbance estimated value are subtracted by the disturbance subtraction unit 82. A second deviation calculating unit 82 that calculates a deviation of the force reference value, and a low-pass filter unit that performs low-pass filter processing LPF on the deviation calculated by the second deviation calculating unit 82 and calculates a disturbance estimated value f _d 83.

The low-pass filter unit 83 feeds back the calculated disturbance estimated value _fd to the disturbance subtracting unit 5. By thus feeding back the calculated disturbance estimated value f _d by the disturbance estimated value calculating section 8, it is possible to reduce the error due to quantization error and the integral-differential, it is possible to suppress the divergence caused during convergence calculation it can.

  The disturbance estimated value calculation unit 8 may be configured without the low-pass filter unit 83. However, by having the low-pass filter unit 83, the divergence of the solution can be more reliably suppressed, and the stability is increased.

  FIG. 3 is a flowchart showing an example of a control processing flow of the control method by the robot control apparatus according to the first embodiment.

First, the command value generation unit 2 generates a position command value x _plan for the hand (step S101), and outputs the generated position command value x _plan to the first deviation calculation unit 3.

Next, the first deviation calculation unit 3 calculates a deviation (x _plan − between the hand position command value x _plan generated by the command value generation unit 2 and the hand control position x from the forward kinematics calculation unit 7. x) is calculated (step S102), and the calculated deviation is output to the force reference value calculation unit 4.

The force reference value calculation unit 4 calculates a hand force reference value f _ref based on the deviation calculated by the first deviation calculation unit 3 (step S103), and the calculated force reference value f _ref is a disturbance subtraction unit 5. Output for.

The disturbance subtracting unit 5 subtracts the disturbance estimated value f _d calculated by the disturbance estimated value calculating unit 8 from the force reference value f _ref calculated by the force reference value calculating unit 4 (step S104), and calculates the disturbance estimated value. The subtracted force reference value is output to the joint angle calculation unit 6 and the disturbance estimated value calculation unit 8.

The joint angle calculation unit 6 adds the transposed Jacobian matrix J ^T and the reciprocal 1 / I of the inertia I of each joint to the force reference value obtained by subtracting the disturbance estimated value by the disturbance subtraction unit 5 and performs integration 1 / s. Each joint angular velocity and each joint angle are calculated (step S105), each calculated joint angular velocity is output to the disturbance estimated value calculation unit 8, and each calculated joint angle θ is output to the forward kinematics calculation unit 7. Output.

  The forward kinematics calculation unit 7 performs forward kinematics calculation based on each joint angle θ calculated by the joint angle calculation unit 6, calculates the control position x of the hand (step S106), and calculates the calculated hand control. The position x is fed back to the first deviation calculation unit 3.

The disturbance estimated value calculation unit 8 adds the Jacobian matrix J to the joint angular velocity calculated by the joint angle calculation unit 6, performs differentiation s, calculates the hand force by adding the inertia matrix M, and calculates the calculated hand tip force. A deviation between the force and the force reference value obtained by subtracting the disturbance estimated value by the disturbance subtracting unit 5 is calculated, low-pass filter processing LPF is performed, a disturbance estimated value _fd is calculated (step S107), and the calculated disturbance estimated value f is calculated. _d is fed back to the disturbance subtraction unit 5.

  FIG. 4 is a simulation result comparing the convergence calculation by the robot control apparatus according to the first embodiment and the convergence calculation by the conventional steepest descent method. In this simulation, the robot to be controlled is a manipulator having two joints, and the states of convergence from a position 0.1 m away from the x coordinate and the y coordinate are compared. As shown in FIG. 4, it can be seen that the robot controller 1 according to the first embodiment (1) has a convergence calculation that is twice as fast as the conventional steepest descent method (2).

As described above, in the robot control device 1 according to the first embodiment, the force reference value is calculated based on the deviation between the hand position command value and the fed back control position, and the calculated force reference value and the transposed Jacobian matrix Each joint torque is calculated based on the above, and each joint angle is calculated based on the calculated joint torque. Thereby, the convergence calculation of the robot control can be performed at high speed and stably. Further, by feeding back the estimated disturbance value f _d calculated by the disturbance estimate calculation unit 8, it is possible to suppress the divergence caused during convergence calculation effectively. In other words, the robot control convergence calculation can be performed at high speed and stably while numerically stabilizing the robot's unique posture.

Embodiment 2.
FIG. 5 is a control block diagram of the robot control apparatus according to the second embodiment of the present invention. In addition to the configuration of the robot control device according to the first embodiment, the robot control device 20 according to the second embodiment performs a step between the low-pass filter unit 83 and the disturbance subtraction unit 5 of the disturbance estimated value calculation unit 8. It further includes a switch (switching means) 9 for switching between a connected state and a disconnected state automatically or continuously.

For example, in a singular posture such as a humanoid robot, the estimated force f _d gradually increases as the hand force corresponding to the force reference value f _ref ((b) in FIG. 5) cannot be generated. It is assumed that the vibration is lost. By providing this switch 9, such a state can be reliably prevented.

When the force reference value f _ref calculated by the force reference value calculation unit 4 includes a plurality of components such as the x-axis, the y-axis, and the z-axis, the switch 9 is in a connected state and a disconnected state for each of these components. Switch.

Further, the switch 9 is, for example, when the absolute value of the estimated disturbance value f _d output from the estimated disturbance value calculation unit 8 exceeds a predetermined threshold value, or the deviation (a) output from the disturbance subtraction unit 5 When the ratio ((a) / (b)) with the hand force (b) output from the force calculation unit 81 of the disturbance estimated value calculation unit 8 exceeds a predetermined threshold, the state is switched to the disconnected state. Note that the switching method of the switch 9 is an example, and the present invention is not limited to this, and any method can be applied.

  In the robot control device 20 according to the second embodiment, since the other configuration is substantially the same as the robot control device 1 according to the first embodiment, the same parts are denoted by the same reference numerals and detailed description thereof will be omitted. Omitted.

  The present invention is not limited to the above-described embodiment, and can be changed as appropriate without departing from the spirit of the present invention.

In the above embodiment, the hand is applied as the predetermined portion of the robot. However, the present invention is not limited to this, and may be applied to an arbitrary part such as a foot tip, a center of gravity position or a posture, for example. For example, when controlling the position of the center of gravity of the robot, the same control as in the first embodiment can be performed based on the following equation (1) by using the center of gravity Jacobian.

In the above-described embodiment, a configuration without the disturbance subtraction unit 5 and the disturbance estimated value calculation unit 8 may be used. In this case, the force reference value calculation unit 4 outputs the calculated force reference value f _ref to the joint angle calculation unit 6, and the joint angle calculation unit 6 calculates the force reference value calculated by the force reference value calculation unit 4. Each joint torque τ is calculated by adding the transposed Jacobian matrix J ^T to f _ref , and each joint angular acceleration is calculated by adding the inverse 1 / I of the inertia I of each joint to each calculated joint torque τ. Each joint angular acceleration is first-order integrated to calculate each joint velocity, and each calculated joint velocity is first-order integrated to calculate each joint angle θ.

  In the above embodiment, for example, control is performed so as to satisfy a plurality of predetermined portions such as a right hand position command value, a left hand position command value, a right foot position command value, and a left foot tip position command value simultaneously. Also good. In this case, a transposed matrix having a Jacobian matrix for each predetermined part as a partial matrix is used.

  In the above-described embodiments, the present invention has been described as a hardware configuration, but the present invention is not limited to this. In the present invention, for example, the processing shown in FIG. 3 can be realized by causing a CPU to execute a computer program.

  The program may be stored using various types of non-transitory computer readable media and supplied to a computer. Non-transitory computer readable media include various types of tangible storage media. Examples of non-transitory computer readable media are magnetic recording media (eg flexible disks, magnetic tapes, hard disk drives), magneto-optical recording media (eg magneto-optical disks), CD-ROM, CD-R, CD-R / W. Semiconductor memory (for example, mask ROM, PROM (Programmable ROM), EPROM (Erasable PROM), flash ROM, RAM).

  The program may also be supplied to the computer by various types of transitory computer readable media. Examples of transitory computer readable media include electrical signals, optical signals, and electromagnetic waves. The temporary computer-readable medium can supply the program to the computer via a wired communication path such as an electric wire and an optical fiber, or a wireless communication path.

DESCRIPTION OF SYMBOLS 1 Robot controller 2 Command value production | generation part 3 1st deviation calculation part 4 Force reference value calculation part 5 Disturbance subtraction part 6 Joint angle calculation part 7 Forward kinematics calculation part 8 Disturbance estimated value calculation part 9 Switch

Claims (9)

Command value generating means for generating a position command value of a predetermined part of the robot;
First deviation calculating means for calculating a deviation between the position command value generated by the command value generating means and the control position of the predetermined portion;
Force reference value calculating means for calculating a force reference value, which is a reference value of force generated in the predetermined portion, based on the deviation calculated by the first deviation calculating means;
Joint angle calculating means for calculating each joint torque based on the force reference value calculated by the force reference value calculating means and the transposed Jacobian matrix, and calculating each joint angle based on the calculated joint torque; ,
Based on each joint angle calculated by the joint angle calculation means, forward kinematics calculation means for performing a forward kinematic calculation and calculating a control position of the predetermined unit;
A robot control device comprising: The robot control device according to claim 1,
Disturbance estimated value calculation means for calculating a disturbance estimated value that is an estimated value of disturbance related to the force generated in the predetermined portion;
Disturbance subtraction means for subtracting the disturbance estimated value calculated by the disturbance estimated value calculating means from the force reference value calculated by the force reference value calculating means;
Further comprising
The disturbance estimated value calculating means calculates the disturbance estimated value based on the joint angular velocity calculated by the joint angle calculating means and the force reference value subtracted by the disturbance subtracting means. Robot control device. The robot control device according to claim 2,
The disturbance estimated value calculating means includes:
The speed of the predetermined part is calculated by adding the Jacobian matrix to the joint angular speed calculated by the joint angle calculating means, the acceleration of the predetermined part is calculated by differentiating the speed, and the inertia matrix is added to the acceleration. A force calculator for calculating the force of the predetermined part,
A second deviation calculating unit that calculates a deviation between the force of the predetermined unit calculated by the force calculating unit and the force reference value obtained by subtracting the estimated disturbance value by the disturbance subtracting unit;
A low-pass filter that performs low-pass filter processing on the deviation calculated by the second deviation calculator, and calculates the disturbance estimated value;
A robot controller characterized by comprising: The robot control device according to claim 2 or 3,
A robot control apparatus, further comprising: a switching unit that switches the disturbance estimated value calculation unit and the disturbance subtraction unit between a connected state and a disconnected state. The robot control device according to any one of claims 1 to 4,
The robot control apparatus, wherein the force reference value calculating unit calculates the force reference value by adding a predetermined control gain and an inertia matrix to the deviation calculated by the first deviation calculating unit. The robot control device according to any one of claims 1 to 5,
The joint angle calculation means integrates the transposed Jacobian matrix and the reciprocal of the inertia of the joint to the force reference value calculated by the force reference value calculation means, and integrates the integration result to obtain each joint angular velocity. And a robot controller characterized by calculating each joint angle. The robot control device according to claim 6,
The joint inertia is a component corresponding to the joint among diagonal components of a nominal value or an inertia matrix. Generating a position command value for a predetermined part of the robot;
Calculating a deviation between the generated position command value and the control position of the predetermined part;
Calculating a force reference value, which is a reference value of the force generated in the predetermined portion, based on the calculated deviation;
Calculating each joint torque based on the calculated force reference value and the transposed Jacobian matrix, and calculating each joint angle based on the calculated joint torque;
Calculating a control position of the predetermined unit by performing a forward kinematics calculation based on the calculated joint angles;
A control method for a robot control apparatus, comprising: Processing for generating a position command value of a predetermined part of the robot;
A process of calculating a deviation between the generated position command value and the control position of the predetermined unit;
A process of calculating a force reference value, which is a reference value of a force generated in the predetermined portion, based on the calculated deviation;
Calculating each joint torque based on the calculated force reference value and the transposed Jacobian matrix, and calculating each joint angle based on each calculated joint torque;
Based on the calculated joint angles, a process for calculating a control position of the predetermined unit by performing forward kinematics calculation;
A control program for a robot control apparatus, characterized in that a computer is executed.

Images