JP2014014901A - Robot controller - Google Patents

Abstract

PROBLEM TO BE SOLVED: To provide a robot controller which enables a user to intuitively and stably operate a robot arm type treatment table.SOLUTION: In a robot controller of an embodiment, a speed joint angle speed command value whose joint angle difference does not exceed a threshold is output from a monitoring unit when an operation direction of a robot is restricted, an excessive ratio of the joint angle difference is calculated by a speed suppression unit when the joint angle difference is restricted by the threshold, a target value is shaped by a target value shaping unit in consideration of the excessive ratio, and the restricted operation direction is displayed to a user by a speed restriction presentation unit.

Description

  Embodiments described herein relate generally to a robot control apparatus.

  In radiation therapy such as heavy particle beams, treatment is performed by irradiating a patient with radiation from various directions. When the irradiation direction of radiation is fixed, it is necessary to accurately position the top board on which the patient is mounted in accordance with the irradiation site. For this reason, in radiation therapy, a treatment table in which a top plate is mounted on the tip of a robot arm composed of multiple links is effective. In order for an operator (user) such as an engineer or a doctor to determine the irradiation position, it is necessary to input the target position of the top board while the patient is mounted. Conventionally, a target value is given to each axis of the orthogonal coordinate system by a teaching pendant widely used in industrial robots.

  However, it is desirable to be able to input more intuitively from the viewpoint of operability. When a pointing device such as a joystick is used, not only can the operation of the top plate be input intuitively, but the operation amount can be continuously changed according to the operation amount. It is easy to reflect the intention. However, the robot may not be able to operate as intended by the user. For example, a singular point where there are infinite solutions satisfying the movable range and target position and orientation of the robot joint, or a sudden change in the joint angular velocity when a target value near the singular point is given. Since the movement of the robot joint and the movement of the top plate are not directly linked, it is difficult for a user who is not familiar with the movement of the robot.

  In the case of a redundant degree of freedom arm in which the link configuration has more degrees of freedom than the target position, there are an infinite number of inverse kinematic solutions for calculating joint target values. However, there are solutions that satisfy the mechanical constraints such as the movable limit and singular point. Furthermore, it is desirable to satisfy the demands such as limiting the range of motion of the joint for securing the user's work space.

  In order to satisfy these constraints, it is known to provide a limit to the range of motion of the joint and to provide a target value that moves away from the limit of motion. However, if sufficient redundancy is utilized, sudden changes in speed may be prevented by operating in the direction approaching the limit of movement. In addition, if the speed is excessive and the user cannot easily present it, the user may continue to make an erroneous input without noticing abnormally.

  In addition, an example in which a user's operation force is converted into a continuous movement amount using a force sensor has been proposed. However, at present, the force sensor is expensive, vulnerable to shocks, and easily breaks down. Therefore, like the teaching pendant widely used in industrial robots, the motion direction assigned to each axis of the Cartesian coordinate system is turned ON and OFF. In many cases, buttons that indicate discretely are provided.

  In an example using a force sensor, an operation program using force control is corrected by user guidance. In this example, the teaching pendant is provided with a means for setting force control. When the user gives a position or force command during playback of teaching data, the command is superimposed on the operation of the operation program to correct the operation. Yes. As a result, the teaching data is corrected by the user's guidance, so that the operation intended by the user can be realized.

  However, the correction amount of the position or force command given by the user is collectively changed by using buttons such as “high” and “low” attached to the teaching pendant. For this reason, user operability is set uniformly. For example, when you want to increase the correction amount of a certain axis, not only do you need a button for adjusting the correction amount for each axis, but also the user needs to set the correction amount for all axes in advance according to the situation. . For this reason, the user operability is deteriorated.

