JP2013146792A - Robot controller, robot system, sensor information processing device and robot control method - Google Patents

Abstract

PROBLEM TO BE SOLVED: To provide a robot controller or the like which performs non-linear impedance control and facilitates the verification of the stability of a solution of a motion equation and hardware implementation.SOLUTION: A robot controller includes a force control unit 20 which outputs a correction value of a target track of a robot 100 based on a detection sensor value acquired from a force sensor 10, a target value outputting unit 60 which performs correction processing on the target track based on the correction value to obtain a target value and outputs the target value, and a robot control unit 80 which performs the feedback control of the robot 100 based on the target value. The force control unit 20 has a digital filter unit 22 which obtains a solution of an ordinary differential equation in force control as the correction value, and performs force control so that a first displacement variation amount being a displacement variation amount for an external force when virtual displacement for the robot 100 corresponding to the correction value is first displacement differs from a second displacement variation amount being a displacement variation amount when the virtual displacement is second displacement.

Description

    The present invention relates to a robot control device, a robot system, a sensor information processing device, a robot control method, and the like.

  In operations performed using a robot such as a manipulator, there are various constraints such as operations involving contact with an object. In such cases, force control is often required in addition to position control. For example, when tracing the surface of an object, fitting one object to another object, gripping a flexible object without destroying it, etc., in addition to simply controlling the position, respond to the reaction force from the object Movement is required.

  A typical technique for performing force control in a robot is a technique called impedance control. Impedance control is a control method for operating a robot as if it had those values suitable for work regardless of its actual mass, viscosity characteristics, and elastic characteristics. This is a control method that solves an equation of motion based on force information obtained from a force sensor attached to a robot and operates the robot according to the solution. By appropriately setting this equation of motion, it becomes possible to operate a robot such as a manipulator as if it has a predetermined mass, viscosity, and elasticity.

  In impedance control, in order to make a robot or the like behave as if it has a desired characteristic (mass, viscosity characteristic, elastic characteristic), an ordinary differential equation (second-order equation) using a coefficient parameter corresponding to the characteristic is used. It is necessary to solve the equation of motion (linear ordinary differential equation). Various methods for solving ordinary differential equations are known, but the Runge-Kutta method, Newton method, and the like are used.

  As a conventional technique related to such impedance control and force control, a technique disclosed in Patent Document 1 or Patent Document 2 is known.

JP-A-10-128855 JP 2011-8360 A

  In the impedance control, for example, force is specified as virtual spring expansion / contraction (virtual displacement).

  However, when a large force is required and a large virtual displacement is specified, when the external force for the force disappears, the manipulator makes a very large movement corresponding to the specified virtual displacement. Such a large movement is very problematic for practical use and safety.

  In addition, by designating a large virtual displacement, it is possible in principle to generate a very large force (external force), but the generated force destroys the workpiece that is the work target of the robot. There are also problems such as fear.

  These are problems that arise due to impedance control that outputs linearly with respect to virtual displacement. Patent Document 1 and Patent Document 2 described above disclose a technique for performing impedance control that performs nonlinear output with respect to virtual displacement.

  First, Patent Document 1 discloses a technique for performing nonlinear impedance control by using an elastic term of an equation of motion in force control as a nonlinear function with respect to virtual displacement.

  However, this technique has a problem that it is very difficult to determine whether the equation of motion used for force control is stable.

  Patent Document 2 has two (plural) control mechanisms and two control parameters, assumes a hyperplane called a sliding plane in the control state space, and is a space where the control state is divided by the hyperplane. A method is disclosed that switches between two (multiple) control mechanisms and two control parameters depending on which is present, thereby constraining the control state on the sliding surface and ensuring the stability and convergence of the control system. ing.

  However, this technique has a problem in that it is difficult to design a sliding mode control system and lacks practicality.

  According to some aspects of the present invention, a robot control device, a robot system, a sensor information processing device, a robot control method, and the like that perform non-linear impedance control and facilitate the verification of the stability of the equation of motion and the implementation of hardware. Can be provided.

  One aspect of the present invention provides a force control unit that outputs a correction value of a target trajectory of a robot based on a detection sensor value acquired from a force sensor, and a correction process based on the correction value with respect to the target trajectory. A target value output unit configured to obtain a target value and output the obtained target value; and a robot control unit configured to perform feedback control of the robot based on the target value. A digital filter unit that obtains a solution of an ordinary differential equation in the control as the correction value, and the force control unit is an external force when the virtual displacement of the robot corresponding to the correction value is the first displacement A robot control device that performs force control in which a first displacement change amount that is a displacement change amount with respect to a second displacement change amount that is a displacement change amount when the virtual displacement is a second displacement is different Related to.

  In one embodiment of the present invention, digital filter processing is performed to obtain a solution of an ordinary differential equation in force control as a correction value. Further, the displacement change amount when the virtual displacement is the first displacement is defined as the first displacement change amount, and the displacement change amount when the virtual displacement is the second displacement different from the first displacement is the second displacement. In the case of the change amount, the force control unit performs force control in which the first displacement change amount and the second displacement change amount are different.

  As described above, by performing force control in which the first displacement change amount and the second displacement change amount are different, nonlinear impedance control can be realized. Furthermore, it is possible to facilitate hardware implementation by digitalizing a process for obtaining a solution of an ordinary differential equation in force control. Moreover, it becomes possible to verify the stability of the solution of the equation of motion by using a combination of linear systems.

  In the aspect of the invention, the force control unit may be configured such that the first displacement change amount is the second displacement when the virtual displacement is the first displacement larger than the second displacement. The correction value that is larger than the displacement change amount may be output.

  Thereby, for example, it is possible to perform force control such that the displacement change amount increases as the absolute value of the virtual displacement increases.

  In the aspect of the invention, the force control unit may be configured such that the first displacement change amount is the second displacement when the virtual displacement is the first displacement larger than the second displacement. The correction value that is smaller than the amount of change in displacement may be output.

  Thereby, for example, it is possible to perform force control such that the displacement change amount becomes smaller as the absolute value of the virtual displacement becomes larger.

  In the aspect of the invention, the force control unit may include a control parameter storage unit that stores a plurality of digital filter parameter sets corresponding to coefficient parameter sets of the terms of the ordinary differential equations, and the force control unit. The section selects the first parameter set of the digital filter when the virtual displacement is the first displacement, and when the virtual displacement is the second displacement, The second parameter set of the digital filter is selected, the solution of the ordinary differential equation in force control is obtained as the correction value using the parameter set of the selected digital filter, and the correction value is output. Good.

  This makes it possible to realize nonlinear impedance control by switching the parameter set of the digital filter used for impedance control.

  In the aspect of the invention, the force control unit may be configured so that the virtual displacement is a third displacement included in a range from the first displacement to the second displacement. A weighting process is performed on the first correction value obtained using the first parameter set and the second correction value obtained using the second parameter set to obtain a third correction value. The obtained third correction value may be output.

  As a result, using a plurality of digital filter parameters, the output value when digital filter processing is performed is obtained, the correction value is obtained by weighted addition of each output value, and nonlinear impedance control is realized, etc. Is possible.

  In one aspect of the present invention, the control parameter storage unit stores an offset parameter used when obtaining the correction value, and the force control unit uses the offset parameter to be used based on the virtual displacement. And the solution of the ordinary differential equation in force control may be obtained.

  Thereby, for example, the continuity of the response of the impedance control can be increased at the point of switching from the first parameter set to the second parameter set.

  In one aspect of the present invention, the force control unit determines the stability of the operation of the digital filter unit for obtaining the correction value, and when the operation of the digital filter unit is determined to be stable. The parameter set of the digital filter may be selected and the solution of the ordinary differential equation in force control may be obtained as the correction value.

  This makes it possible to determine the stability of the digital filter.

  In the aspect of the invention, the force control unit performs the first force control when the direction of the virtual displacement of the robot corresponding to the correction value is the first direction. When the direction of the virtual displacement is the second direction opposite to the first direction, the second force control different from the first force control may be performed.

  This makes it possible to switch the force control to be executed based on the virtual displacement direction.

  In the aspect of the invention, the ordinary differential equation may be an equation of motion having a virtual mass term, a virtual viscosity term, and a virtual elasticity term as coefficient parameters.

  Thereby, it is possible to obtain a solution of the equation of motion.

  According to another aspect of the present invention, a force control unit that outputs a correction value of a target trajectory of a robot based on a detection sensor value acquired from a force sensor, and the correction value based on the correction value for the target trajectory. A target value output unit that performs correction processing to obtain a target value, and outputs the obtained target value; and a robot control unit that performs feedback control of the robot based on the target value, the force control unit Is a first displacement change amount that is a displacement change amount with respect to an external force when a virtual displacement of the robot corresponding to the correction value is a first displacement, and the virtual displacement is a second displacement. This is related to a robot control apparatus that performs force control different from the second displacement change amount that is the displacement change amount.

  Another aspect of the present invention relates to a robot system including the robot control device and the robot that operates each unit based on the target value acquired from the target value output unit.

  As a result, it is possible to realize a robot system that executes the processing of this embodiment as well as the robot control device.

  According to another aspect of the present invention, a sensor information processing unit that outputs a correction value of a target trajectory of a control object based on a detection sensor value acquired from a sensor, and the correction value for the target trajectory. A target value output unit that performs a correction process based on the target value and outputs the calculated target value; and a control unit that performs feedback control of the control object based on the target value, and the sensor The information processing unit includes a digital filter unit, and the sensor information processing unit is a displacement change amount with respect to an external force when a virtual displacement of the control object corresponding to the correction value is a first displacement. The present invention relates to a sensor information processing apparatus that performs force control in which a certain first displacement change amount is different from a second displacement change amount that is the displacement change amount when the virtual displacement is a second displacement.

  Accordingly, it is possible to realize a sensor information processing apparatus that obtains an ordinary differential equation solution and outputs an output value by digital filter processing on sensor information.

  In another aspect of the present invention, a solution of an ordinary differential equation in force control is obtained as a correction value of a target trajectory of the robot based on a detection sensor value acquired from a force sensor, and the correction value corresponds to the correction value. A first displacement change amount that is a displacement change amount with respect to an external force when the virtual displacement of the robot is a first displacement, and the displacement change amount when the virtual displacement is a second displacement. A force control different from the second displacement change amount is performed, a correction process based on the correction value is performed on the target trajectory to obtain a target value, the calculated target value is output, and the target value is The present invention relates to a robot control method characterized by performing feedback control of the robot based on the above.

  According to another aspect of the present invention, there is provided a method for controlling a robot having a force sensor, wherein a displacement change amount of the position of the robot when an external force having a first magnitude is applied to the force sensor; Force control different from the displacement change amount of the position of the robot when an external force of a second magnitude that is different from the first magnitude is applied to the force sensor is performed. It relates to the robot control method.

