Position and force control device

Abstract

To provide a position and force control device that can prevent deterioration of position tracking performance even when a limit value of a contact force allowed for contact with a control object is small and when the error between a position command value and a position response value is large.SOLUTION: A position and force control device 1 includes a position controller 2, a saturation compensator 3, and a force controller 4. When a control object is in contact with an environment 6 (Ksw=0) and an allowable upper limit value (fcmd) or an allowable lower limit value (-fcmd) of a contact force at which the control object comes into contact with the environment 6 is a force reference value frefp or less or more, the saturation compensator 3 outputs a limit value that limits the force reference value to the upper limit value or the lower limit value as a force demand value fref. When the control object is in contact with the environment 6 (Ksw=0) and the force reference value does not exceed or fall below the upper limit value or the lower limit value, or when the control object is not in contact with the environment 6 (Ksw=1), the saturation compensator 3 outputs the force reference value as the force demand value.SELECTED DRAWING: Figure 1

Description

[Technical Field]

[0001] The present invention relates to a control device for controlling the position of a controlled object and the force exerted by the controlled object on an object. [Background technology]

[0002] Conventionally, as a control device of this type, there is a compliance control device for a robot disclosed in Patent Document 1, for example.

[0003] This compliance control device includes a means for setting a limit value for the magnitude of the force applied to the contact object by the virtual spring, and a means for calculating the robot's control position based on the saturation characteristics of the virtual spring force determined by the limit value. These means prevent excessive force from being generated when the robot comes into contact with an obstacle or the like, resulting in a situation where the target position and the current position differ significantly, and the force generated by the robot is always kept below a certain limit value. [Prior art documents] [Patent documents]

[0004] [Patent Document 1] Patent No. 3466223 Summary of the Invention [Problem to be solved by the invention]

[0005] However, with the compliance control device disclosed in the above-mentioned conventional Patent Document 1, when the limit value set for the virtual spring force is small, or when the difference between the target position and the current position is large, the force generated by the robot is always kept below the limit value. Therefore, with the above-mentioned conventional compliance control device, in such cases, the virtual spring force becomes saturated even if the robot is not in contact with an obstacle or the like. As a result, the robot cannot quickly track the target position, and position tracking performance deteriorates. [Means for solving the problem]

[0006] The present invention has been made to solve such problems, a position controller that calculates a force reference value that should be output by the driving source so that the position response value follows the position command value, based on a comparison result between a position command value that indicates a target position or trajectory of the controlled object and a position response value of the controlled object due to a driving force generated by the driving source in response to the position command value; a saturation compensator that outputs a limit value obtained by limiting the force reference value to the upper limit value or lower limit value as a force demand value when the controlled object is in contact with an object other than the controlled object and the allowable upper limit value or lower limit value of the contact force with which the controlled object is in contact with said object is equal to or less than a force reference value, and outputs the force reference value as a force demand value when the controlled object is in contact with said object and the force reference value is neither above nor below the upper limit value or lower limit value, and when the controlled object is not in contact with said object; a force controller that, when the controlled object comes into contact with the object and a limit value is input from the saturation compensator as a force request value, performs force control such that the controlled object comes into contact with the object at the limit value based on a comparison result between the limit value and a force response value of the controlled object applied as a reaction force from the object to the controlled object when the controlled object comes into contact with the object at the limit value; when the controlled object comes into contact with the object and a force reference value is input from the saturation compensator as a force request value, performs force control such that the controlled object comes into contact with the object at a contact force equal to or less than the limit value based on a comparison result between the force reference value and a force response value of the controlled object applied as a reaction force from the object to the controlled object; and, when the controlled object is not in contact with the object and a force reference value is input from the saturation compensator as a force request value, performs position control such that the position response value follows the position command value based on a comparison result between the position command value and the position response value. A position and force control device was constructed using the above.

[0007] According to this configuration, the saturation compensator automatically switches between position control and force control of the controlled object by the position and force control device depending on whether or not the controlled object is in contact with an object other than the controlled object.

[0008] That is, when the controlled object is in contact with the object and the force reference value output from the position controller is outside the range of the upper or lower limit, the force reference value is limited to the limit value by the saturation compensator and input to the force controller, and the force controller performs force control such that the controlled object contacts the object with a contact force of the limit value.Also, when the controlled object is in contact with the object and the force reference value output from the position controller is within the range of the upper or lower limit, the saturation compensator inputs a force reference value smaller than the limit value as a force request value to the force controller, and the force controller performs force control such that the controlled object contacts the object with a contact force according to the force reference value.

[0009] On the other hand, when the controlled object is not in contact with the object, regardless of whether the force reference value output from the position controller is outside or within the range of the upper or lower limit value, the force reference value is output from the saturation compensator to the force controller, and position control is performed by the position controller in which the position response value follows the position command value.

[0010] Therefore, when the upper or lower limit of the contact force allowable for contact of the controlled object with the object is small, and when the error between the position command value and the position response value is large, and the controlled object is not in contact with the object, the force reference value output from the position controller is not limited by the saturation compensator, and position control is performed in which the position response value tracks the position command value according to the force reference value calculated by the position controller. Therefore, even when the upper or lower limit of the contact force allowable for contact of the controlled object with the object is small, and when the error between the position command value and the position response value is large, the controlled object can quickly track the target position or trajectory, preventing deterioration of position tracking performance.

[0011] The present invention also provides The saturation compensator is a saturation function element that limits the force reference value to a limit value when the upper limit value or the lower limit value is equal to or less than or equal to the force reference value and outputs the limit value, and that outputs the force reference value when the force reference value does not exceed or fall below the upper limit value or the lower limit value; a difference calculation element that calculates the difference between the force reference value and the output value of the saturation function element; a variable calculation element that multiplies the difference by a switching gain that is 0 when the controlled object is in contact with the object and 1 when the controlled object is not in contact with the object, and outputs the multiplication result; an addition element that adds the output value of the saturation function element and the multiplication result and outputs the addition result as a force request value; The present invention is characterized in that it is composed of:

[0012] According to this configuration, the saturation compensator adds together the difference between the force reference value output from the position controller and the output value of the saturation function element, multiplied by the switching gain, and outputs the result of adding the result to the output value of the saturation function element as the force demand value to the force controller. Therefore, when the controlled object is in contact with the object and the switching gain is 0, the multiplication result is 0, and the force demand value provided to the force controller is the output value of the saturation function element. Therefore, the saturation compensator outputs to the force controller a limit value that limits the force reference value, or a force reference value. When a limit value is input to the force controller, force control is performed so that the controlled object contacts the object with a contact force of the limit value. Furthermore, when the controlled object is in contact with the object and a force reference value is input, force control is performed so that the controlled object contacts the object with a contact force corresponding to the force reference value.