JP-A-2005-199412 JP2011-224696A

  The present embodiment provides a robot control device that allows a user to intuitively and stably operate a robot arm type treatment table.

  The robot control apparatus according to the present embodiment includes an actuator that drives a joint axis of a robot having an arm-type treatment table for each control period, a joint angle detection unit that detects an angle of the joint axis, the joint angle, and a link parameter. A control point position calculation unit for calculating the position coordinates of the control point of the robot based on the relationship between the joint coordinate system and the work coordinate system at each joint based on the detected joint angle and the link parameter A Jacobian matrix calculating unit that calculates a Jacobian matrix, a target value input unit that inputs a target position of the control point and a target value of the control point, a target value shaping unit that shapes the target value of the control point, and the target value The difference between the target value of the control point shaped by the shaping unit and the current value of the control point calculated by the control point position calculation unit is calculated, and inverse kinematic calculation is performed using the Jacobian matrix. A target speed calculation unit that calculates a target speed at the control point; and the target speed is monitored, and when the calculated target speed exceeds a threshold, the target speed is limited to be equal to or less than the threshold. A speed monitoring unit; a speed suppression unit that corrects a target value of the control point when the calculated target speed exceeds the threshold; and an operation direction of the robot is limited by the speed suppression unit A command value is calculated based on the speed limit presenting unit that presents the restricted motion direction to the user, the target speed and the joint angle output from the speed monitoring unit, and the command value is calculated based on the command value. A first drive unit that drives the actuator, and the target value shaping unit shapes the corrected target value when the target value of the control point is corrected by the speed suppressing unit. And butterflies.

FIG. 3 is an external view illustrating an example of a robot that is a control target. The block diagram which shows the robot control apparatus by 1st Embodiment. The figure which shows an example of a speed limitation presentation part. The block diagram which shows the robot control apparatus by 2nd Embodiment. The flowchart which shows operation | movement of the input signal evaluation part which concerns on 2nd Embodiment. The figure which shows an example of the signal in the input signal evaluation part which concerns on 2nd Embodiment. The block diagram which shows the robot control apparatus by the 1st modification of 2nd Embodiment.

  Embodiments will be described below with reference to the drawings. In the following description of the drawings, the same or similar parts are denoted by the same or similar reference numerals. However, the drawings are schematic, and specific dimensions should be determined in consideration of the following description. Further, it goes without saying that the drawings include portions having different dimensional relationships and ratios.