2 is a basic configuration example of a robot control device and a robot system. 2A and 2B are examples of the robot system. FIG. 3A to FIG. 3C are explanatory diagrams of force control. FIG. 4A and FIG. 4B are explanatory diagrams regarding compliance control. FIGS. 5A and 5B are explanatory diagrams of impedance control. FIG. 6A to FIG. 6C are explanatory diagrams of problems in linear impedance control. A basic configuration example of a control system when force feedback is not included. The example of basic composition of a control system in case a force feedback is included. The basic form of a digital filter when finding the solution of the equation of motion. FIG. 10A to FIG. 10C are explanatory diagrams of a system stability determination method. 2 is a basic configuration example of a robot control device and a robot system using a digital filter. FIG. 12A to FIG. 12D are explanatory diagrams of nonlinear impedance control. 1 is a detailed system configuration example according to a first embodiment. The digital filter of 1st Embodiment. The flowchart explaining the impedance digital filter process of 1st Embodiment. Example of linear impedance control response to sinusoidal external force. Nonlinear impedance control response example to sinusoidal external force. An example of an inappropriate technique for switching digital filter parameters. The detailed system configuration example of 2nd Embodiment. The digital filter of 2nd Embodiment. The flowchart explaining the impedance digital filter process of 2nd Embodiment. An example of weighted addition of two impedance processing outputs. FIG. 23A and FIG. 23B are specific system configuration examples for obtaining a target trajectory, a correction value, and a target value.

  Hereinafter, this embodiment will be described. First, an outline of the present embodiment will be described. Next, a system configuration example and detailed processing will be described for the first embodiment and the second embodiment, respectively. In addition, this embodiment demonstrated below does not unduly limit the content of this invention described in the claim. In addition, all the configurations described in the present embodiment are not necessarily essential configuration requirements of the present invention.

1. 1. Overview 1.1 Basic Configuration FIG. 1 shows a configuration example of a robot control device (manipulator control device) of the present embodiment and a robot system including the same. Note that the robot control apparatus and the robot system according to the present embodiment are not limited to the configuration shown in FIG. 1, and various modifications may be made such as omitting some of the components or adding other components. Is possible. In addition, the configuration of the sensor information processing apparatus that can also control objects other than the robot can be the same as the configuration of the robot control apparatus described below.

  The robot control apparatus according to the present embodiment includes a force control unit 20, a target value output unit 60, and a robot control unit 80. The robot system according to this embodiment includes the robot control device and the robot 100 (force sensor 10).

  The target value output unit 60 outputs a target value for feedback control of the robot (manipulator in a narrow sense). Based on this target value, feedback control of the robot 100 is realized. Taking an articulated robot or the like as an example, this target value is information on the joint angle of the robot. The joint angle information of the robot is information representing the angle of each joint (angle formed by the joint axis) in the link mechanism of the robot arm, for example.

  The target value output unit 60 can include a trajectory generation unit 62 and an inverse kinematics processing unit 64. The trajectory generator 62 outputs the trajectory information of the robot. The trajectory information can include position information (x, y, z) of the end effector section (end point) of the robot and rotation angle information (u, v, w) about each coordinate axis. The inverse kinematics processing unit 64 performs inverse kinematics processing based on the trajectory information from the trajectory generation unit 62, and outputs, for example, robot joint angle information as a target value. The inverse kinematics process is a process for calculating the motion of a robot having a joint, and is a process for calculating joint angle information and the like by inverse kinematics from the position and orientation of the end effector unit of the robot.

  The force control unit 20 (impedance control unit in a narrow sense) performs force control (force control) based on sensor information from the force sensor 10, and outputs a correction value of the target value. More specifically, the force control unit 20 (impedance control unit) performs impedance control (or compliance control) based on sensor information (force information and moment information) from the force sensor 10. Force control is, for example, control in which force feedback is added to conventional position control. Impedance control is a technique for making the ease of displacement (mechanical impedance) of the end effector section (hand) with respect to external force a desirable state by control. Specifically, in a model in which a mass, a viscosity coefficient, and an elastic element are connected to an end effector unit of a robot, the control is performed so that the object comes into contact with the mass, the viscosity coefficient, and the elastic coefficient set as targets. The force sensor 10 is a sensor that detects a force or moment received as a reaction force of the force generated by the robot 100. The force sensor 10 is usually attached to the wrist portion of the arm of the robot 100, and the detected force and moment are used for various types of force control (impedance control) as sensor information.

  The robot control unit 80 performs feedback control of the robot based on the target value from the target value output unit 60. Specifically, the feedback control of the robot is performed based on the target value output as a result of the correction process based on the correction value from the force control unit 20. For example, feedback control of the robot 100 is performed based on the target value and a feedback signal from the robot 100. For example, the robot control unit 80 includes a plurality of drive control units 82-1 to 82-N (in a narrow sense, motor control units), and controls the control signals for the drive units 102-1 to 102-N included in the robot 100. Is output. Here, the drive units 102-1 to 102-N are drive mechanisms for moving each joint of the robot 100, and are realized by, for example, a motor or the like.

  FIG. 2A shows an example of a robot system including the robot control apparatus of this embodiment. This robot system includes a control device 300 (information processing device) and a robot 310 (robot 100 in FIG. 1). The control device 300 performs control processing for the robot 310. Specifically, control for operating the robot 310 is performed based on the operation sequence information (scenario information). The robot 310 includes an arm 320 and a hand (gripping unit) 330. Then, it operates according to an operation instruction from the control device 300. For example, operations such as gripping or moving a workpiece placed on a pallet (not shown) are performed. Further, information such as the posture of the robot and the position of the workpiece is detected based on captured image information acquired by an imaging device (not shown), and the detected information is sent to the control device 300.

  The robot control apparatus according to the present embodiment is provided, for example, in the control apparatus 300 in FIG. 2A, and the robot control apparatus is realized by hardware or a program of the control apparatus 300. And according to the robot control apparatus of this embodiment, while being able to reduce the performance requirement with respect to control hardware, such as the control apparatus 300, it becomes possible to operate the robot 310 with high responsiveness.

  In FIG. 2A, the robot body 310 (robot) and the control device 300 (robot control device) are configured separately, but the robot of this embodiment is limited to the configuration of FIG. 2A. Instead, the robot body 310 and the control device 300 may be configured integrally as shown in FIG. Specifically, as shown in FIG. 2B, the robot includes a robot main body 310 (having an arm 320 and a hand 330), and a base unit that supports the robot main body 310, and the control unit 300 is included in the base unit. Is stored. In the robot shown in FIG. 2B, wheels and the like are provided in the base unit portion, and the entire robot is movable. 2A is an example of a single arm type, the robot may be a multi-arm type robot such as a double arm type as shown in FIG. 2B. Note that the robot may be moved manually or by providing a motor for driving the wheels and controlling the motor by the control device 300.