[0013] Furthermore, when the controlled object is not in contact with the object and the switching gain is 1, the addition result becomes a force reference value, and the force reference value is given from the saturation compensator to the force controller as a force request value. When the controlled object is not in contact with the object and a force reference value is input, the force controller performs position control using a position controller in which the position response value follows the position command value.

[0014] The present invention also provides The driving source is an actuator, the position controller comprises a position error calculation element which calculates a difference between a position command value and a position response value as a position error, a position error proportional value calculation element which calculates a position error proportional value by multiplying the position error by a predetermined control gain, a speed command proportional value calculation element which calculates a speed command proportional value by multiplying a speed command value indicating a target speed of the controlled object by the predetermined control gain, a first speed response proportional value calculation element which calculates a first speed response proportional value by multiplying a speed response value of the controlled object due to a driving force generated by the actuator in accordance with the speed command value by a predetermined first control gain, a superposition error calculation element which calculates a position speed superposition error by adding the position error proportional value to the difference between the speed command proportional value and the first speed response proportional value, and a force reference value calculation element which calculates a force reference value by multiplying the position speed superposition error by a nominal inertia value of the actuator, The force controller is composed of a force error calculation element that calculates, as a force error, the difference between a force demand value output from the saturation compensator and a force response value corresponding to the force demand value; a force error proportional value calculation element that multiplies the force error by a predetermined control gain to calculate a force error proportional value; a second speed response proportional value calculation element that multiplies the speed response value by a predetermined second control gain to calculate a second speed response proportional value; an acceleration reference value calculation element that calculates, as an acceleration reference value, the difference between the force error proportional value and the second speed response proportional value; and a force controller output calculation element that multiplies the acceleration reference value by a nominal inertia value of the actuator to calculate a force controller output to the actuator. It is characterized by:

[0015] PD control of position is usually performed in a position controller by multiplying a position error corresponding to the difference between a position command value and a position response value, and a speed error corresponding to the difference between a speed command value and a speed response value, by a predetermined control gain, and then adding the respective proportional values. In this configuration, the force controller feeds back the speed response value required for force control and position control, so the feedback of the speed response value is performed separately in the position controller and the force controller. In other words, the speed response value fed back in the position controller is multiplied by a predetermined first control gain, and the speed response value fed back in the force controller is multiplied by a predetermined second control gain, and their proportional values, the first speed response proportional value and the second speed response proportional value, are first calculated.

[0016] The predetermined first control gain is set to a value that compensates for the insufficiency of the predetermined second control gain in order to cause the position controller to perform PD control of the desired position, and the position controller and force controller perform feedback of the velocity response value, so that the velocity response value is calculated from a control gain equivalent to the sum of the first control gain and the second control gain, which is required for PD control of the desired position. The velocity command value is also multiplied by the predetermined control gain in the position controller, and its proportional value is first calculated, and then the error from the proportional value of the velocity response value is taken. This makes it possible to perform PD control of the desired position while feeding back the velocity response value required for force control and position control in the force controller. [Effects of the Invention]

[0017] According to the present invention, it is possible to provide a position and force control device that can prevent deterioration of position tracking performance even when the upper or lower limit of the contact force allowable for contact with the object to be controlled is small, and even when the error between the position command value and the position response value is large. [Brief explanation of the drawings]

[0018] [Figure 1] 1 is a block diagram showing the configuration of a position and force control device according to an embodiment of the present invention; [Figure 2] 2 is a graph showing a switching gain using a sigmoid function used in the calculation of the saturation compensator shown in FIG. 1; [Figure 3] FIG. 1 is a block diagram for explaining PD control of position in a conventional position controller. [Figure 4] 1(a) is a photograph taken from the side of an experimental machine for confirming the effect of a position and force control device according to one embodiment, and FIG. 1(b) is a photograph taken from diagonally above the experimental machine during an experiment. [Figure 5] (a) is an enlarged photograph of a part of the experimental device near the rod shown in Figure 4, and (b) is a photograph showing the environment in contact with the rod. [Figure 6]FIG. 5 is a conceptual diagram of a position and force control device configured in an experiment using the experimental machine shown in FIG. [Figure 7] Graph (a) shows the change in the position of the rod tip over time in an experiment using the experimental device shown in FIG. 4, and graph (b) shows the change in the force applied to the rod from the environment over time. [Figure 8] 7(a) is an enlarged view of a portion of the graph showing the change in position over time shown in FIG. 7(a), and FIG. 7(b) is a graph showing the change in force demand value over time over the entire range in an experiment using the experimental machine shown in FIG. 4. [Figure 9] 1 is a conceptual diagram of a position and force control device according to an embodiment applied to an articulated manipulator; [Figure 10] (a) is a perspective side view of a three-joint manipulator using the position and force control device shown in Figure 9, and (b) is a side view showing the experimental setup using the three-joint manipulator shown in (a). [Figure 11] (a) is a graph showing the time change in the y-axis position of the hand when the conventional method is used in an experiment using a three-joint manipulator, and (b) is a graph showing the time change in the force in the y-axis direction. [Figure 12] (a) is a graph showing the time change in the y-axis position of the hand of a three-joint manipulator using the proposed method with the position and force control device shown in Figure 9, and (b) is a graph showing the time change in the y-axis force. [Figure 13] FIG. 10 is a diagram showing a specific example in which a three-joint manipulator using the position and force control device shown in FIG. 9 is applied to mechanizing the installation of ALC panels at a construction site. DETAILED DESCRIPTION OF THE INVENTION

[0019] Next, an embodiment of a position and force control device according to the present invention will be described.

[0020] [Configuration of position and force controller] FIG. 1 is a block diagram showing the configuration of a position and force control device 1 according to one embodiment.

[0021] The position and force control device 1 controls, for example, a motor 5 provided at a joint of a multi-joint manipulator as a driving source for the hand. The hand comes into contact with an environment 6, which is an object other than the controlled object. Hereinafter, the object other than the controlled object will be described as the environment 6. If the environment 6 is modeled as a spring-damper system, its transfer function is (D e s+K e ) where D e is the viscosity model for environment 6, K e represents the stiffness model of the environment 6, and s represents the Laplace operator. The position and force control device 1 enables switching between position control and force control depending on the presence or absence of contact force between the hand of a multi-joint manipulator or the like and the environment 6.

[0022] The position of the motor 5 is detected by an encoder as a displacement if the motor 5 is a linear motor, or as an angle if the motor 5 is a rotary motor. The contact force between the motor 5 and the environment 6 can be acquired by a load cell, a reaction force observer (RFOB) 7, or the like. In this embodiment, the contact force that the hand actually receives from the environment 6 is calculated as a force response value f res This contact force estimated by the reaction force estimation observer 7 is expressed as the estimated force response value f res est In this embodiment, for simplicity, the force response value f res The estimation accuracy of is sufficiently high, and the estimated force response value f res est is the actual force response value f res The following explanation will be given assuming that they are equal to