(First embodiment)
The robot control apparatus according to the first embodiment will be described with reference to FIGS. FIG. 1 is an external view of a robot controlled by the robot control apparatus of this embodiment, and FIG. 2 is a block diagram showing the robot control apparatus of this embodiment.

  The robot control apparatus according to the present embodiment controls, for example, a robot 10 that has a patient mounted on the tip of an arm serially linked to a link (joint axis) as shown in FIG. The robot 10 includes first to seventh joint shafts 11, 12, 13, 14, 15, 16, 17 and a top board 20 on which a patient is mounted.

  The first joint shaft 11 rotates in the horizontal direction like a turntable with respect to the chamber in which the robot 10 is provided. The second joint shaft 12 is connected to the first joint shaft and rotates in the horizontal direction with respect to the first joint shaft 11. In addition, the rotation operation | movement of the 1st and 2nd joint shafts 11 and 12 becomes a structure in which clockwise rotation and counterclockwise rotation are possible when it sees from the top. The third joint shaft 13 is connected to the second joint shaft 12 and operates in the vertical direction, that is, in the vertical direction. The fourth joint shaft 14 is connected to the third joint shaft 13 and rotates in the horizontal direction. This rotation operation is configured to allow clockwise rotation and counterclockwise rotation when viewed from above.

  The fifth joint shaft 15 is connected to the fourth joint shaft 14 and extends in the horizontal direction, and rotates around the extending shaft. This rotation operation is configured to allow clockwise rotation and counterclockwise rotation when viewed from the extending shaft. The sixth joint shaft 16 is connected to the fifth joint shaft 15 and operates in the vertical direction, that is, in the vertical direction. The seventh joint shaft 17 is connected to the sixth joint shaft 16 and rotates in the horizontal direction. This rotation operation is configured to allow clockwise rotation and counterclockwise rotation when viewed from above. The top plate 20 is connected to the seventh joint shaft 17. In the robot 10 configured as described above, the top plate 20 can be inclined with respect to the horizontal direction by rotating the fifth joint shaft 15.

  As shown in FIG. 2, the robot control apparatus of the present embodiment includes a central processing unit (CPU) 1, an actuator 100, a drive unit (amplifier) 101, a joint angle detection unit 102, and a target value input unit 103. A speed limit presentation unit 104, a link parameter storage unit 201, and a maximum speed storage unit 202.

  The joint angle detection unit 102 detects the angle of the joint axis of the robot 10, and a position sensor such as an encoder can be used, and may include a filter that removes a predetermined frequency component. When the joint shaft is configured from the drive shaft via the transmission mechanism, and the drive shaft is provided with the detection unit, the joint angle is determined by the displacement of the position of the drive shaft (drive shaft angle). It is calculated according to the ratio of the axis and the joint axis.

  The target value input unit 103 sets the final target position / posture (target value) of the robot 10. The target value may be stored in advance in a database, or may be given by a user using a pointing device such as a joystick.

  The speed restriction presenting unit 104 presents the restricted movement direction in an easy-to-understand manner to the user when the movement direction of the robot 10 is restricted by a speed suppression unit 110 described later. For example, when the target value input unit 103 is the pointing device 200, an area 202 shown in FIG. 3 is a direction in which the operation direction of the robot 10 is restricted.

  The CPU 1 shown in FIG. 2 includes a control point position calculation unit 105, a Jacobian matrix calculation unit 106, a target value shaping unit 107, a target speed calculation unit 108, a speed monitoring unit 109, a speed suppression unit 110, a redundancy And a sex evaluation unit 111 as modules. That is, each element constituting the CPU 1 may be realized by hardware or software.

  The control point position calculation unit 105 reads the link parameter of the robot 10 from the link parameter storage unit 201, and from the joint angle detected by the joint angle detection unit 102, the work coordinates of the robot 10 are calculated by forward kinematics calculation using the link parameter. Calculate the position and orientation of the control points in the system.

The Jacobian matrix calculation unit 106 calculates a Jacobian matrix based on the joint angle calculated by the joint angle detection unit 102 and the link parameter stored in the link parameter storage unit 201. The Jacobian matrix is a matrix expressing a minute displacement relationship between the work coordinate system and the joint coordinate system of the robot 10. Assuming that the Jacobian matrix is J, the position change Δx and the joint angle difference Δθ satisfy the relationship of the following equation (1).

Δx = JΔθ (1)

  The target value shaping unit 107 shapes the target value of the robot 10 input by the target value input unit 103 more smoothly using a filter or the like. Further, when the joint angle difference Δθ of the robot 10 is limited by the speed suppressing unit 110 described later, the target value shaping unit 107 shapes the target value in consideration of the excessive ratio.

The target speed calculation unit 108 calculates the position difference Δx using the target value shaped by the target value shaping unit 107 and the current value of the control point position calculated by the control point position calculation unit 105, and the Jacobian matrix The joint angle difference Δθ is calculated using the following equation (2) using the inverse matrix J ⁻¹ of J.

Δθ = J ⁻¹ Δx (2)

  The calculation of the joint angle difference Δθ is performed every predetermined cycle, for example, every control cycle of the actuator 100 described later. Therefore, the joint angle difference Δθ represents the target angular velocity during the period.

The speed monitoring unit 109 reads the maximum value of the angular velocity of the joint from the maximum velocity storage unit 202, and the joint angle difference Δθ calculated by the target speed calculation unit 108 is set to the set maximum angular velocity (maximum angle in the above period) θ _max. A limit is set so that the joint angular velocity command value is not exceeded.

When the joint angle difference Δθ is limited by the maximum angular velocity θ _max in the speed monitoring unit 109, the speed suppressing unit 110 calculates an excessive ratio α of the calculated joint angle difference Δθ and transmits the calculated excess rate α to the target value shaping unit 107. . For example, the excess ratio α is calculated as follows.