1.2 Force Control / Impedance Control Next, an outline of force control and impedance control (compliance control) will be described.

  FIG. 3A shows a situation where the object OB is sandwiched between the left arm AL and the right arm AR of the robot. For example, the position control alone may cause an object to be dropped or destroyed. According to the force control, a flexible object or a fragile object can be moved while being sandwiched with appropriate force from both sides as shown in FIG.

  Further, according to the force control, as shown in FIG. 3B, it is possible to trace the surface SF of an uncertain object with the arm AM or the like. Such control cannot be realized only by position control. Further, according to the force control, as shown in FIG. 3C, after rough positioning, it is possible to probe and align the object OB into the hole HL.

  However, there is a problem that the application is limited in force control using actual mechanical parts such as a spring. In addition, in such force control using mechanical parts, it is difficult to dynamically switch characteristics.

  On the other hand, torque control for controlling the torque of the motor is simple, but there is a problem that the positional accuracy is deteriorated. In addition, a problem such as a collision occurs when an abnormality occurs. For example, in FIG. 3A, when an abnormal situation occurs and the object OB is dropped, there is no reaction force to be balanced in the torque control, so the left and right arms AL and AR collide. Problems arise.

  On the other hand, although impedance control (compliance control) is complicated, there is an advantage that versatility and safety are high.

  FIG. 4A and FIG. 4B are diagrams for explaining the compliance control which is one of the impedance controls. Compliance means the reciprocal of the spring constant, and the spring constant represents hardness, whereas compliance means softness. Control that provides compliance, which is mechanical flexibility, when interaction between the robot and the environment works is called compliance control.

  For example, in FIG. 4A, a force sensor SE is attached to the arm AM of the robot. The robot arm AM is programmed so that its posture changes according to sensor information (force / torque information) obtained by the force sensor SE. Specifically, the robot is controlled as if a virtual spring indicated by A1 in FIG. 4A is attached to the tip of the arm AM.

  For example, it is assumed that the spring constant of the spring indicated by A1 is 100 kg / m. If this is pressed with a force of 5 kg as shown at A2 in FIG. 4B, the spring contracts by 5 cm as shown at A3. In other words, if it shrinks by 5 cm, it can be said that it is pushed with a force of 5 kg. That is, force information and position information are associated linearly.

  In the compliance control, control is performed as if the virtual spring indicated by A1 is attached to the tip of the arm AM. Specifically, the robot operates in response to the input of the force sensor SE, and is controlled to move back by 5 cm as shown in A3 with respect to the 5 kg weight shown in A2, corresponding to the force information. The position information is controlled to change.

  Such simple compliance control does not include a time term, but the control including the time term and considering the second order term is impedance control. Specifically, the second-order term is a mass term, the first-order term is a viscosity term, and the impedance control model can be expressed by an equation of motion as shown in the following equation (1).

上式（１）において、ｍは質量、μは粘性係数、ｋは弾性係数、ｆは力、ｘは目標位置からの変位である。またｘの１次微分、２次微分は、各々、速度、加速度に対応する。インピーダンス制御では、上式（１）の特性をアームの先端であるエンドエフェクター部に持たせるための制御系を構成する。即ち上式（１）で表される仮想質量、仮想粘性係数、仮想弾性係数を、あたかもアームの先端が持っているかのように制御を行う。

In the above equation (1), m is mass, μ is a viscosity coefficient, k is an elastic coefficient, f is a force, and x is a displacement from a target position. The first and second derivatives of x correspond to speed and acceleration, respectively. In the impedance control, a control system for providing the end effector unit, which is the tip of the arm, with the characteristic of the above formula (1) is configured. That is, control is performed as if the tip of the arm has the virtual mass, virtual viscosity coefficient, and virtual elastic coefficient represented by the above equation (1).

  As described above, impedance control is control that makes a contact with an object with a viscosity coefficient and an elastic coefficient set as an object in a model in which a viscous element and an elastic element are connected to the mass of the tip of the arm in each direction. .

  For example, as shown in FIG. 5A, let us consider a control in which an object OB is held by robot arms AL and AR and moved along a trajectory TR. In this case, the trajectory TRL is a trajectory through which the point PL set inside the left side of the object OB passes, and is a virtual left-hand trajectory determined on the assumption of impedance control. The trajectory TRR is a trajectory through which the point PR set inside the right side of the object OB passes, and is a virtual right-hand trajectory determined on the assumption of impedance control. In this case, the arm AL is controlled so that a force corresponding to the distance difference between the tip of the arm AL and the point PL is generated. The arm AR is controlled so that a force corresponding to the distance difference between the tip of the arm AR and the point PR is generated. In this way, it is possible to realize impedance control that moves the object OB while grasping it softly. In the impedance control, even if the situation where the object OB falls as shown by B1 in FIG. 5A occurs, the ends of the arms AL and AR are points PL and PR as shown by B2 and B3. It is controlled to stop at the position. That is, if the virtual trajectory is not a collision trajectory, it is possible to prevent the arms AL and AR from colliding.

  In addition, as shown in FIG. 5B, when the control is performed so that the surface SF of the object is traced, the impedance control is performed according to the distance difference DF between the virtual trajectory TRVA and the tip with respect to the tip of the arm AM. It is controlled so that the applied force works. Therefore, the arm AM can be controlled to trace the surface SF while applying a force.

  These examples show how linear impedance control is performed. Therefore, for example, in the example of FIG. 5A, when the object OB cannot be gripped unless a stronger force is applied, it is necessary to set PL to the right hand AR side and PR to the left hand AL side. . In this case, if the object OB falls, the right hand AR of the robot moves to the point PR and the left hand AL moves to the point PL as it is, and there is a possibility that both hands collide. Thus, in linear impedance control, since a linear solution (force, external force) is determined with respect to virtual displacement, the force applied to the object OB is maintained while the points PL and PR are set at the positions shown in FIG. I can't be strong.

  Similarly, FIGS. 6A to 6C show examples in which the fingertip of the robot moves along the surface of an object with unevenness or moves while removing the unevenness on the surface.

  When not much force is required as in the case of cutting the convex OBS1 in FIG. 6A, the force required to cut the convex OBS1 with a small virtual displacement such that the target position of the fingertip of the robot is PD1. Can be generated.

  However, when a large force is required as in the case of cutting the convex OBS2 in FIG. 6B by using only the same impedance control parameters as in FIG. 6A, the target position of the fingertip of the robot is set as PD2. It is necessary to generate such a large virtual displacement.

  At this time, if the reaction force disappears as shown in FIG. 6C, the manipulator of the robot will move greatly. Although this can occur in human work, in reality, humans change the force level by the force applied to the object and the virtual displacement.

  Therefore, in an embodiment to be described later, a method of outputting a non-linear solution (force) with respect to the virtual displacement and avoiding the collision in FIG. 5A and the large movement in FIG. 6C is used.

1.3 Configuration of Control System Here, FIG. 7 shows a basic configuration example of a control system when force feedback is not included.

  The trajectory generation unit 562 generates trajectory information p (xyzuww) and outputs it to the inverse kinematics processing unit 564. Here, the trajectory information p includes, for example, position information (xyz) of the tip of the arm (end effector section) and rotation information (uvw) about each axis. Then, the inverse kinematics processing unit 564 performs inverse kinematics processing based on the trajectory information p, and generates and outputs a joint angle θ of each joint as a target value. Then, by performing motor control based on the joint angle θ, the operation control of the robot arm is performed. In this case, the control of the motor (M) in FIG. 7 is realized by known PID control. Since this PID control is a known technique, a detailed description thereof is omitted here.

  In FIG. 7, the trajectory generation unit 562 and the inverse kinematics processing unit 564 constitute a target value output unit. The processing of this target value output unit is the overall processing of the robot. On the other hand, the latter-stage motor control is control for each joint.

  FIG. 8 shows a basic configuration example of the control system in the case of including force feedback. 8 further includes a force sensor 510, a posture correction unit 532, a hand / tool self-weight correction unit 534, a motion equation processing unit 536, and a forward kinematics processing unit 540, compared to FIG. Yes.

  In FIG. 8, in response to sensor information from the force sensor 510, the posture correction unit 532 performs sensor posture correction, and the hand / tool self-weight correction unit 534 performs hand / tool self-weight correction. Then, the equation of motion processing unit 536 performs processing for obtaining a solution of the equation of motion as shown in the above equation (1), and outputs a correction value Δp. By correcting the trajectory information p with this correction value Δp, a correction process for the joint angle θ, which is a target value, is performed. The forward kinematics processing unit 540 performs forward kinematics processing, obtains the trajectory information p ′ of the robot, and feeds it back to the trajectory generation unit 562. Information for specifying the posture is output to the posture correction unit 532 and the hand / tool self-weight correction unit 534. The feedback of the robot trajectory information p ′ to the trajectory generation unit 562 is for performing a trajectory correction process based on p ′. If the correction process is not performed, the feedback is not necessarily performed. unnecessary.

  The hand / tool self-weight correction unit 534 performs hand / tool self-weight correction, and the posture correction unit 532 performs posture correction. Here, the hand / tool self-weight correction is a correction process for offsetting the influence of the weight of the robot hand and the weight of the tool held by the hand from the sensor information (force information) from the force sensor 10. The posture correction is a correction process for offsetting the influence of the posture of the force sensor 10 from the sensor information (force information). These hand tool self-weight correction and posture correction can be expressed as, for example, the following expression (2).