[0023] The position and force control device 1 is configured by the arithmetic processing of a computer (calculator), and includes a position controller 2, a saturation compensator 3, and a force controller 4. The position controller 2, the saturation compensator 3, and the force controller 4 are connected in series. The environment 6 represents a physical phenomenon.

[0024] The position controller 2 receives a position command value x that represents the target position or trajectory of the controlled object. cmdand the position response value x of the controlled object res and the position command value x cmd position response value x res The force reference value f that the motor 5, which is the driving source, should output in order to follow refp Calculate the position command value x cmd can be set to any value depending on the task to be achieved, as long as it does not exceed the mechanical limits of the motor 5. For example, a predetermined trajectory can be set, or the operator can give the trajectory using a device such as a joystick.

[0025] Position response value x res is the position command value x cmd The acceleration response value x" of the motor 5 is due to the driving force generated by the motor 5 in response to the res (=(1 / M)×(f m -f dis )) is a second-order integral. M in the transfer function element (1 / M) of the motor 5 is the inertia value of the motor 5, and the transfer function element (1 / s) using the Laplace operator s represents the integral. m is the control input to motor 5, f dis represents the disturbance value to the motor 5. This disturbance value f dis is the force response value f res and the friction force f inside the motor 5 fric The model is based on the assumption that the above is the sum of the above.

[0026] Also, the initial position of environment 6 is x env When the motor 5 is a single-axis linear motor and the position is expressed as a displacement from the initial position of the motor 5, the initial position x of the environment 6 is expressed as env is the surface of the environment 6 as will be described later (see FIG. 6). In this case, the initial position of the motor 5 is set as the origin, and the coordinate axis is set in the direction of the linear motor movement. The position response value x res From the initial position x env The model assumes that a reaction force from the environment 6 occurs by the amount subtracted from the position response value x resis measured by an encoder (not shown) provided on the motor 5.

[0027] In this embodiment, in order to perform PD (proportional-differential) control of the position in the position controller 2, the position controller 2 is provided with a speed command value x' that indicates a target speed of the object to be controlled. cmd and the estimated speed response value x' res est is also input. Estimated speed response value x' res est is the speed command value x' cmd The position response value x is generated by the driving force generated by the motor 5 in response to the res is pseudo-differentiated by the pseudo-differential operator 10 to obtain the velocity response value x' res The pseudodifferential due to the pseudodifferential operator 10 is calculated as (g pd s) / (s+g pd ) where g pd / (s+g pd ) is a low-pass filter, g pd is the cutoff frequency of the low-pass filter, and s is the differentiation by the Laplace operator. In this embodiment, for simplicity, the velocity response value x' res The estimation accuracy of is sufficiently high, and the estimated speed response value x' res est is the actual speed response value x' res The following explanation will be given assuming that it is equal to

[0028] Speed ​​command value x' cmd and the speed response value x' res In the position controller 2, to which is further input, the position command value x cmd and the position response value x representing the current position of motor 5 res In addition to the comparison result with the speed command value x' cmd and the speed response value x' res The comparison result with the force reference value f refp is calculated and output.

[0029] More specifically, the position controller 2 includes a position error calculation element 2a, a position error proportional value calculation element 2b, a speed command proportional value calculation element 2c, a first speed response proportional value calculation element 2d, a superposition error calculation element 2e, and a force reference value calculation element 2f. The position error calculation element 2a is composed of a summing point and is used to calculate the position command value x cmd to the position response value x res is subtracted to obtain the position command value x cmd and the position response value x res The difference between (x cmd -x res ) is calculated as the position error.