α = | Δθ / θ _max | (3)

  The drive unit 101 drives the actuator 100 for each control cycle according to the joint angular velocity command value calculated by the speed monitoring unit 109.

In the robot control apparatus of the present embodiment, it is conceivable that the robot 10 to be controlled has redundant degrees of freedom as shown in FIG. Here, having redundancy degree of freedom means that there are more actuators of the robot 10 than the dimension of the input target value. In this case, there is no inverse matrix in the Jacobian matrix J. For this reason, the joint angle difference Δθ cannot be calculated using the equation (2). Therefore, using the pseudo inverse matrix J ⁺ of the Jacobian matrix J, the target speed calculation unit 108 calculates the joint angle difference Δθ as shown in the following equation (4).

Δθ = J ⁺ Δx + (I−JJ ⁺ ) k (4)

Here, I is a unit matrix, and k is a redundancy parameter. The redundancy parameter k is calculated by the redundancy evaluation unit 111. The redundancy evaluation unit 111 calculates the target movable speed and the singular point proximity of the Jacobian matrix as the evaluation function so that the target speed is calculated when the robot 10 approaches the movement limit or the singular point. Calculate the parameters. Various indices such as the degree of manipulability and the number of conditions shown below have been proposed as the degree of singularity proximity κ. The manipulability is obtained by taking the square root of the determinant of the product of J and its transposed matrix as shown in the following equation.

κ = √ (det (JJ ^T ))

Here, det (K) is a determinant of the matrix K. When approaching a singular point, it decreases rapidly and becomes zero at a singular point.

The condition number is defined by the ratio of the eigenvalues of J as shown in the following equation.

κ = σ ₁ / σ _m

Here, σ ₁ is the smallest of the eigenvalues of J, and σ _m is the largest of the eigenvalues of J. It can be said that the closer the condition number is to 1, the farther from the singular value.

The redundancy parameter k is set using the singular value proximity κ as described above.

k = k ₀ * f (κ)

Here, k ₀ is a reference value of the redundancy parameter, and may be selected depending on the operation. For example, in order to secure the work area for the user and move the arm by moving the first axis greatly, the following settings may be made.

k ₀ = [k ₁ , 0..., 0]

  F is an evaluation function based on the degree of singularity proximity. If it is far from the singular point, it may be 1, and when it approaches the singular point, a value that suppresses the joint velocity is output to prevent the joint angular velocity from rapidly increasing.

Further, Δx = 0 at the start or stop of the operation, and Δθ = 0 must be satisfied. At this time, if (I−JJ ⁺ ) ≠ 0, if k = 0 is not set, Δθ ≠ 0 and sudden acceleration occurs, which is dangerous. Therefore, regardless of the term (I−JJ ⁺ ), a smooth target trajectory is generated by multiplying k by a time-dependent acceleration / deceleration coefficient β as defined by the following equation.

β = t / T ₀ (0 <t <T ₀ )
β = 1 (T ₀ <t <T−T ₀ )
β = (T−t) / T ₀ (T−T ₀ <t <T)

Here, T ₀ is the acceleration / deceleration time, and T is the time required for completing the operation.

As described above, in this embodiment, when the movement direction of the robot 10 is limited, a velocity joint angular velocity command value at which the joint angle difference Δθ does not exceed the maximum angular velocity (maximum angle in the above cycle) θ _max is obtained. When the joint angle difference Δθ is output from the monitoring unit 109 and is limited by the maximum angular velocity θ _max , the excessive ratio is calculated by the speed suppressing unit 110, and the target value is set to the target value shaping unit 107 in consideration of the excessive ratio. Since the speed limit presenting unit 104 displays the movement direction restricted for the user in an easy-to-understand manner, the user can intuitively and stably operate the robot arm type treatment table.

  In the above-described embodiment, the CPU 1, the link parameter storage unit 201, the maximum speed storage unit 202, and the like may be embedded and integrated in a robot that is a control target. Further, the CPU 1, the link parameter storage unit 201, the maximum speed storage unit 202, and the like are outside the robot to be controlled, and the robot can be remotely controlled by wire or wireless.