上式（２）において、Ｆｘ，Ｆｙ，Ｆｚ、Ｆｕ，Ｆｖ，Ｆｗは力覚センサー１０からのセンサー情報である力情報、トルク情報である。またＢｘ，Ｂｙ，Ｂｚ、Ｂｕ，Ｂｖ，Ｂｗはバイアス項である。そして、補正後のセンサー情報（力情報、トルク情報）であるｆｘ，ｆｙ，ｆｚ、ｆｕ，ｆｖ，ｆｗが運動方程式処理部５３６に入力される。なお、データには固定値があるため、実質的な補正係数は６×７＝４２個となる。これらのハンド・ツール自重補正及び姿勢補正は公知の補正処理であるため、詳しい説明は省略する。

In the above equation (2), Fx, Fy, Fz, Fu, Fv, and Fw are force information and torque information that are sensor information from the force sensor 10. Bx, By, Bz, Bu, Bv, and Bw are bias terms. Then, the corrected sensor information (force information, torque information) fx, fy, fz, fu, fv, fw is input to the motion equation processing unit 536. Since the data has a fixed value, the substantial correction coefficient is 6 × 7 = 42. Since these hand tool self-weight correction and posture correction are known correction processes, detailed description thereof will be omitted.

1.4 Digital Filter Processing The equation of motion processing unit 536 in FIG. 8 needs to find the solution of the equation of motion (ordinary differential equation in a broad sense). Conventionally, a Newton method, a Runge-Kutta method, or the like has been used to obtain a solution of the equation of motion. However, these methods are not suitable for hardware and it is difficult to determine stability. Further, there is a problem that it is difficult to cope with switching of responsiveness.

  Therefore, the present applicant uses a digital filter as a technique for solving an ordinary differential equation in order to deal with the above three problems.

1.4.1 Solving equations of motion using digital filters The equations of motion are expressed in the form of equation (1) above. Since the equation of motion is a linear ordinary differential equation, if an impulse response that is a solution to the impulse input is obtained, a solution for an arbitrary external force term can be obtained by convolution of the impulse response and the external force term.

  Here, if the step of obtaining the solution of the equation of motion is regarded as a filter that outputs a solution (for example, position information) in response to input of sensor information of the force sensor, from the form of the above equation (1), It can be thought of as a two-pole analog filter.

  That is, since the solution of the equation of motion can be obtained as the output of the analog filter, the equation of motion can be solved using the digital filter by converting the analog filter into a digital filter.

  Various methods for converting an analog filter into a digital filter are known. For example, an impulse invariance method may be used. This is a technique for considering a digital filter that gives the same impulse response as the value obtained by sampling the impulse response of an analog filter at a discrete time T. Since the Impulse Invariance method is a known method, detailed description thereof is omitted.

As a result, the solution of the ordinary differential equation can be obtained as the output of the digital filter. If it is an equation of motion, it will become a two-pole digital filter as shown in FIG. In FIG. 9, d is a delay for one sample, and C ₀ , C ₁ , and C ₂ are digital filter coefficients (digital filter parameters). The relationship between the input value F and an output value Y _n of the digital filter in FIG. 9 can be expressed by Equation (3).

デジタルフィルターによる処理であれば、ハードウェアー化は容易であるし、後述するように安定性の判定も容易である。また、デジタルフィルターの係数を切り替えれば、特性（柔らかく動かすか固く動かすか等）を切り替えることもできるし、フィルター駆動周波数を切り替えて解の応答性を切り替えることもできる。

If the processing is performed using a digital filter, it is easy to implement hardware, and it is also easy to determine stability as described later. In addition, by switching the coefficient of the digital filter, it is possible to switch the characteristics (whether it is moved softly or hardly), or to switch the response of the solution by switching the filter drive frequency.

1.4.2 Judgment of digital filter stability In impedance control, depending on the setting of the mass term (m), viscosity term (μ), and elasticity term (k) of the equation of motion, there is a possibility that an unstable system may be formed. There is. In an extreme example, once a force is applied to a robot, it can be an oscillating system where the robot continues to vibrate even though it has not been touched at all. Such a system with low stability (stability) is not preferable in practice, and therefore it is necessary to determine the stability of the system related to the equation of motion, and to take some measures if it is not stable.

  However, with the Newton method and the Runge-Kutta method described above, it is possible to determine the solution of the equation of motion, but it is not possible to determine the stability. Therefore, a process for determining stability is required separately from the process for obtaining a solution, but it is generally known that the process for determining stability is not easy.

  In the method of the present embodiment, since the equation of motion is processed using a digital filter, the determination of the stability of the system related to the equation of motion is nothing but the determination of the stability of the corresponding digital filter. The stability of the digital filter can be easily determined, and it is only necessary to determine whether the pole is in the unit circle.

  Specific examples are shown in FIGS. 10A to 10C. These are all examples in which the pole is within the unit circle, but if the pole is outside the unit circle, it is determined that the pole is not stable. In addition, there is no particular problem when there is a pole at a position away from the circumference of the unit circle to some extent as shown in FIG. However, when there is a pole at a position very close to the unit circle as shown in FIG. 10A (note that FIG. 10A is an example in which there are two poles at a position that is infinitely close rather than a multiple root), Caution must be taken. This is because an error may occur with respect to the design value depending on the digital filter mounting method. If the error causes the pole position to move in the direction outside the unit circle, a digital filter with no margin of stability as shown in FIG. Since some operations may be unstable at times, some countermeasure is required.

1.4.3 Configuration Example Using Digital Filter FIG. 11 shows a configuration example of a robot control device, a robot system including the same, and a sensor information device in the case of obtaining a solution of an equation of motion using the digital filter. Note that the robot control device, the robot system, and the sensor information device of the present embodiment are not limited to the configuration in FIG. 11, and various components such as omitting some of the components or adding other components. Variations are possible.

  The force sensor 10, the target value output unit 60, the robot control unit 80, and the robot 100 are the same as those in FIG.

  The force control unit 20 includes a digital filter unit 22. The digital filter unit 22 performs digital filter processing on sensor information from the force sensor (including information obtained by performing correction processing and band limitation processing on the sensor information), and uses the output value as a correction value as a target. Output to the value output unit 60. The force control unit 20 may include a band limiting unit 25 that performs band limiting processing on the sensor information.

  The digital filter unit 22 includes a digital filter calculation unit 221, a digital filter coefficient output unit 222, and a digital filter stability determination unit 223. The digital filter calculation unit 221 performs digital filter processing based on the sensor information and the digital filter coefficient to obtain a solution of the equation of motion. The digital filter coefficient output unit 222 obtains a digital filter coefficient based on the coefficient parameters of the equation of motion (mass term m, viscosity term μ, elasticity term k, and driving period T), and the digital filter calculation unit 221 and the digital filter stability The data is output to the determination unit 223. The digital filter stability determination unit 223 determines the stability of the digital filter based on the digital filter coefficient.

  The digital filter coefficient output unit 222 may include a digital filter coefficient storage unit 224 and a digital filter coefficient conversion unit 225. The digital filter coefficient conversion unit 225 converts coefficient parameters of the equation of motion into digital filter coefficients. The digital filter coefficient storage unit 224 stores the converted digital filter coefficient. If a plurality of digital filter coefficients are stored in the digital filter coefficient storage unit 224, it is possible to switch the operation characteristics of the robot and the responsiveness of the solution by switching the output digital filter coefficients.

1.5 Nonlinear Impedance Control Next, an outline of nonlinear impedance control realized in the first embodiment and the second embodiment described below will be described with reference to FIGS. 12 (A) to 12 (C). .

  In the first embodiment and the second embodiment, the displacement change amount of the position of the robot when an external force of the first magnitude is applied to the force sensor is different from the first magnitude. Force control different from the displacement change amount of the position of the robot when an external force of the second magnitude is applied to the force sensor is performed.

  Specifically, first, FIG. 12A shows an example of linear impedance control. The graph in FIG. 12A shows that the displacement increases proportionally as the external force increases. Such a relationship is established between the external force and the displacement in FIGS. 5 (A), 5 (B) and 6 (A) to 6 (C).

  Next, FIG. 12B shows an example of nonlinear impedance control in which the displacement is less likely to increase when the external force increases beyond a predetermined threshold. That is, the control is such that the generated external force tends to increase as the displacement increases. When such nonlinear impedance control is performed, for example, as shown in FIGS. 6A to 6C, even when there is a convex OBS2 that cannot be cut unless a large force is applied, the displacement is not increased (the target position PD2). Without having to be set far away). As a result, it is possible to prevent the manipulator from moving too much even after the convex OBS 2 is cut. Note that the graph of FIG. 12B requires an inclination to make the relationship between the external force and the displacement unique, so the inclination is not reduced to 0 no matter how much the external force is increased.

  Further, FIG. 12C shows an example of nonlinear impedance control in which the external force (drag) is less likely to increase when the displacement becomes larger than a predetermined threshold. In other words, when the external force is large, the displacement is very large and nonlinear impedance control is performed. If such nonlinear impedance control is performed, for example, when the object OB is gripped as shown in FIG. 5A, the displacement can be increased without crushing the object OB.

  Hereinafter, a first embodiment and a second embodiment that realize nonlinear impedance control as shown in FIGS. 12B and 12C based on the configuration of FIG. 11 will be described. The first embodiment is a basic form of the present invention and is an example in which nonlinear impedance control is performed by switching parameters of a digital filter to be used. In the second embodiment, each of the two types of digital filter parameters is used to weight the output value when impedance control is performed, and the sum of the two types of weighted output values is used as a correction value. It is an example which performs force control.