[0030] The position error proportional value calculation element 2b calculates the position error (x cmd -x res ) with the given control gain K pc is multiplied to obtain the position error proportional value (K pc (x cmd -x res The speed command proportional value calculation element 2c is a transfer element that calculates the speed command value x', which represents the target speed of the controlled object. cmd The given control gain K vc is multiplied to obtain the speed command proportional value (K vc ·x' cmd The first speed response proportional value calculation element 2d is a transfer element that calculates the speed response value x' of the controlled object. res The first control gain K vr The first speed response proportional value (K vr ·x' res ) is a transfer element that calculates

[0031] The superposition error calculation element 2e is composed of summing points and calculates the speed command proportional value (K vc ·x' cmd ) to the first speed response proportional value (K vr ·x' res ) is the difference between the speed command proportional value and the first speed response proportional value (K vc ·x' cmd -K vr ·x' res ), and the position error proportional value (K pc (x cmd -x res)) to obtain the position velocity superposition error ((K vc ·x' cmd -K vr ·x' res )+K pc (x cmd -x res The force reference value calculation element 2f calculates the nominal value M of the inertia of the motor 5 by adding the position-velocity superposition error. n Multiplying by the force reference value f refp Calculate the following.

[0032] Therefore, the position controller 2 sends the force reference value f refp will be output. f refp =M n (K pc (x cmd -x res )+K vc ·x' cmd -K vr ·x' res ) …(1)

[0033] The saturation compensator 3 receives the force reference value f refp is input, and the force response value f calculated by the reaction force estimation observer 7 is res Refer to the force demand value f ref to the force controller 4. The saturation compensator 3 is composed of a saturation function element 3a, a difference calculation element 3b, a variable calculation element 3c, and an addition element 3d.

[0034] The saturation function element 3a is a function of determining whether the control object contacts the environment 6 and whether the allowable upper or lower limit of the contact force of the control object contacts the environment 6 is greater than the force reference value f refp When it is less than or greater than the force reference value f refp is limited to the limit value and output. Also, the upper or lower limit is set to the force reference value f refp When does not exceed or fall below the force reference value f refp Output.

[0035] In this embodiment, the saturation function element 3a is a force reference value f refp The saturation function sat(frefp ) and the output is calculated according to the following formula (2):

[0036] For simplicity, the allowable upper limit of the contact force with which the controlled object comes into contact with the environment 6 is set as a positive force command value f cmd , the lower limit is the negative force command value -f cmd The upper and lower limits are assumed to be constants with the same absolute value but different signs. However, the settings of these values ​​are not necessarily limited to this. The force command value f cmd represents the force that the controlled object should exert on the environment 6. f cmd ≦f refp When sat(f refp )=f cmd -f cmd <f refp <f cmd When sat(f refp )=f refp f refp ≦-f cmd When sat(f refp )=-f cmd …(2)

[0037] The limit value is the force command value f cmd By using the force reference value f refp When is saturated, the force command value f cmd is given as an input to the force controller 4.

[0038] The difference calculation element 3b is composed of summing points and calculates the force reference value f refp The output value sat(f refp ) to obtain the force reference value f refp and the difference between the output value of the saturation function element 3a (f refp -sat(f refp )) is calculated.

[0039] The variable calculation element 3c is a switching gain K that is 0 when the controlled object is in contact with the environment 6 and 1 when the controlled object is not in contact with the environment 6. sw Multiply the difference by the above and get the multiplication result (K sw(f refp -sat(f refp The presence or absence of contact between the controlled object and the environment 6 is determined by the force response value f calculated by the reaction force estimation observer 7. res The force response value f res If there is, it is determined that the controlled object is in contact with the environment 6, and the switching gain K sw becomes 0. The force response value f res If there is no contact, it is determined that the controlled object is not in contact with the environment 6, and the switching gain K sw becomes 1. This switching gain K sw is defined as shown in the following equation (3). |f res |>0 when K sw =0 f res When K = 0 sw =1 …(3)

[0040] However, in general, the measured and estimated values ​​of the reaction force contain measurement and estimation errors such as noise and offset. Therefore, in practice, it is necessary to set a threshold value to take into account the measurement and estimation errors of the reaction force. For this reason, the switching gain K expressed in equation (3) sw Instead of the definition of the switching gain K, an approximation using a sigmoid function is used. sw The definition of is shown in the following equation (4). th is the threshold for considering the error, and a is the weight (gain) of the sigmoid function. K sw =1 / (1+exp(a(|f res |-f th )) …(4)

[0041] Figure 2 shows the results for a=500, f th = 0.1, the switching gain K using a sigmoid function sw The horizontal axis of the graph represents the force response value f res , the vertical axis is the switching gain K sw Indicates the value of

[0042] The summation element 3d is composed of summing points and outputs the output value sat(frefp ) and switching gain K sw and the multiplication result of the difference (K sw (f refp -sat(f refp ))) and the result is the force demand value f ref This force demand value f ref is shown in the following equation (5). f ref =sat(f refp )+K sw (f refp -sat(f refp )) …(5)

[0043] The first term on the right side of the above equation (5) represents the saturation element due to the saturation function element 3a, and the second term on the right side represents compensation for saturation due to the variable calculation element 3c. According to equation (5), the switching gain K sw When is 1, the force demand value f ref = force reference value f refp In other words, the force reference value f refp Even if the value of is limited, the force reference value f refp The value of is restored, and the force reference value f is transmitted from the saturation compensator 3 to the force controller 4. refp The value of f is the force requirement value. ref On the other hand, the switching gain K sw When is 0, the force demand value f ref =sat(f refp ) that is, the force reference value f within the range of the limit value by the saturation function element 3a. refp is the force demand value f ref and is output to the force controller 4.

[0044] That is, the saturated compensator 3 is configured to adjust the control object to the environment 6 (K sw =0), and the allowable upper limit of the contact force at which the controlled object comes into contact with the environment 6 (f cmd ) or lower limit (-f cmd ) is the force reference value f refp Below (f cmd ≦f refp ) or greater than (f refp ≦-f cmd ), the force reference value frefp upper limit (f cmd ) or lower limit (-f cmd ) is the limit value of the force demand value f ref In addition, when the controlled object comes into contact with the environment 6 (K sw =0), and the upper limit (f cmd ) or lower limit (-f cmd ) to the force reference value f refp does not exceed or fall below (-f cmd <f refp <f cmd ) and when the controlled object is not in contact with the environment 6 (K sw =1) contains the force reference value f refp The force demand value f ref Output as

[0045] The force controller 4 is composed of a force error calculation element 4a, a force error proportional value calculation element 4b, a second velocity response proportional value calculation element 4c, an acceleration reference value calculation element 4d, and a force controller output calculation element 4e, and is configured by adding velocity feedback to P (proportional) control.

[0046] The force error calculation element 4a is composed of summing points and calculates the force demand value f ref to the required force value f ref Force response value f according to res By subtracting ref and the force response value f res The difference between (f ref -f res ) is calculated as the force error.

[0047] The force error proportional value calculation element 4b calculates a predetermined control gain K f Multiplying by the force error proportional value (K f (f ref -f res The second speed response proportional value calculation element 4c is a transfer element that calculates the speed response value x' res In order to feed back the second control gain K d The speed response value x' res Multiplying by the second speed response proportional value (Kd ·x' res ) is a transfer element that calculates

[0048] The acceleration reference value calculation element 4d is composed of summing points and calculates the force error proportional value (K f (f ref -f res )) to the second speed response proportional value (K d ·x' res ) to obtain the proportional force error value (K f (f ref -f res )) and the second speed response proportional value (K d ·x' res ) is the acceleration reference value x” ref Acceleration reference value x” ref This difference is expressed by the following equation (6). x” ref =K f (f ref -f res )-K d ·x' res …(6)

[0049] The acceleration reference value calculation element 4d calculates the force reference value f refp is saturated (f refp =f cmd ) when the force command value (f cmd Acceleration reference value x” to follow ref is calculated as follows: x” ref =K f (f cmd -f res )-K d ·x' res …(7)

[0050] In addition, when the controlled object is in contact with the environment 6 and the force reference value f refp is not saturated (-f cmd <f refp <f cmd ) when the force reference value f refp (=f ref Acceleration reference value x” to followref On the other hand, when the controlled object is not in contact with the environment 6, the force response value f res = 0, the force reference value f refp (see equation (1)) plus velocity feedback, i.e., the position command value x cmd Acceleration reference value x” to follow ref is calculated as follows: x” ref =K f M n (K pc (x cmd -x res )+K vc ·x' cmd -K vr ·x' res ) -K d ·x' res …(8)

[0051] The force controller output calculation element 4e calculates the acceleration reference value x ref The nominal value of the inertia of motor 5, M n is multiplied to obtain the force controller output (M n ·x” ref ) is the transfer element that calculates the final control input f m is the force controller output (M n ·x” ref ) and the disturbance estimate f output from the disturbance observer (DOB) 11 for disturbance compensation. dis est This control input f m is calculated at summing point 12 and is shown in the following equation (9). f m =M n ·x” ref +f dis est …(9)

[0052] The disturbance observer 11 receives the control input f of the motor 5. m and the velocity response value x' output from the pseudo-differential operator 10. res and the estimated disturbance value fdis est The estimated disturbance value f dis est The estimation accuracy is also high enough, and the actual disturbance value f dis Assume that it is equal to

[0053] As described above, the force controller 4 controls the controlled object when it comes into contact with the environment 6 and the saturation compensator 3 outputs the limit value (f cmd or -f cmd ) to the force demand value f ref When the input is as follows, the controlled object contacts the environment 6 at the limit value, and the force response value f res Based on the comparison result between the limit value and the control object, the force control is performed so that the control object contacts the environment 6 at the limit value. refp The force demand value f ref When input as , the controlled object applies a force reference value f refp The force response value f applied to the controlled object as a reaction force from the environment 6 upon contact at res and the force reference value f refp Based on the comparison result, the controlled object applies a force reference value f refp The contact force is controlled to be equal to or less than a limit value according to the contact force.

[0054] On the other hand, the force controller 4 is configured to operate when the controlled object is not in contact with the environment 6 and the force reference value f refp The force demand value f ref When input as cmd and the position response value x res Based on the comparison result with res is the position command value x cmd The position controller 2 performs position control that follows the

[0055] [Functions and Effects of the Embodiment] According to this embodiment, the position control and force control of the control object by the position and force control device 1 are automatically switched by the saturation compensator 3 depending on whether or not the control object is in contact with the environment 6.

[0056] That is, when the controlled object is in contact with the environment 6, the force reference value f refp is the upper limit (f cmd ) or lower limit (-f cmd ), the force reference value f refp is limited by the saturation compensator 3 (f cmd or -f cmd ) and input to the force controller 4, and the force controller 4 performs force control so that the controlled object contacts the environment 6 with the contact force of the limited value. refp is the upper limit (f cmd ) or lower limit (-f cmd ) range, the limit value (f cmd or -f cmd ) smaller than the force reference value f refp is calculated by the saturated compensator 3 as the force demand value f ref is input to the force controller 4 as a force reference value f refp The force controller 4 performs force control such that the controlled object contacts the environment 6 with a contact force according to the force.

[0057] On the other hand, when the controlled object is not in contact with the environment 6, the force reference value f refp is the upper limit (f cmd ) or lower limit (-f cmd ) both outside and within the range of the force reference value f refp is output from the saturation compensator 3 to the force controller 4, and the position response value x res is the position command value x cmd The position is controlled by the position controller 2 that follows the

[0058] Therefore, the upper limit of the contact force (f cmd ) or lower limit (-f cmd ) is small, and the position command value x cmd and the position response value x res When the error is large and the controlled object is not in contact with the environment 6, the force reference value f refpis not limited by the saturation compensator 3, and the force reference value f refp Depending on res is the position command value x cmd Therefore, the upper limit of the contact force (f cmd ) or lower limit (-f cmd ) is small, and the position command value x cmd and the position response value x res Even when the error is large, the target position or trajectory of the controlled object can be quickly tracked, and the deterioration of the position tracking performance can be prevented.

[0059] As described above, according to this embodiment, a position and force control device 1 is configured that executes position control when the controlled object is not in contact with the environment 6, and executes force control when in contact. Position control and force control are automatically switched depending on the reaction force from the environment 6, and performance degradation of position control due to saturation function element 3a in particular is avoided. Table 1 below summarizes the conditions under which position control and force control are executed. The user of the position and force control device 1 provides a position command value and a force command value according to the task to be achieved. [Table 1]

[0060] In addition, the PD control of the position is usually performed by controlling the position command value x in the position controller as shown in Figure 3. cmd and the position response value x res The position error corresponding to the difference between the cmd and the speed response value x' res The speed error corresponding to the difference between pc , K. vc In this embodiment, the force controller 4 multiplies the velocity response value x' required for performing force control and position control. res To provide feedback, the speed response value x' resThe feedback of the velocity response value x' is performed separately by the position controller 2 and the force controller 4. That is, the velocity response value x' is fed back by the position controller 2. res The first control gain K vr is multiplied by the velocity response value x', which is fed back to the force controller 4. res The second control gain K d are multiplied to obtain the proportional value of the first speed response (K vr ·x' res ) and the second speed response proportional value (K d ·x' res ) is calculated first.

[0061] Predetermined second control gain K d is set to improve the stability of the force control. Also, a predetermined first control gain K vr is a predetermined second control gain K d Therefore, the velocity response value x' in the position controller 2 and the force controller 4 is set to a value taking into consideration the above. res By these feedbacks, the velocity response value x' res is the first control gain K required for PD control of the desired position. vr and the second control gain K d The control gain (K vr +K d ) is calculated from the speed command value x'. cmd In addition, the position controller 2 uses a predetermined control gain K vc is multiplied and the proportional value (K vc ·x' cmd ) is calculated first, and then the speed response value x' res The proportional value of (K vr ·x' res ) is taken as an error. Therefore, the force controller 4 obtains the velocity response value x' required for performing force control and position control. res It is possible to perform PD control for a desired position while providing feedback.

[0062] In the above embodiment, the position controller 2 employs PD control, and the force controller 4 employs P control with velocity feedback, but this is not necessarily the case. Also, the reaction force is estimated using the reaction force estimation observer 7, but it may be measured by a sensor. Also, the force command value f cmd The upper and lower limit values ​​do not need to be equal in absolute value as long as they have different signs. sw does not necessarily have to be a sigmoid function.

[0063] [Example of application of this invention to an experimental aircraft] Next, an example in which the position and force control device 1 according to the embodiment described above is applied to an experimental machine will be described.

[0064] Figure 4(a) is a photograph of the experimental machine from the side, and Figure 4(b) is a photograph of the experimental machine during the experiment, looking down from diagonally above. The control object in this experiment is a linear motor, which is a single-axis direct-acting motor. The mover of the linear motor is supported by a linear guide device and a jig, and is equipped with a rod at its tip.

[0065] Figure 5(a) is an enlarged photograph of a part near the rod, and Figure 5(b) shows the aluminum block, hard sponge, and soft sponge used as the environment 6 in contact with the rod.

[0066] A conceptual diagram of the position and force control device 1A configured in the experiment is shown in FIG. 6. In FIG. 6, the same or corresponding parts as in FIG. 1 are denoted by the same reference numerals. For simplicity, however, the motor driver is omitted, and the control input f m The linear motor is shown as being able to directly output a force when a current is applied. The linear motor is driven by a motor driver. The position and force control device 1A converts the control input calculated in the computer into a voltage, and sends a voltage signal from the computer to the motor driver. The motor driver outputs a corresponding current, causing the linear motor to generate a force.

[0067] The only sensor used is the encoder, which calculates the position response value x resThe velocity response value x' of the linear motor required to configure the position and force control device 1A is measured. res , the disturbance value f applied to the linear motor dis and the force response value f, which is the reaction force applied to the linear motor from the environment 6. res are estimated using the pseudo-differential operator 10, the disturbance observer 11, and the reaction force estimation observer 7, as in the case shown in Figure 1, and the estimated speed response value x' res est , the estimated disturbance value f dis est and the estimated force response value f res est Let's say.

[0068] For modeling purposes, the initial position of the environment surface is set as x env However, it is not measured by sensors. The linear motor pushes the environment (x res -x env The model assumes that the greater the force acting on the object, the greater the stiffness of the environment, and the greater the reaction force that will be generated. However, since it is difficult to actually obtain this environmental information, it is not used in the position and force control device 1A, and the model is merely used to show the overall configuration of the position and force control device 1A.

[0069] In practice, in order to stably operate the position and force control device 1A, the velocity response value x' res The force (or acceleration) to be output by the linear motor is calculated by feeding back the control input f m is expressed as the sum of the output of the force controller 4 and the output of the disturbance observer 11.

[0070] In this experiment, the linear motor is given a position command value x cmd A square wave (maximum value 10 mm, minimum value 0 mm, period 4 seconds) shown by the dotted characteristic line A in the graph of Fig. 7(a) is given, and the force command value f cmdA constant value of 2 [N] is given to the graph in Figure 7(b) as shown by the dotted characteristic line A. The vertical axis of the graph in Figure 7(a) represents the position [mm] of the linear motor rod tip, and the vertical axis of the graph in Figure 7(b) represents the force [N] applied to the rod from the environment. The horizontal axis of each graph also represents the same time [s].

[0071] The experiment was carried out by continuously outputting a square wave for four periods, and the environments were switched in order so that the linear motor did not come into contact with the environment via the rod in the first period from 1 to 3 [s], it came into contact with the aluminum block in the second period from 5 to 7 [s], it came into contact with a hard sponge in the third period from 9 to 11 [s], and it came into contact with a soft sponge in the final period from 13 to 15 [s]. The experimenter manually removed and placed the environments at this time. In addition, the initial position x of each environment surface was env To avoid a large change in the thickness of the aluminum block and the two sponges, we used the same thickness and installed the environment so that the back of the environment was aligned with the wall (see Figure 5(a)). Figure 5(a) shows the state when the aluminum block and the hard sponge are swapped.

[0072] In addition, the conventional method is a case where a compensator for saturation, that is, the difference calculation element 3b, the variable calculation element 3c, and the addition element 3d excluding the saturation function element 3a in the saturation compensator 3, are not used, that is, the case where only the saturation function element 3a is used, and the proposed method using the position and force control device 1A is a case where the difference calculation element 3b, the variable calculation element 3c, and the addition element 3d are used in addition to the saturation function element 3a, and the conventional method and the proposed method are compared.

[0073] In the graphs of Figures 7(a) and (b), the characteristic line B shown in black solid line represents the response position and force by the conventional method, and the characteristic line C shown in gray represents the response position and force by the proposed method. In the first period of the square wave, the environment is not set within the operating range of the linear motor, so as shown in Figure 7(a), the linear motor moves in the direction of the position command value x cmdIt can be observed that the reaction force at this time is ideally zero, but from the graph in Figure 7(b), it can be observed that a sudden change occurs the moment the linear motor starts to move, and that a slight deviation occurs when the linear motor reaches the position command value and stops. This is due to an estimation error in the reaction force estimation observer 7.

[0074] In the variable calculation element 3c constituting the compensator for saturation, the switching gain K is set so that the compensation is switched in consideration of the estimation error of the reaction force estimation observer 7 in practice. sw In this experiment, we conducted a similar experiment in advance to estimate the magnitude of the estimation error and to design the switching gain K sw The threshold value f th (See equation (4)) was set.

[0075] In the second and third periods of the square wave, when the linear motor operates in the forward direction, the rod tip is in contact with the aluminum block and the hard sponge, respectively. The position response at this time is, as shown in Figure 7(a), the position command value x cmd On the other hand, as shown in Fig. 7(b), the force response generates an impact force the moment the linear motor collides with the environment, but the force command value f cmd It can be confirmed that it follows the

[0076] In the section where a reaction force is generated, the linear motor is in contact with the environment, and the response shown in Fig. 7 is cmd This shows that the robot came into contact with the environment midway through the trajectory represented by the square wave, and switched from position control to force control. Also, comparing the position responses shown in Figure 7(a) of the conventional method and the proposed method in the second and third periods of the square wave, there is a deviation between the two responses in the section where the linear motor and the environment are in contact. This is because the environment was placed by hand, and the initial position x of the environment surface between the conventional method and the proposed method env This is thought to be due to the difference.

[0077] In the fourth period of the square wave, when the linear motor operates in the forward direction, the tip of the rod comes into contact with the soft sponge. At this time, neither the position response shown in Figure 7(a) nor the force response shown in Figure 7(b) follow the command value indicated by characteristic line A. This is because the sponge is too soft and does not generate a sufficient reaction force. In both the conventional method and the proposed method, the reaction force applied from the environment is greater than the force command value f cmd If the reaction force is less than the force command value f cmd It can be confirmed that the required operation is met by being controlled to be below (2[N]).

[0078] The graph in Figure 8(a) shows an enlarged view of the position response when the linear motor operates in the positive direction during the first period of the square wave. The vertical and horizontal axes and symbols in this graph are the same as those in the graph in Figure 7(a).

[0079] Comparing the conventional method and the proposed method, the proposed method, represented by characteristic line C, achieves a position command value x represented by characteristic line A more quickly than the conventional method, represented by characteristic line B. cmd In the conventional method, even when the linear motor does not come into contact with the environment, the force reference value f refp On the other hand, in the proposed method, when the linear motor is not in contact with the environment, the force reference value f refp is restored to the magnitude before saturation, the position command value x cmd is following suit.

[0080] The graph in Figure 8(b) shows the force demand f ref The vertical axis of the graph shows the force demand value f ref The horizontal axis is the force [N], and the horizontal axis is the time [s]. In the conventional method, regardless of whether or not there is contact with the environment, the force demand value f ref is the force command value f cmdOn the other hand, in the proposed method represented by characteristic line C, when in contact with the environment (except for the fourth period of the square wave), the force demand value f ref is the force command value f cmd When there is no contact with the environment, such as when starting a movement, the force demand value f ref is the force command value f cmd The above values ​​are shown.

[0081] [Application of the present invention to multi-joint manipulators] The proposed method can also be applied to articulated manipulators. We will apply it to an articulated manipulator that operates on the xy plane, with the horizontal direction being the x-axis and the vertical direction being the y-axis, and consider controlling the position of the end effector and the reaction force acting on the end effector. In this example, we will explain the case where the driving source is an actuator that includes a hydraulic actuator, instead of a motor, which is an electric actuator.

[0082] Figure 9 shows a conceptual diagram of a position and force control device 1B applied to an articulated manipulator. In Figure 9, parts that are the same as or correspond to those in Figure 1 are denoted by the same reference numerals. A position controller 2, a saturation compensator 3, and a force controller 4 are configured on the reference coordinate system. Each of these controllers performs calculations on the x-axis and y-axis, respectively, so the aforementioned command values, reference values, and response values ​​are expanded into vectors, and control gains are expanded into matrices. The control input calculated on the reference coordinate system must be converted into the control input of the actuator that drives each joint by coordinate transformation using a coordinate transformer 13. When disturbance compensation is performed in the joint space, a disturbance is estimated in the joint space by a disturbance estimator 14, and the sum of the control input after coordinate transformation and the estimated disturbance in the joint space is input to the actuator that drives each joint.

[0083] When a multi-joint manipulator has a force sensor at its end and an encoder at each actuator, the reaction force f acting on the end sens , displacement of each joint θ res , the velocity of each joint θ' res When the articulated manipulator has three joints, θ resis a vector whose elements are the displacements of the first, second, and third joints, and θ' res is a vector whose elements are the velocity response values ​​of the first, second, and third joints. f sens is a vector consisting of six variables: the force acting in one direction perpendicular to the surface of the force sensor and two directions horizontal to it, and the torque acting in the directions of those axes. By converting these variables using the coordinate converter 15, the force response value f res , hand position response value x res , speed response value x' res The proposed method can be implemented in articulated manipulators by feeding back the response values ​​converted onto the reference coordinate system to the position controller 2, saturation compensator 3, and force controller 4.

[0084] An example of a three-joint manipulator 20 is shown in the perspective side view of Figure 10(a). In this figure, the x-axis is taken horizontally and the y-axis is taken vertically. The three-joint manipulator 20 has a rotary joint 21 as a first joint, a prismatic joint 22 as a second joint, and a rotary joint 23 as a third joint. The rotary joint 21 is driven by a prismatic actuator 25, the prismatic joint 22 by a prismatic actuator 22, and the rotary joint 23 by a rotary actuator 23a. The displacements of each joint are measured by encoders, and a vector θ res The velocity of each joint is calculated by pseudo-differentiation, and the vector θ' res The rotary joint 23 is provided with a hand 24, which is provided with a force sensor 24a. The measured value of the force sensor 24a is expressed as a vector f sens It is expressed as:

[0085] The position and posture of the hand 24 are determined by the displacement θ of each joint 21, 22, and 23. res Since the hand 24 is equipped with a force sensor 24a, when the hand 24 comes into contact with the environment, f sens By converting the coordinates, the force response value f res As shown in Figure 9, the force response value f resis fed back to the saturation compensator 3 and the force controller 4. Also, the velocity response value x' of the end effector 24 res is fed back to the position controller 2 and the force controller 4. In addition, the position response value x res is fed back to the position controller 2.

[0086] Figure 10(b) shows the experimental setup using a three-joint manipulator 20. Here, the origin O of the reference coordinate system is set on the rotation axis of the first joint, the rotary joint 21. A wooden block 31 is used as the environment that comes into contact with the hand 24. Figure 11 shows the experimental results when the conventional method is applied to this three-joint manipulator 20, and Figure 12 shows the experimental results when the proposed method is applied.

[0087] Fig. 11(a) is a graph showing the time change in the y-axis position of the hand 24 in the conventional method, and Fig. 12(a) is a graph showing the time change in the y-axis position of the hand 24 in the proposed method. The vertical axis of each graph is the y-axis position [m], and the horizontal axis is time [s]. The dotted characteristic line D indicates the position command value x cmd The solid characteristic line E represents the position response value x res 11(b) is a graph showing the time change of the force in the y-axis direction that the hand 24 receives in the conventional method, and FIG. 12(b) is a graph showing the time change of the force in the y-axis direction that the hand 24 receives in the proposed method. The vertical axis of each graph is the force in the y-axis direction [N], and the horizontal axis is time [s]. The dotted characteristic line F indicates the force command value f cmd , the characteristic line G shown in gray is the force demand value f ref The solid line H indicates the force response value f res Represents.

[0088] In this experiment, when the hand 24 moves in the negative direction of the y-axis, the hand 24 comes into contact with the wooden block 31, which is the environment, and the force response value f res The coordinate system is in the opposite direction to that of the three-joint manipulator 20.

[0089] In this experiment, the position command value x set in advance on the computer was used for 25 seconds from the start. cmdAfter that, the external operation device is used to set the position command value x cmd 11(a) and 12(a), after 25 seconds, the position command value x cmd The force command value f shown by the characteristic line F is different. cmd In both the conventional and proposed methods, while the hand 24 is in contact with the wooden block 31 (between 44 and 52 seconds and 63 and 72 seconds in Fig. 11, and between 46 and 56 seconds and 63 and 70 seconds in Fig. 12), the force response value f res It was confirmed that the force can be controlled to a constant value of -10N.

[0090] Regarding the position response, after about 70 seconds, the wooden block 31 moves in the opposite direction, that is, in the positive direction of the y-axis, with the position command value x cmd When the position command value x is changed significantly, the characteristic line E cannot keep up with the rising edge of the characteristic line D in the conventional method shown in Fig. 11(a). cmd position response value x res It can be seen that the force demand value f shown by the characteristic line G at this time is not able to follow the ref Even though the hand 24 is not in contact with the wooden block 31, the force demand value f ref is limited, and it can be seen that the force required for position tracking cannot be output.

[0091] On the other hand, in the proposed method shown in Fig. 12(a), the characteristic line E follows the rising edge of the characteristic line D, and the position command value x cmd position response value x res The force demand value f shown by the characteristic line G in Fig. 12(b) at this time is ref , it shows a value exceeding 10 N after 70 seconds. As the force required for position tracking is restored in this way, the effect of the compensators (difference calculation element 3b, variable calculation element 3c, and addition element 3d) on saturation can be confirmed.

[0092] [Example of a multi-joint manipulator] Figure 13 shows a specific example in which an articulated manipulator is applied to mechanizing the installation of ALC panels at a construction site. In this figure, parts that are the same as or correspond to those in Figure 10 will be explained using the same reference numerals.

[0093] At construction sites for high-rise buildings and apartment buildings, ALC panels are often used as exterior walls and partition walls. Panels weighing around 200 kg each are often used, and mechanization of ALC panel installation work is being promoted to reduce the burden on workers, save labor, and improve productivity. Here, we consider the task of using an articulated manipulator-type machine 41 to hold an ALC panel 42 and adjust its position and posture while installing it in a specified location.

[0094] Fixing brackets 43 for fixing ALC panels 42 are provided at specified locations. The mechanical device 41 is controlled by an operator 44 who operates a remote control 45 held in his / her hand. The remote control 45 is connected by wire to a position and force control device 1B, which is provided on the mechanical device 41 and is composed of a computer (not shown in the figure). In addition, an assistant 47 waits on scaffolding 46 to fix the ALC panels 42 carried by the mechanical device 41 to the fixing brackets 43. The mechanical device 41 is configured to be freely movable, with a three-joint manipulator 20 mounted on a transport vehicle 48.

[0095] This work requires at least the following series of tasks (1) to (5). (1) The ALC panel 42 placed on the floor is grasped by the three-joint manipulator 20. (2) Operate the three-joint manipulator 20 to move the ALC panel 42 around the installation location. (3) Adjust the position and posture of the ALC panel 42 so that it matches the fixing bracket 43 installed at the installation location. (4) While the assistant 47 fixes the ALC panel 42 to the fixing bracket 43 by welding or the like, the three-joint manipulator 20 continues to hold the ALC panel 42. (5) The grip of the ALC panel 42 by the three-joint manipulator 20 is released.

[0096] As a method of operating the three-joint manipulator 20, an operator 44 directly inputs a position command value x using a controller such as a pendant switch or a joystick provided on a remote control 45. cmd or a method in which the operator 44 measures the force applied to the three-joint manipulator 20 body or the ALC panel 42 with a force sensor and generates a position command value x cmd There is a way to convert it into

[0097] In the above task (2), the ALC panel 42, which is a heavy object, is moved a relatively long distance. At this time, to ensure sufficient operability, it is necessary to prevent deterioration of the tracking performance of the position control. Furthermore, in task (3), in order to position the ALC panel 42 without any gaps, the ALC panel 42 being held must be brought into contact with the environment, such as the fixing bracket 43 or adjacent ALC panels 42. For this reason, it is necessary to switch the controller, such as controlling the position of the ALC panel 42 until it comes into contact with the environment, and then controlling the force to maintain contact after contact. The above-mentioned operations can be easily realized by using the above three-joint manipulator 20 equipped with the position and force control device 1B.

[0098] Furthermore, in order to realize this, in order to calculate the position of the ALC panel 42 or the tip of the three-joint manipulator 20 in the workspace, the actuators for driving the joints of the three-joint manipulator 20 must be equipped with position sensors such as encoders. Also, in order to measure the contact force acting on the ALC panel 42, it is desirable to equip the tip of the three-joint manipulator 20 with a force sensor, but it is also possible to apply a technology that estimates the contact force from the input and output of the actuator, such as a reaction force estimation observer.

[0099] As described above, according to the present invention, by configuring explicit force control within the position control so as to obtain a desired force command value, it is possible to control the contact force between the controlled object and the environment to a desired value or less. Therefore, even if unexpected contact with the environment occurs during position control, excessive contact force is not generated, ensuring the safety of the controlled object itself or the environment. Furthermore, since the position controller 2 and the force controller 4 are configured in series and do not interfere with each other, the control performance of either controller does not deteriorate, as would occur if the position controller 2 and the force controller 4 were configured in parallel. In particular, when there are no obstacles or other obstacles in the environment, the original position control performance is maintained.

[0100] Furthermore, because the position and force control device according to the present invention does not require environmental information that has been modeled in advance, it is effective for automating tasks in spaces where the environment changes daily, such as construction sites, and for collaborative work between humans and machines. In particular, machines that handle heavy objects, such as construction materials, are prone to position tracking errors at start-up. In order to move heavy objects without impairing operability, it is desirable to prevent deterioration of position control performance, also from the perspective of shortening construction time and improving workability. Furthermore, preventing excessive force from being generated in the controlled object or the environment is essential from the perspective of improving safety. [Explanation of symbols]

[0101] 1, 1A, 1B... Position and force control device 2...Position controller 2a...Position error calculation element 2b...Position error proportional value calculation element 2c...Speed ​​command proportional value calculation element 2d...1st speed response proportional value calculation element 2e…Superposition error calculation element 2f...Force reference value calculation element 3…Saturation compensator 3a...Saturation function element 3b…Difference calculation element 3c...Variable calculation element 3d...additive elements 4...Force controller 4a...Force error calculation element 4b...Force error proportional value calculation element 4c...Second speed response proportional value calculation element 4d...Acceleration reference value calculation element 4e...Force controller output calculation element 5...Motor 6…Environment 7...Reaction force estimation observer 11...Disturbance estimation observer

Claims

1. a position controller that calculates a force reference value to be output by the driving source so that the position response value follows the position command value, based on a comparison result between a position command value representing a target position or trajectory of a controlled object and a position response value of the controlled object due to a driving force generated by the driving source in response to the position command value; a saturation compensator that outputs a limit value obtained by limiting the force reference value to the upper limit value or lower limit value as a force request value when the controlled object is in contact with an object other than the controlled object and an allowable upper limit value or lower limit value of the contact force with which the controlled object comes into contact with the object is equal to or less than the force reference value, and outputs the force reference value as the force request value when the controlled object is in contact with the object and the force reference value does not exceed or fall below the upper limit value or lower limit value, and when the controlled object is not in contact with the object; a force controller that, when the controlled object comes into contact with the object and the limit value is input from the saturation compensator, performs force control such that the controlled object comes into contact with the object at the limit value based on a comparison result between the limit value and a force response value applied as a reaction force from the object to the controlled object when the controlled object comes into contact with the object at the limit value; when the controlled object comes into contact with the object and the force reference value is input from the saturation compensator as the force request value, performs force control such that the controlled object comes into contact with the object with a contact force equal to or less than the limit value based on a comparison result between the force reference value and a force response value applied as a reaction force from the object to the controlled object when the controlled object comes into contact with the object at the force reference value; and when the controlled object is not in contact with the object and the force reference value is input from the saturation compensator as the force request value, performs position control such that the position response value follows the position command value based on a comparison result between the position command value and the position response value. A position and force control device comprising:

2. The saturation compensator comprises: a saturation function element that limits the force reference value to the limit value when the upper limit value or the lower limit value is equal to or less than the force reference value and outputs the limit value, and that outputs the force reference value when the force reference value does not exceed or fall below the upper limit value or the lower limit value; a difference calculation element that calculates a difference between the force reference value and the output value of the saturation function element; a variable calculation element that multiplies the difference by a switching gain that is 0 when the controlled object is in contact with the object and 1 when the controlled object is not in contact with the object, and outputs the multiplication result; an addition element that adds the output value of the saturation function element and the multiplication result and outputs the addition result as the force request value; 2. The position and force control device according to claim 1, comprising:

3. the drive source is an actuator, the position controller comprises a position error calculation element which calculates a difference between the position command value and the position response value as a position error, a position error proportional value calculation element which calculates a position error proportional value by multiplying the position error by a predetermined control gain, a speed command proportional value calculation element which calculates a speed command proportional value by multiplying a speed command value indicating a target speed of the controlled object by a predetermined control gain, a first speed response proportional value calculation element which calculates a first speed response proportional value by multiplying a speed response value of the controlled object due to a driving force generated by the actuator in response to the speed command value by a predetermined first control gain, a superposition error calculation element which calculates a position speed superposition error by adding the position error proportional value to the difference between the speed command proportional value and the first speed response proportional value, and a force reference value calculation element which calculates the force reference value by multiplying the position speed superposition error by a nominal inertia value of the actuator, The force controller is composed of a force error calculation element that calculates, as a force error, the difference between the force request value output from the saturation compensator and the force response value corresponding to the force request value; a force error proportional value calculation element that multiplies the force error by a predetermined control gain to calculate a force error proportional value; a second speed response proportional value calculation element that multiplies the speed response value by a predetermined second control gain to calculate a second speed response proportional value; an acceleration reference value calculation element that calculates, as an acceleration reference value, the difference between the force error proportional value and the second speed response proportional value; and a force controller output calculation element that multiplies the acceleration reference value by a nominal inertia value of the actuator to calculate a force controller output to the actuator.

3. The position and force control device of claim 2.

Images