(Second Embodiment)
FIG. 4 shows a robot control apparatus according to the second embodiment. As in the first embodiment, the robot control apparatus according to the second embodiment uses, for example, the robot 10 shown in FIG. 1 as a control target. The robot control apparatus of this embodiment has a configuration in which, in the control apparatus of the first embodiment, the drive unit 101 is replaced with the drive unit 101A, the CPU 1 is replaced with the CPU 1A, and the operation direction specifying unit 302 is newly provided. The CPU 1A according to the second embodiment includes a tip speed target value calculation unit 303, a joint speed target value calculation unit 305, an input signal evaluation unit 307, an inertia moment calculation unit 308, and a feedforward in the CPU 1 shown in FIG. A command value calculation unit 309 is newly provided. Note that this CPU 1A does not show each component of the CPU 1 other than the Jacobian matrix calculation unit 106 shown in FIG. Each component constituting the CPU 1A is a module and may be realized by hardware or software.

  The motion direction designation unit 302 designates the motion direction of the control point input by the user, such as a momentary motion button indicating the positive or negative direction, or a push / pull motion button indicating the positive direction, stop, or negative direction. May be provided. Further, a continuous input may be given by a pointing device such as a joystick or a force sensor.

The tip speed target value calculation unit 303 is a tip speed target value (speed at the control point) at each control cycle within a range not exceeding a predetermined maximum acceleration from the motion direction at the control point obtained by the motion direction designating unit 302. Target value). When the previous tip speed target value is v _Ri−1 , the current tip speed target value is v _Ri , the maximum acceleration is a, and the control cycle is ΔT, for example, the calculation is performed as shown in Equation (5).

v _Ri = v _Ri−1 + aΔT (5)

Actually, the target position to be reached may not be reached due to the previous tip speed target value. Therefore, it is preferable to calculate an error Δx between the target position assumed and the current position and correct it as shown in Expression (6).

v _Ri = v _Ri−1 + aΔT + Δx (6)

  A smoother operation may be achieved by performing filter processing on the obtained tip speed target value.

When the Jacobian matrix calculated by the Jacobian matrix calculation unit 106 is J, the speed v of the control point and the joint speed ω satisfy the relationship shown in Expression (7).

v = Jω (7)

The joint speed target value calculation unit 305 uses the tip speed target value v _Ri calculated by the tip speed target value calculation unit 303 and the inverse matrix J ⁻¹ of the Jacobian matrix J to obtain the joint speed target value ω _Ri as It calculates using Formula (8).

ω _Ri = J ⁻¹ v _Ri (8)

In the case where the robot 10 has redundant degrees of freedom as shown in FIG. 1, the joint velocity target value angle difference ω using the pseudo inverse matrix J ⁺ of the Jacobian matrix J as described in the first embodiment. _The joint speed target value calculation unit 305 calculates _Ri .

The drive unit 101 drives the actuator 100 for each control period in accordance with the joint speed target value ω _Ri calculated by the joint speed target value calculation unit 305. The drive unit 101 is often a speed command type driver. For example, when a speed FF-IP control system, which is a two-degree-of-freedom control system, is configured, a drive torque command value τ _Ri as shown in Expression (9) is calculated.

τ _Ri = K _P (K _I ∫ (ω _Ri −ω) dt−ω + K _FF ω _Ri ) (9)