2. First Embodiment 2.1 Configuration FIG. 13 shows a configuration example of a robot control apparatus according to the first embodiment.

  The force sensor 10, target value output unit 60 (orbit generation unit 62, inverse kinematics processing unit 64), robot control unit 80 (motor control unit 82-1 to motor control unit 82-N), and the like are the same as in FIG. Therefore, detailed description is omitted. The input correction unit 30 performs correction processing on the detected sensor value (sensor information), and may include, for example, the posture correction unit 532 and the hand / tool self-weight correction unit 534 shown in FIG. The forward kinematics processing unit 40 corresponds to the forward kinematics processing unit 540 of FIG. 8 and outputs the result of the forward kinematics processing to the input correction unit 30 and also outputs it to the trajectory generation unit 62 as necessary. Also good.

  The force control unit 20 of the robot control device includes an impedance processing unit 21, a control parameter storage unit 24-1 to a control parameter storage unit 24-N, a parameter selection unit 26, and a correction determination unit 27. Note that the force control unit 20 of the present embodiment is not limited to the configuration shown in FIG. 13, and various modifications such as omitting some of the components or adding other components are possible. is there.

  The impedance processing unit 21 in FIG. 13 corresponds to the digital filter calculation unit 221 in FIG. Furthermore, the control parameter storage unit 24-1 to control parameter storage unit 24-N in FIG. 13 correspond to the digital filter coefficient storage unit 224 in FIG. 11, and the parameter selection unit 26 in FIG. This corresponds to the conversion unit 225.

  The control parameter storage unit 24-1 to the control parameter storage unit 24-N store different control parameters. Here, the control parameter may be a coefficient parameter of an equation of motion described later, or may be a parameter of a digital filter. Furthermore, you may memorize | store the offset parameter mentioned later as a control parameter. The function of the control parameter storage unit can be realized by a memory such as a RAM, an HDD (hard disk drive), etc., and may actually be constituted by a single memory or a plurality of memories. .

  The parameter selection unit 26 selects a control parameter to be used from among the control parameters stored in the control parameter storage unit 24-1 to the control parameter storage unit 24-N, and outputs the control parameter to the impedance processing unit 21. Further, when the control parameter stored in the control parameter storage unit 24-1 to control parameter storage unit 24-N is a coefficient parameter of the equation of motion, a process of converting the coefficient parameter into a parameter of the digital filter is performed.

  The correction determination unit 27 determines whether to change (switch) the parameter of the digital filter used for impedance control based on the correction value (virtual displacement) calculated by the impedance processing unit 21, and determines to change Is notified to the parameter selection unit 26.

Further, when the offset parameter is used, the digital filter is as shown in FIG. In the digital filter shown in FIG. 14, as expressed in Expression (4), a value (Output) obtained by adding an offset parameter (Offset) to the output value Y _n (see Expression (3)) of the digital filter in FIG. 9 is a correction value. Calculated as (output value).

図１４に示したデジタルフィルターによる処理であれば、ハードウェアー化は容易であるし、安定性の判定も容易である。

If the processing by the digital filter shown in FIG. 14 is performed, hardware implementation is easy and determination of stability is also easy.

2.2 Detailed Processing Next, the flow of digital filter processing performed by the force control unit in the present embodiment will be described using the flowchart of FIG. FIG. 16 shows a response example of linear impedance control to a sine wave external force, and FIG. 17 shows a response example of nonlinear impedance control to a sine wave external force. Here, the description of FIG. 16 will be made first, and then the explanation of FIG. 17 will be made with the explanation of the flowchart of FIG.

  First, in FIG. 16, a curve of the external force F is represented with the vertical axis representing an arbitrary scale and the horizontal axis representing time (seconds). The graph of FIG. 16 shows the response of linear impedance control to the external force F when the coefficient parameters (virtual mass, virtual viscosity, virtual stiffness) of the equation of motion are set to PR1 and PR2 shown in FIG. Yes. The driving cycle is 125 microseconds (8 ksamples / second), which is the same as the cycle of the current control loop of the manipulator, and the external force is a sine wave with a cycle of 1 second.

  In PR1 and PR2 of FIG. 16, the virtual mass and the virtual viscosity are the same, but the virtual stiffness is PR2 that is twice as large as PR1. Therefore, the robot responds like a so-called soft spring when setting PR1, and responds like a spring that is twice as hard as setting PR1 when setting PR2. Since there are a viscosity term and a mass term, the phase of response when setting PR1 or PR2 is shifted from the phase of external force.

  Next, FIG. 17 will be described. The external force F, response at the time of setting PR1, and response at the time of setting PR2 shown in FIG. 17 are the same as those in FIG. In FIG. 17, a response curve (VPR1) when nonlinear impedance control is performed by changing a parameter (or coefficient parameter) of a digital filter to be used is newly added based on the virtual displacement.

  In FIG. 17, the setting used when the output (response) is in the range from −1 to +1 (hereinafter referred to as the soft region) is PR1, and the output (response) is other than that (hereinafter referred to as the hard region). The setting used for is PR2.

  As a flow of the digital filter processing, first, the output timing is waited (S101), and when the output timing is reached, an external force (external force value) F after posture correction is acquired from the input correction unit (S102).