_{Here, K I, K P, K} FF represents the speed deviation integrated feedback control gain, respectively, the speed proportional feedback control gain, a speed target value feedforward control gain.

  The input signal evaluation unit 307 records the motion direction input by the motion direction designating unit 302 and the time when the motion direction is changed, and evaluates the discrete input signal as being approximated by a continuous function. Then, the trajectory parameters such as the maximum speed and maximum acceleration of the control point are changed, and the changed trajectory parameters are given to the tip speed target value calculation unit 303, and the gain is changed and the changed gain is fed. This is given to the forward command value calculation unit 309. That is, the input signal evaluation unit 307 changes the trajectory parameters such as the maximum speed and maximum acceleration of the control point based on the history of operation direction and the operation time.

  Next, the operation of the input signal evaluation unit 307 according to the second embodiment will be described with reference to FIGS. FIG. 5 is a flowchart showing the operation of the input signal evaluation unit 307, and FIG. 6 is a diagram showing the waveform of the input signal in the input signal evaluation unit 307.

  (A) When an input signal is input to the input signal evaluation unit 307, processing is started (S300 in FIG. 5).

  (B) Next, the input signal is encoded (S301 in FIG. 5). For example, in the vertical direction, for example, the movement direction designation unit 302 assigns codes such as “1” for ascending (UP), “−1” for descending (DOWN), and “0” for stopping (STOP). Suppose. While the switch is pressed, the same numerical value is continuously input (momentary operation). An example of the input signal is shown as a waveform 401 in FIG. In this waveform 401, first, the operation UP and the operation STOP are alternately inputted, and after the operation DOWN is inputted after 3 seconds, the operation UP and the operation STOP are quickly switched.

(C) If the input signal has not changed (S302 in FIG. 5), the input signal evaluation unit 307 approximates this input history with a certain continuous function. For example, a function shown in Expression (10) is prepared as an approximate expression (S303 in FIG. 5).

y _i = K (u _i −y _i−1 ) + y _i−1 (10)

Here, u _i is an input, y _i is an output, y _i-1 is a previous output, and K is a coefficient. This expression expresses the expression of the first-order lag element (gain 1) in a differential format. If the input does not change, the output will converge to the input over time.

  (D) Here, if the difference between the input and the output is regarded as an error and is within a predetermined range (S304 in FIG. 5), the convergence time dT is calculated (S306 in FIG. 5). Here, the convergence time is the time elapsed from the time when the input signal changes until the time when the error falls within a predetermined range. If the error is large, the process is terminated (S305 in FIG. 5).

(E) If the input signal changes in step S302, the time T elapsed until the input changes is compared with the convergence time dT calculated in step S306, and the coefficient K is changed each time the input signal changes. For example, the rule shown in Formula (11) can be considered.

K _j = K _j−1 / α (when dT <βT)
K _j = αK _j−1 (when dT> T) (11)
K _j = K _j-1 (other cases)

Here, α> 1 and 0 <β <1.

  An example of the output is shown as a waveform 402 in FIG. Since the convergence time is short at the initial value, the coefficient decreases and the convergence time gradually increases. However, it can be seen that when the input changes quickly, the coefficient increases and the convergence time decreases. In addition, an ARX (Auto Regressive with eXogenous inputs or Auto Regressive with eXternal inputs) model etc. can be considered as an approximate expression.

The inertia moment calculation unit 308 calculates the inertia moment around each joint axis from the link parameters such as the mass of each link, the center of gravity position, and the inertia moment around the center of gravity. In the case of no load, what is calculated in advance from design data as an initial value may be recorded in the storage unit. However, if the applied load changes dynamically as in the case where the patient is transferred to the top table 20 of the treatment table arm, the moment of inertia also changes. In addition, the position of the center of gravity may be different in each case. The relationship shown in Expression (12) is established between the external force F and the torque τ _d acting on each joint from the principle of virtual work.

F = (J ^T ) ⁻¹ τ _d (12)

Here, J ^T is a transposed matrix of the Jacobian matrix J. When the robot 10 has redundant degrees of freedom as shown in FIG. 1, the external force F is calculated using the pseudo inverse matrix J ^T of the transposed matrix J ^T of the Jacobian matrix J, as described in the first embodiment. To do. τ _d is calculated by subtracting the joint drive torque (≈gravity torque) at no load and at rest from the joint drive torque. For this reason, an external force is calculated by Formula (12). Further, if the arm posture is made non-interfering, the amount of calculation is reduced, and the load and the position of the center of gravity are calculated. Alternatively, there is a method of continuously estimating parameters while changing the posture of the arm.

Feedforward command value evaluation unit 309, the inertia moment calculated by the moment of inertia calculating unit 308 changes the feedforward control gain K _FF used in the driving section 101A. In the case of the control system shown in Expression (9), the gain can take a numerical value of 0 to 1.

  As described above, since the second embodiment includes the respective elements constituting the robot control device similar to the first embodiment, the user can set the robot arm type treatment table as in the first embodiment. It can be operated intuitively and stably.

  Further, in the second embodiment, the trajectory parameters such as the maximum speed and the maximum acceleration of the control point are changed by the input signal evaluation unit 307 based on the history of operation direction and the operation time, and the joint angle, link parameter, and drive torque are changed. From the command value, the load mounted on the robot (such as the patient's weight) is estimated by the moment of inertia calculation unit, and the moment of inertia of the link is calculated. Based on this moment of inertia and the evaluation value of the input signal evaluation unit, drive Since the feedforward gain used in the unit is calculated by the feedforward command value calculation unit, even if the movement direction indicated by the user is a discrete signal such as only ON and OFF, the robot arm type treatment table It can operate smoothly and with good responsiveness.

  Even if the input signal of the user is discrete, it is possible to easily operate according to the user's intention by automatically adjusting the responsiveness from the history data.

(First modification)
A robot control apparatus according to a first modification of the second embodiment is shown in FIG. The robot control apparatus according to the first modification has a configuration in which, in the robot control apparatus according to the second embodiment shown in FIG. 4, the drive unit 101A is replaced with the drive unit 101B, and the CPU 1A is replaced with the CPU 1B. The drive unit 101B is a torque command type drive unit that receives a drive torque command value and generates a current for driving the actuator. The CPU 1B has a configuration in which a control command calculation unit 310 is newly provided in the CPU 1A. Note that the control command calculation unit 310 is a module, and may be realized by hardware or software.

  In this first modified example, as shown in FIG. 7, the control command calculation unit 310 uses the joint speed target value calculated by the joint speed target value calculation unit 305 and the joint angle detected by the joint angle detection unit 102. From the feedback gain calculated by the feedforward command value calculation unit 309, the drive torque command value is calculated as shown in Expression (9) and sent to the drive unit 101B.

  The drive unit 101B drives the actuator 100 based on the drive torque command value calculated by the control command calculation unit 310.

  The inertia moment calculation unit 308 calculates the load load mounted on the robot from the drive torque command value calculated by the control command calculation unit 310, and calculates the inertia moment based on this.

  This first modification can also achieve the same effects as those of the second embodiment.

  As described above, each embodiment has been described. However, it should not be understood that the description and drawings constituting a part of this disclosure limit the present invention. From this disclosure, various alternative embodiments, examples and operational techniques will be apparent to those skilled in the art. For example, in the above-described embodiment, the description has been made on the rotary joint, but it is obvious that the present invention can be similarly applied to a linear joint.

  Further, the CPUs 1A, 1B, the link parameter storage unit 201, and the like may be embedded and integrated in the robot to be controlled. The CPUs 1A and 1B, the link parameter storage unit 201, and the like are outside the robot to be controlled, and the robot can be remotely controlled by wire or wirelessly.

  Although several embodiments of the present invention have been described, these embodiments are presented by way of example and are not intended to limit the scope of the invention. These embodiments can be implemented in various other forms, and various omissions, replacements, and changes can be made without departing from the scope of the invention. These embodiments and modifications thereof are included in the invention described in the claims and equivalents thereof as well as included in the scope and gist of the invention.

1, 1A, 1B Central processing unit (CPU)
DESCRIPTION OF SYMBOLS 10 Robot 11 1st joint axis 12 2nd joint axis 13 3rd joint axis 14 4th joint axis 15 5th joint axis 16 6th joint axis 17 7th joint axis 20 Top plate 100 Actuator 101, 101A, 101B Drive part 102 Joint angle detection unit 103 Target value input unit 104 Speed limit presentation unit 105 Control point position calculation unit 106 Jacobian matrix calculation unit 107 Target value shaping unit 108 Target speed calculation unit 109 Speed monitoring unit 110 Speed suppression unit 111 Redundancy evaluation unit 201 Link Parameter storage unit 202 Maximum speed storage unit 302 Motion direction designation unit 303 Tip speed target value calculation unit 305 Joint speed target value calculation unit 307 Input signal evaluation unit 308 Inertia moment calculation unit 309 Feedforward command value calculation unit 310 Control command calculation unit

Claims (5)

An actuator for driving a joint axis of a robot equipped with an arm-type treatment table for each control cycle;
A joint angle detector for detecting an angle of the joint axis;
A control point position calculation unit that calculates position coordinates of the control point of the robot based on the joint angle and the link parameter;
A Jacobian matrix calculating unit that calculates a Jacobian matrix between a joint coordinate system and a work coordinate system in each joint based on the detected joint angle and the link parameter;
A target value input unit for inputting a target position of the control point and a target value of the control point;
A target value shaping unit for shaping the target value of the control point;
Calculate the difference between the target value of the control point shaped by the target value shaping unit and the current value of the control point calculated by the control point position calculation unit, perform inverse kinematics calculation using the Jacobian matrix, A target speed calculator for calculating a target speed at the control point;
A speed monitor that monitors the target speed and limits the target speed to be equal to or lower than the threshold when the calculated target speed exceeds a threshold;
A speed suppression unit that corrects a target value of the control point when the calculated target speed exceeds the threshold;
A speed limit presenting unit that presents a restricted motion direction to the user when the motion direction of the robot is restricted by the speed suppression unit;
A first drive unit that calculates a command value based on the target speed output from the speed monitoring unit and the joint angle, and drives the actuator based on the command value;
With
The target value shaping unit, when the target value of the control point is corrected by the speed suppression unit, shapes the corrected target value. An operation direction designating unit for instructing an operation direction of a control point of the robot;
A tip speed target value calculation unit that calculates a tip speed target value for each control period at the control point based on the instructed movement direction;
A joint velocity target value calculation unit for performing inverse kinematics calculation using the calculated tip velocity target value and the Jacobian matrix, and calculating a velocity target value of the joint axis;
An input signal evaluator that changes the trajectory parameters based on the history and operation time of the movement direction;
Based on the detected joint angle, the link parameter, and a command value of the drive unit, an inertia moment calculation unit that estimates a patient load mounted on the robot and calculates a moment of inertia of the link; ,
A feedforward command value calculation unit that calculates a feedforward command value based on the inertia moment and the evaluation value of the input signal evaluation unit;
The robot control apparatus according to claim 1, further comprising: a second drive unit that drives the actuator based on the calculated speed target value and the feedforward command value. A control command calculator that calculates a drive torque command value based on the calculated speed target value and the feedforward command value;
The inertia moment calculation unit estimates a load of a patient mounted on the robot based on the detected joint angle, the link parameter, and the drive torque command value, and calculates a moment of inertia of the link. And
The robot control apparatus according to claim 2, wherein the second drive unit drives the actuator based on the drive torque command value.   4. The robot control apparatus according to claim 2, wherein the input signal evaluation unit records a change pattern of the input signal, and changes a response parameter included in the designed approximate function according to a use situation of the user. . The robot has an actuator more redundant than the degree of freedom of the target value of the control point set in the target value input unit,
Redundancy for calculating a redundancy parameter calculated by using the movable range of the joint and the degree of proximity of the singular point of the Jacobian matrix as an evaluation function so that the target speed is calculated when the robot approaches the limit of movement or the singular point The robot controller further includes a sex evaluation unit,
5. The robot according to claim 1, wherein the target speed calculation unit calculates the target speed by performing an inverse kinematic calculation of a redundancy degree of freedom based on the redundancy parameter. Control device.

Images