Next, Equation (3) is calculated to obtain Y _n (S103). Further, an offset value (Offset) is added to the obtained Y _n to obtain an output value (Output) (S104). Since the initial value of the digital filter parameter is the soft parameter (PR1), the response curve of VPR1 is along the soft parameter curve (response curve when setting PR1).

  Here, it is determined whether the previous output value and the currently calculated output value sandwich the setting boundary (S105).

  If it is determined that the previous output value and the currently calculated output value sandwich the setting boundary, a digital filter parameter set corresponding to the region to which the currently calculated output value belongs is acquired (S106). . The setting boundary (boundary threshold) can be arbitrarily set. In this example, −1 and +1 on the vertical axis are set as the setting boundaries. Therefore, in this example, the digital filter parameter is switched to PR2 for the first time when the response at the time of setting VPR1 becomes +1 or more for the first time. Then, the next digital filter processing is performed using the acquired digital filter parameter set. The parameter of the digital filter is switched to the hard parameter (PR2), but since there are mass (inertia) term and viscosity term, the response curve when VPR1 is set gradually changes from the soft parameter curve to the hard parameter curve (PR2 setting). To the response curve. In addition, when the response at the time of setting VPR1 becomes +1 or less (or when it becomes -1 or more), the hard parameter is switched to the soft parameter. Similarly, in this case, the mass (inertia) term and the viscosity term are switched. Therefore, the curve gradually changes from a hard curve to a soft curve.

  On the other hand, if it is determined that the previous output value and the output value calculated this time do not sandwich the setting boundary, the process returns to step S101 without changing the digital filter parameter set (S107). In this example, such processing is repeated.

  When switching the parameters of the digital filter to be used as in this example, in the soft region, the slope of the response curve is steep, the displacement (correction value) for generating the same force is large, and the resolution is high. On the other hand, in the hard region, the slope of the response curve is gentle, and the displacement for generating the same force is further suppressed.

  The robot control device according to the present embodiment described above is based on the detection sensor value acquired from the force sensor 10 and outputs a correction value for the target trajectory of the robot 100, and a correction value for the target trajectory. A target value output unit 60 that performs a correction process based on the target value to obtain a target value and outputs the obtained target value, and a robot control unit 80 that performs feedback control of the robot 100 based on the target value. And the force control part 20 has the digital filter part 22 which calculates | requires the solution of the ordinary differential equation in force control as a correction value. Furthermore, the force control unit 20 has a first displacement change amount that is a displacement change amount with respect to an external force when the virtual displacement of the robot 100 corresponding to the correction value is the first displacement, and the virtual displacement is Force control different from the second displacement change amount that is the displacement change amount in the case of the second displacement is performed.

  First, in the force control, the correction value of the target trajectory of the robot 100 is obtained based on the detection sensor value acquired from the force sensor 10, and the obtained correction value is output.

  Here, the detection sensor value (sensor information) used for the digital filter processing may be an output value itself from the force sensor 10 or a correction process by the input correction unit 30 is performed on the output value. It may be what was done. Moreover, the band limiting process by the band limiting unit 25 (described in FIG. 11) may be performed. Furthermore, the information may be mathematically equivalent to these.

  The correction value is a value obtained by the force control unit 20 and used for correcting the target trajectory by the target value output unit 60. For example, the correction value is the displacement represented in the graphs of FIGS. 12B and 12C. The displacements shown in the graphs of FIGS. 12B and 12C are impedance control responses (outputs) to external forces, and are not values that represent the distance or the like that the manipulator or the like of the robot 100 actually moved. . Therefore, in order to distinguish from the displacement when the manipulator or the like of the robot 100 actually moves, this displacement is also referred to as virtual displacement (virtual displacement).

  Then, the target value output unit obtains the target value by performing correction processing based on the correction value with respect to the target trajectory.

  Here, the target value is a target value in the feedback control of the robot 100, and the control in the robot control unit 80 is performed based on the target value. The target value can be acquired by performing a correction process using a correction value on the target trajectory.

  Further, the target trajectory may represent a change in the spatial target position of the end effector section (end point) of the robot 100 in a narrow sense. One target position is represented by, for example, three-dimensional space coordinates xyz (a rotation angle uvw around each axis may be added in consideration of the posture), and the target trajectory is a set of the target positions. Become. However, the target trajectory is not limited to this, and may be a set of target joint angles of the robot 100. In the robot 100 having a joint, when the angle of each joint is determined, the position of the end effector unit is uniquely determined by forward kinematics processing. That is, in an N-joint robot, one target position can be expressed by N joint angles (θ1 to θN). If the set of N joint angles is one target joint angle, what is a target trajectory? It can be considered as a set of target joint angles. Therefore, the correction value output from the force control unit 20 may be a value related to the position or a value related to the joint angle.

  Specific examples are shown in FIGS. 23A and 23B. If the equation of motion of the above equation (1) is used as an ordinary differential equation for which a solution is obtained by digital filter processing, the solution of the equation of motion is a value related to the position. Therefore, when the target trajectory is the target position, the solution may be used as a correction value as it is, and a system configuration example is as shown in FIG. Note that the target value may be a value related to a position or a value related to a joint angle, but it is generally assumed that the feedback control of the robot 100 uses a joint angle.

  On the other hand, in addition to the inverse kinematics processing unit 64 of the target value output unit 60, a case as shown in FIG. 23B in which the force control unit 20 includes the inverse kinematics processing unit 23 is also conceivable. For example, this is a case where the target trajectory generation process in the target value output unit 60 and the correction value output process in the force control unit 20 have different processing timings and processing rates. In this case, the target trajectory is the target joint angle, and the force control unit 20 performs a conversion process (for example, inverse kinematics process) on the solution of the motion equation to obtain a correction value.

  The force control unit 20 includes a digital filter unit 22 that obtains a solution of an ordinary differential equation in force control as a correction value. The ordinary differential equation in force control is an ordinary differential equation that needs to obtain a solution in force control. That is. In a narrow sense, it may be a linear ordinary differential equation. In a more narrow sense, it is an ordinary differential equation that requires a solution to behave as if the robot had the desired characteristics (mass, viscosity, elasticity, etc.), as shown in equation (1) It may be an equation of motion.

  Further, the displacement change amount when the virtual displacement is the first displacement is defined as the first displacement change amount, and the displacement change amount when the virtual displacement is the second displacement different from the first displacement is the second displacement. In the case of the change amount, the force control unit 20 performs force control in which the first displacement change amount and the second displacement change amount are different.

  Here, the displacement change amount means a change amount of displacement with respect to an external force. For example, in the graph of FIG. 12B, the displacement change amount refers to the slope of a curve representing the displacement. In other words, when a force is applied to the manipulator of the robot from a certain direction, the momentary displacement of the manipulator is measured by gradually changing the force, and when graphed as shown in FIG. The slope of the graph is the displacement change amount.

  Therefore, in order to confirm such a displacement change amount, for example, the first external force, the second external force, and the third external force having different sizes are applied to the manipulator of the robot, and the first external force is applied. The displacement of the manipulator at the time, the displacement when the second external force is applied, and the displacement when the third external force is applied are obtained. Then, the difference between the displacement of the manipulator when the first external force is applied and the displacement when the second external force is applied, and the displacement when the second external force is applied and the third external force are applied. The difference from the displacement can be confirmed as the displacement change amount. If the difference between the first external force and the second external force (second external force and third external force) is made small, a value similar to the slope of the graph can be obtained. That is, the variation amount of the mutation can also be referred to as a difference between the displacement when the first external force is applied and the displacement when the second external force different from the first external force is applied. At this time, the difference between the first external force and the second external force may be minute. Here, minute means that it is close to zero.

  Further, when the displacement change amount is confirmed as described above, if the displacement increases in proportion to the external force, it is understood that the relationship as shown in FIG. 12A is established between the displacement and the external force. If the displacement does not increase even though the external force is increased, it can be seen that the relationship shown in FIG. 12B is established between the displacement and the external force.

  In this way, nonlinear force control can be realized by performing force control in which the first displacement change amount and the second displacement change amount are different. Specifically, even when the external force suddenly disappears, for example, when the external force is large, the movement of the manipulator can be limited to a predetermined range by performing control as shown in FIG. it can. For example, even if an object gripped by the manipulator of the robot is fragile, for example, by performing the control as shown in FIG. ) Can be limited within a predetermined range. This is very effective for practical use and safety of the robot.

  In addition, since it is possible to use a digital filter to perform a process necessary in force control for obtaining a solution of an ordinary differential equation, when using a method such as the Newton method, the Runge-Kutta method, or the above-described patent document Compared with the case where the sliding mode control as shown in FIG. The response characteristics can be easily switched by switching the digital filter used for the digital filter processing (for example, switching the filter coefficient).

  Furthermore, since the robot control apparatus of this embodiment is a combination of linear systems, it is easy to determine the stability of the solution of the equation of motion.

  Further, the force control unit 20 outputs a correction value that causes the first displacement change amount to be larger than the second displacement change amount when the virtual displacement is the first displacement larger than the second displacement. May be.

  Thereby, for example, it is possible to perform force control such that the displacement change amount increases as the absolute value of the virtual displacement increases. That is, force control as shown in the graph of FIG.

  For example, while the force applied to the robot manipulator is small to some extent, it is difficult to push the manipulator as if the manipulator repelled, but when the force applied to the manipulator increases, the manipulator does not resist and the manipulator moves according to the applied force It becomes like this.

  Further, the force control unit 20 outputs a correction value in which the first displacement change amount becomes smaller than the second displacement change amount when the virtual displacement is the first displacement larger than the second displacement. May be.

  Thereby, for example, it is possible to perform force control such that the displacement change amount becomes smaller as the absolute value of the virtual displacement becomes larger. That is, force control as shown in the graph of FIG.

  For example, while the force applied to the manipulator of the robot is small to some extent, the manipulator is easy to push in, and the manipulator moves according to the applied force. However, when the force applied to the manipulator increases, it becomes difficult to push the manipulator as if it had repelled.

  In addition, by combining these force controls, when the first displacement is within the first range, force control is performed so that the displacement change amount increases as the absolute value of the virtual displacement increases. When the first displacement is in the second range, it is possible to perform a modification such as performing force control so that the displacement change amount decreases as the absolute value of the virtual displacement increases.

  Further, the force control unit 20 may include a control parameter storage unit 24 that stores a plurality of digital filter parameter sets corresponding to coefficient parameter sets for each term of the ordinary differential equation. Then, when the virtual displacement is the first displacement, the force control unit 20 may select the first parameter set of the digital filter. Further, when the virtual displacement is the second displacement, the force control unit 20 selects the second parameter set of the digital filter, and uses the selected parameter set of the digital filter to constantly perform the force control. A solution of the differential equation may be obtained as a correction value, and the correction value may be output.

  Here, the coefficient parameter of each term of the ordinary differential equation refers to the constant term, the coefficient of the first derivative term, the coefficient of the second derivative term,... The coefficient of the nth derivative term in the ordinary differential equation. . In the case of the above-described equation (1), m, μ, and k are coefficient parameters.

On the other hand, the digital filter parameter refers to the coefficient of each term in the equation (3). Specifically, it is C ₀ , C ₁ , C ₂ . Note that any one of the first parameter set and the second parameter set is different.

  Specifically, for example, as shown in FIG. 12D, when a predetermined external force and the corresponding virtual displacement belong to the hatched area AR1, the first parameter set PR1 is selected, When the external force and the corresponding virtual displacement belong to the area AR2 outside the area AR1, a selection process such as selecting the second parameter set PR2 is performed.

  In this embodiment, the selection process of the parameter set of the digital filter used for impedance control is performed based on the virtual displacement. However, the selection process may be performed based on an external force instead of the virtual displacement.

  This makes it possible to realize nonlinear impedance control by switching the parameter set of the digital filter used for impedance control.

  Alternatively, the first parameter set and the second parameter set may be mixed to create a third parameter set, which may be used. That is, when the virtual displacement is the third displacement included in the range from the first displacement to the second displacement, the force control unit 20 performs the first parameter set and the second parameter set. The third parameter set of the digital filter may be obtained by performing a weighting process on at least one of the parameter sets, and the solution of the ordinary differential equation may be obtained as a correction value using the third parameter set. .

  Further, the control parameter storage unit 24 may store an offset parameter used when obtaining the correction value. And the force control part 20 may obtain | require the solution of the ordinary differential equation in force control by switching the offset parameter to be used based on virtual displacement.

Here, the offset parameter refers to a virtual displacement when the external force is zero. As shown in FIG. 14 and Expression (4), the offset parameter is used to obtain a correction value by adding to the output value Y _n of Expression (3). For example, in the example of FIG. 12D, the offset parameter of the first parameter set PR1 is 0, and the offset parameter of the second parameter set PR2 is 0 or more. In the examples shown in FIGS. 12B, 12C, 16 and 17, all the offset parameters are 0.

  Thereby, for example, the continuity of the response of the impedance control can be increased at the point of switching from the first parameter set to the second parameter set.

  Further, the force control unit 20 determines the stability of the operation of the digital filter unit 22 for obtaining the correction value, and when the operation of the digital filter unit 22 is determined to be stable, the digital filter parameter set selection process. And the solution of the ordinary differential equation in force control may be obtained as a correction value.

  This makes it possible to determine the stability of the digital filter. Depending on the setting, the coefficient parameter of the ordinary differential equation in force control may create a system (for example, an oscillating robot) that is impossible in reality. Therefore, it is necessary to determine the stability of the ordinary differential equation. However, if a digital filter is used, the determination becomes easy.

  The force control unit 20 performs the first force control when the direction of the virtual displacement of the robot 100 corresponding to the correction value is the first direction, and the direction of the virtual displacement is the first direction. When the second direction is opposite to the first direction, second force control different from the first force control may be performed.

  This makes it possible to switch the force control to be executed based on the virtual displacement direction. For example, consider the case of sharpening a pencil with a pencil sharpener. In this case, in the direction in which the pencil is pushed into the pencil sharpener (first direction), force control is performed to increase the virtual displacement, while in the direction in which the pencil is pulled from the pencil sharpener (second direction). It becomes possible to perform force control to reduce the virtual displacement. In this example, it becomes easier to push the pencil into the pencil sharpener, but it is difficult to pull out the pencil from the pencil sharpener. Note that the force control to be executed may be switched based on the direction of the external force instead of the direction of the virtual displacement. That is, when the direction of the external force represented by the detection sensor value is the first direction, the force control unit 20 performs the first force control, and the direction of the external force is opposite to the first direction. In the case of the second direction, second force control different from the first force control may be performed.

  The ordinary differential equation may be an equation of motion having a virtual mass term, a virtual viscosity term, and a virtual elasticity term as coefficient parameters.

  Thereby, it is possible to obtain a solution of the equation of motion. Therefore, the robot 100 can behave as if it has a mass corresponding to the virtual mass term, a viscosity corresponding to the virtual viscosity term, and an elasticity corresponding to the virtual elasticity term.

  Further, the present embodiment described above is based on the above-described robot control device (including the force control unit 20, the target value output unit 60, and the robot control unit 80) and the target value acquired from the target value output unit 60. The present invention relates to a robot system including the robot 100 that operates each unit.

  As a result, it is possible to realize a robot system that executes the processing of this embodiment as well as the robot control device.

  Further, the present embodiment described above includes a sensor information processing unit that outputs a correction value of the target trajectory of the control target based on the detection sensor value acquired from the sensor, and a correction process based on the correction value with respect to the target trajectory. It can be applied to a sensor information processing apparatus including a target value output unit that obtains a target value by performing the above and outputs the obtained target value, and a control unit that performs feedback control of the controlled object based on the target value. In the sensor information processing apparatus, the sensor information processing unit includes a digital filter unit. Then, the sensor information processing unit includes a first displacement change amount that is a displacement change amount with respect to an external force when the virtual displacement of the control object corresponding to the correction value is the first displacement, and the virtual displacement. Force control that is different from the second displacement change amount, which is the displacement change amount when is the second displacement, is performed.

  In the sensor information processing apparatus, the sensor corresponds to the force sensor 10 in FIG. 13 and the like, and the sensor information processing unit corresponds to the force control unit 20. However, the sensor information processing apparatus is not limited to application to the robot control apparatus, and may be applied to other control systems. In that case, information used in the control is output based on sensor information processing (including digital filter processing) for the sensor information. Furthermore, the sensor information processing apparatus is not limited to application to a control system, and may be used for other systems (apparatuses). For example, it may be used in a detection system that detects various information using a sensor.

  Here, the sensor information processing is processing for obtaining useful information by applying some processing to the output value from the sensor, and includes at least processing for obtaining a solution of an ordinary differential equation. For example, in the robot control apparatus, conversion processing into information having physically different meanings such as obtaining a correction value (for example, a correction value of a target position of the robot) from force information related to the force sensor is performed. In the case of a detection system, a process for improving detection accuracy (for example, a process for reducing noise included in sensor information) may be considered.

  The sensor that outputs sensor information is not limited to the force sensor so that the application of the sensor information processing apparatus is not limited to the robot control device. For example, an acceleration sensor, an angular velocity sensor, an orientation sensor, a biological sensor (such as a pulse sensor), or a GPS may be used.

  Accordingly, it is possible to realize a sensor information processing apparatus that obtains an ordinary differential equation solution and outputs an output value by digital filter processing on sensor information. This sensor information processing apparatus may be realized as an IC chip including a sensor, for example. Since the solution of the ordinary differential equation using the sensor information instead of the sensor information itself can be output, the usefulness of the output value of the apparatus increases when combined with other systems.

  In the above description, the case where force control is performed using a digital filter has been described. However, the digital filter may be replaced by other components. The case where the digital filter is replaced by another component is also included in the scope of the present invention.

3. Second Embodiment Here, an example in which the technique of switching digital filter parameters in the first embodiment is inappropriate is shown in FIG. The various settings of the example of FIG. 18 are the same as those of the example of FIG. 16, but are different from the example of FIG. 16 in that a coefficient parameter of PR3 is used instead of PR2 as shown in FIG. Further, the response when impedance control is performed by changing the parameters of the digital filter to be used is shown in the graph as VPR2.

  In the example of FIG. 18, the undulation is generated around P1 of the curve of VPR2. Originally, it should be a smooth curve. Therefore, when the combination of coefficient parameters set of the equation of motion in force control is set to PR1 and PR3, it can be said that such a method of simply switching the digital filter parameters is inappropriate.

  Therefore, in the second embodiment, each of the two types of digital filter parameters is used to weight the output value when impedance control is performed, and the sum of the two types of weighted output values is used as the correction value. Force control.

3.1 Configuration FIG. 19 shows a configuration example of a robot control apparatus according to the second embodiment.

  Force sensor 10, input correction unit 30, forward kinematics processing unit 40, target value output unit 60 (orbit generation unit 62, inverse kinematics processing unit 64), robot control unit 80 (motor control unit 82-1 to motor control) The part 82-N) and the like are the same as those in FIG.

  The force control unit 20 of the robot control device includes an impedance processing unit 21-1, an impedance processing unit 21-2, a control parameter storage unit 24-1, a control parameter storage unit 24-2, a correction mixing unit 28, including. Note that the force control unit 20 of the present embodiment is not limited to the configuration of FIG. 19, and various modifications may be made such as omitting some of the components or adding other components. is there.

  The operations of the impedance processing units and the control parameter storage unit are the same as those in FIG. 13, except that the impedance processing unit and the control parameter storage unit are associated with each other on a one-to-one basis. The control parameter storage unit 24-1 and the control parameter storage unit 24-2 store different control parameters, respectively, and the impedance processing unit 21-1 and the impedance processing unit 21-2 store in the corresponding control parameter storage unit. Impedance processing is performed based on the controlled parameters.

  And the correction | amendment mixing part 28 weights with respect to the output value acquired from the impedance process part 21-1 and the impedance process part 21-2, and calculates the sum of each weighted output value as a correction value.

  The digital filter in the second embodiment is as shown in FIG.

3.2 Detailed Processing Next, the flow of the digital filter processing performed by the force control unit in the present embodiment will be described using the flowchart of FIG.

  The flow of the digital filter process is the same as steps S101 to S104 in FIG. 15 from step S201 to S204. In the first step S203, a parameter corresponding to PR1 is used as a digital filter parameter.

  Then, it is determined whether or not the virtual displacement is included in the range from the first displacement to the second displacement (S205). That is, it is determined whether or not two sets of impedance processes are operated in parallel and the outputs are weighted and added.

  When it is determined that the virtual displacement is included in the range from the first displacement to the second displacement, the processing of step S203 and step S204 is performed using the digital filter parameter not used in step S203 (S206). ). In the case of the first time, since the parameter PR1 is used in step S203, the parameter PR3 is used in step S206.

  Then, a weighted sum of the output values of step S204 and step S206 is obtained according to equation (6) described later (S207), output as an impedance control response (output), and the process returns to step S201.

  On the other hand, if it is determined that the virtual displacement is not included in the range from the first displacement to the second displacement, it is determined whether the previous output value and the calculated output value sandwich the setting boundary. (S208). The setting boundaries are set to -1 and +1 on the vertical axis, as in the example of FIG.

  Next, when it is determined that the previous output value and the calculated output value sandwich the setting boundary, the digital filter parameter corresponding to the region to which the currently calculated output value belongs is acquired (S209). ).

  On the other hand, if it is determined that the previous output value and the currently calculated output value do not sandwich the setting boundary, the digital filter parameter set is not changed, and the process directly returns to step S201. In this example, such processing is repeated.

  FIG. 22 shows an example of a response when weighted addition of two types of impedance processing outputs is obtained. This is the case where the same two sets of control parameters (PR1 and PR3) as in FIG. 18 are used. In this example, instead of switching the parameters, two sets of impedance processes are operated in parallel, and the outputs are weighted and added. doing. In this example, for simplification of description, the weighted sum (CPR1) is always obtained regardless of the magnitude of the virtual displacement.

Note that the output of the soft parameters (PR1) when setting _{Y s,} the output of the hard parameters (PR3) set at a _{Y h,} the weight W in equation (5) represents the output value Output in equation (6).

重み付け関数は、正負両方向に飽和特性を持ち、単調増加関数であれば他の関数でも良いが、ここではｔａｎｈあるいはシグモイド関数（実質的にはｔａｎｈと同一のもの）を用いる。シグモイド関数は２値の系の最大エントロピー状態を与える関数であり、さまざまな場面で現れる。ただし、ここで最大エントロピーに特別の意味があるわけではない。

The weighting function has saturation characteristics in both positive and negative directions, and may be another function as long as it is a monotonically increasing function. Here, a tanh or sigmoid function (substantially the same as tanh) is used. The sigmoid function is a function that gives the maximum entropy state of a binary system and appears in various situations. However, the maximum entropy does not have any special meaning here.

  When the virtual displacement is the third displacement included in the range from the first displacement to the second displacement, the force control unit 20 of the robot control device of the present embodiment described above is The first correction value obtained using the parameter set and the second correction value obtained using the second parameter set are weighted to obtain a third correction value, and the obtained first A correction value of 3 may be output.

  For example, the force control unit 20 may obtain the sum of the first correction value and the second correction value after the weighting process as the third correction value.

  As a result, using a plurality of digital filter parameters, the output value when digital filter processing is performed is obtained, the correction value is obtained by weighted addition of each output value, and nonlinear impedance control is realized, etc. Is possible.

  Although the present embodiment has been described in detail as described above, it will be easily understood by those skilled in the art that many modifications can be made without departing from the novel matters and effects of the present invention. Accordingly, all such modifications are intended to be included in the scope of the present invention. For example, a term described at least once together with a different term having a broader meaning or the same meaning in the specification or the drawings can be replaced with the different term in any part of the specification or the drawings. Further, the configurations and operations of the robot control device, the robot system, and the sensor information processing device are not limited to those described in the present embodiment, and various modifications can be made.

10 force sensor, 20 force control unit, 21 impedance processing unit,
22 digital filter section, 23 inverse kinematics processing section,
24 control parameter storage unit, 25 band limiting unit, 26 parameter selection unit,
27 correction determination unit, 28 correction mixing unit, 30 input correction unit,
40 forward kinematics processing unit, 60 target value output unit, 62 trajectory generation unit,
64 inverse kinematics processing unit, 80 robot control unit,
82 drive control unit (motor control unit), 100 robot, 102 drive unit,
221 digital filter operation unit, 222 digital filter coefficient output unit,
223 digital filter stability determination unit, 224 digital filter coefficient storage unit,
225 Digital filter coefficient conversion unit, 300 controller, 310 robot,
320 arms, 330 hands, 510 force sensor, 532 posture correction unit,
534 Hand tool self-weight correction unit, 536 motion equation processing unit,
540 forward kinematics processing unit, 562 orbit generation unit,
564 Inverse Kinematics Processing Department

Claims (14)

A force control unit that outputs a correction value of the target trajectory of the robot based on a detection sensor value acquired from the force sensor;
A target value output unit that performs correction processing based on the correction value for the target trajectory to obtain a target value, and outputs the calculated target value;
A robot controller that performs feedback control of the robot based on the target value;
Including
The force control unit
A digital filter unit for obtaining a solution of an ordinary differential equation in force control as the correction value;
The force control unit
A first displacement change amount that is a displacement change amount with respect to an external force when a virtual displacement of the robot corresponding to the correction value is a first displacement, and the virtual displacement is a second displacement. A robot control apparatus that performs force control different from the second displacement change amount that is the displacement change amount in the case. In claim 1,
The force control unit
When the virtual displacement is the first displacement larger than the second displacement, the correction value that causes the first displacement change amount to be larger than the second displacement change amount is output. A robot controller characterized by the above. In claim 1,
The force control unit
When the virtual displacement is the first displacement larger than the second displacement, the correction value is output so that the first displacement change amount becomes smaller than the second displacement change amount. A robot controller characterized by the above. In any one of Claims 1 thru | or 3,
The force control unit
A control parameter storage unit that stores a plurality of digital filter parameter sets corresponding to coefficient parameter sets of each term of the ordinary differential equation;
The force control unit
If the virtual displacement is the first displacement, select a first parameter set of the digital filter;
If the virtual displacement is the second displacement, select a second parameter set of the digital filter;
A robot control apparatus characterized in that, using the parameter set of the selected digital filter, a solution of the ordinary differential equation in force control is obtained as the correction value, and the correction value is output. In claim 4,
The force control unit
When the virtual displacement is a third displacement included in the range from the first displacement to the second displacement, a first correction value obtained using the first parameter set And a second correction value obtained using the second parameter set is weighted to obtain a third correction value, and the obtained third correction value is output. Robot control device. In claim 4 or 5,
The control parameter storage unit is
Storing an offset parameter used when obtaining the correction value;
The force control unit
A robot control apparatus characterized by obtaining a solution of the ordinary differential equation in force control by switching the offset parameter to be used based on the virtual displacement. In any one of Claims 4 thru | or 6.
The force control unit
Determining the stability of the operation of the digital filter unit for obtaining the correction value;
When it is determined that the operation of the digital filter unit is stable, the parameter set of the digital filter is selected, and the solution of the ordinary differential equation in force control is obtained as the correction value. Robot control device. In any one of Claims 1 thru | or 7,
The force control unit
When the direction of the virtual displacement for the robot corresponding to the correction value is the first direction, the first force control is performed,
When the virtual displacement direction is a second direction opposite to the first direction, a second force control different from the first force control is performed. . In any one of Claims 1 thru | or 8.
The ordinary differential equation is
A robot control device characterized by an equation of motion having a virtual mass term, a virtual viscosity term, and a virtual elasticity term as coefficient parameters. A force control unit that outputs a correction value of the target trajectory of the robot based on a detection sensor value acquired from the force sensor;
A target value output unit that performs correction processing based on the correction value for the target trajectory to obtain a target value, and outputs the calculated target value;
A robot controller that performs feedback control of the robot based on the target value;
Including
The force control unit
A first displacement change amount that is a displacement change amount with respect to an external force when a virtual displacement of the robot corresponding to the correction value is a first displacement, and the virtual displacement is a second displacement. A robot control apparatus that performs force control different from the second displacement change amount that is the displacement change amount in the case. The robot control device according to any one of claims 1 to 10,
The robot that operates each unit based on the target value acquired from the target value output unit;
A robot system characterized by including: A sensor information processing unit that outputs a correction value of a target trajectory of the control object based on a detection sensor value acquired from the sensor;
A target value output unit that performs correction processing based on the correction value for the target trajectory to obtain a target value, and outputs the calculated target value;
A control unit that performs feedback control of the controlled object based on the target value;
Including
The sensor information processing unit
Has a digital filter section,
The sensor information processing unit
A first displacement change amount that is a displacement change amount with respect to an external force when a virtual displacement of the control object corresponding to the correction value is a first displacement, and the virtual displacement is a second displacement. The sensor information processing apparatus performs force control different from the second displacement change amount which is the displacement change amount in the case of. Based on the detection sensor value obtained from the force sensor, find the solution of the ordinary differential equation in force control as the correction value of the target trajectory of the robot,
A first displacement change amount that is a displacement change amount with respect to an external force when a virtual displacement of the robot corresponding to the correction value is a first displacement, and the virtual displacement is a second displacement. Force control different from the second displacement change amount which is the displacement change amount in the case,
Perform a correction process based on the correction value for the target trajectory to obtain a target value, and output the calculated target value,
A robot control method, wherein feedback control of the robot is performed based on the target value. A method for controlling a robot having a force sensor,
A displacement change amount of the position of the robot when an external force of a first magnitude is applied to the force sensor;
Force control different from the displacement change amount of the position of the robot when an external force of a second magnitude different from the first magnitude is applied to the force sensor is performed. Robot control method.

Images