Load control device

Abstract

To reduce load calculation for a control target subjected to locus constraint and ensure the degree of freedom of load design.SOLUTION: A mode inverse transformation (Q2a-1) unit 2 calculates modes for a tangent line and a normal line by using a mode transformation matrix for a motion locus of a control target from position responses of actuators. A load controller 3 calculates an acceleration reference value for controlling a load to be applied to the motion of the control target along the tangential line on the basis of a command value of the mode for the tangential line. A position controller 4 calculates an acceleration reference value for constraining the position control of the control target on a linear locus on the basis of a command value of the mode for the normal line. A mode inverse transformation (Q2a-1) unit 5 performs inverse transformation of the mode from both acceleration reference values to the acceleration reference values of the respective actuators. An acceleration controller 6 calculates torque commands for the respective actuators from the acceleration reference values of the respective actuators.SELECTED DRAWING: Figure 1

Description

[Technical Field]

[0001] The present invention relates to a device for controlling a trajectory on which a controlled object moves and for controlling a load on the trajectory. [Background technology]

[0002] Non-Patent Document 1 formulates an optimization problem in which "a constraining trajectory and a torque limit are used as constraint conditions, and a control input is obtained to move toward the constraining trajectory in the shortest time," and calculates a solution.

[0003] Patent Document 1 and Non-Patent Document 2 generate a potential field that guides the position of the robot's hand along a path. Generating such a potential field requires superposition and weight adjustment of many potential fields whose elements are kernel Gaussian functions. Potential designs include making the slope of the valley cross section steeper to strengthen the constraint on the path, and increasing the drop in the path at the valley bottom to increase the propulsion force along the path. [Prior art documents] [Patent documents]

[0004] [Patent Document 1] US11554485B2 [Non-patent literature]

[0005] [Non-Patent Document 1] “An Efficient Robot Arm Control Under Geometric Path Constraints” [Non-patent document 2] “Learning Potential Functions from Human Demonstrations with Encapsulated Dynamic and Compliant Behaviors” [Non-patent document 3] Kiyoshi Ohishi, Yuzuru Ohba, Seiichiro Katsura “Kinematics and Dynamics of Motion Control Based on Acceleration Control” IEEJ Trans.IA,Vol.127,No6.2007 Summary of the Invention [Problem to be solved by the invention]

[0006] However, although the technology of Non-Patent Document 1 can constrain the trajectory of the controlled object, it does not anticipate adjusting the load on the movement on the trajectory.

[0007] Therefore, the techniques of Non-Patent Document 2 and Patent Document 1 have been proposed, but the more complex the potential field, the more overlapping and weight adjustment of element functions becomes necessary, which may increase the calculation load.

[0008] Furthermore, since the load on the orbit is applied through the conservative force of the potential field, it is not possible to apply load changes over time, which may limit the degree of freedom in load design.

[0009] The present invention has been made to solve the problems of the prior art, and aims to reduce the load calculation for a trajectory-constrained controlled object and ensure the degree of freedom in load design. [Means for solving the problem]

[0010] (1) The present invention is a device for controlling an actuator that controls a trajectory of a controlled object and applies a load to the trajectory, a mode conversion unit that calculates tangential and normal modes for a motion trajectory of a controlled object from the position response of the actuator using a mode conversion matrix; a load controller that calculates an acceleration reference value that controls a load applied to the operation of the controlled object along the tangent line based on the command value of the tangent mode; a position controller that calculates an acceleration reference value that constrains the position control of the controlled object to a linear trajectory based on the command value of the normal mode; a mode inverse conversion unit that performs an inverse conversion of a mode from the two acceleration reference values ​​to an acceleration reference value of each of the actuators; an acceleration controller that calculates a torque command for each of the actuators from an acceleration reference value of each of the actuators; Equipped with controlling the load on the constraint trajectory in the tangential direction, while controlling the constraint of the trajectory in the normal direction; The control of the tangential direction and the control of the normal direction are each controlled independently.

[0011] (2) In one embodiment of the present invention, The mode conversion unit is Using the mode transformation matrix, the tangential and normal modes [x c ,x d ] while calculating The position controller This method is characterized by constraining the movement of the controlled object to a trajectory on a straight line expressed as "y = ax" by executing position control that sets the command value (Equation 3) of the normal mode to zero.

[0012]

number

[0013] (3) In another aspect of the present invention, The mode conversion unit is Using the mode transformation matrix, the tangential and normal modes [x c ,x d ] while calculating The position controller includes: This is characterized by the fact that the trajectory of the movement of the controlled object is constrained to a straight line expressed by "y=ax+b" by executing position control of the command value shown in Equation 15.

[0014]

number

[0015] (4) In yet another embodiment of the present invention, The position controller The system is characterized in that the motion of the controlled object is constrained to a nonlinear curved trajectory expressed by "y = f(x)" by sequentially updating the parameters "a, b" of the straight line.

[0016] (5) In yet another embodiment of the present invention, The control object is The actuator includes: An electric motor for rotational motion on the x-axis, an electric motor for linear motion in the y-axis; Equipped with The mode conversion unit converts the position response [x, y] of each of the electric motors into the position response [x1, x2] of the actuator, The present invention is characterized in that the motion of each of the above exercises is constrained and a load is applied. [Effects of the Invention]

[0017] According to the present invention, it is possible to reduce the load calculation for a controlled object that is trajectory constrained and ensure the degree of freedom in load design. [Brief explanation of the drawings]

[0018] [Figure 1] FIG. 1 is a block diagram of a first embodiment. [Figure 2] (a) is a block diagram of the load controller, (b) is a block diagram of the position controller, (c) is a block diagram of the acceleration controller, and (d) is a block diagram of the acceleration controller using a disturbance observer. [Figure 3] Graph showing an overview of Example 3. [Figure 4] FIG. 1 is a perspective view of a fitness machine used in a simulation. [Figure 5]Configuration diagram of the simulation target. [Figure 6] Simulation operation diagram of Figure 5. [Figure 7] (a) is a graph showing the response position of each degree of freedom of the simulation target, (b) is a graph showing the command value and response value of the normal mode “xg” in Example 1, and (c) is a graph showing the driving force and torque of each degree of freedom of the same. [Figure 8] FIG. 1 is a plot diagram of Example 1 on the xy plane. [Figure 9] 1A is a plot diagram of Example 2 on the xy plane, and FIG. 1B is a plot diagram of Example 3 on the xy plane. DETAILED DESCRIPTION OF THE INVENTION

[0019] A load control device according to an embodiment of the present invention will now be described. This load control device relates to motion control of a robot to be controlled. Specifically, the device constrains the motion of a specific position of the robot on a trajectory and controls the load applied to the motion on the trajectory.

[0020] This allows an appropriate load to be applied to the robot's operator, making it possible to implement motion constraints appropriate for fitness training movements or games when used as a game controller, for example. [Example]

[0021] A load control device according to a first embodiment will be described with reference to Figures 1 and 2. In Figure 1, reference numeral 1 denotes the load control device of this embodiment, and 10 denotes the object to be controlled.

[0022] The control object is assumed to be a multi-degree-of-freedom robot capable of controlling the position and force of the hand, and the load control device 1 performs trajectory constraints on a straight line (y=ax) and load control for motion on a two-dimensional plane. Specifically, the load control device 1 is configured by a computer and performs mode conversion (Q 2a ) unit 2, load controller 3, position controller 4, mode inverse conversion unit 5, and acceleration control unit 6 are implemented.

[0023] Mode Conversion (Q2a ) section 2 calculates the tangential and normal modes [x c ,x d ] is calculated. For this calculation, the mode transformation matrix of Equation 1 is used. The calculated tangential mode [x c ] is output to the load controller 3, and the normal mode [x d ] is output to the position controller 4.

[0024]

number

[0025] The load controller 3 receives a command value for the tangential mode shown in Equation 2. The command value of Equation 2 is determined in advance, and the mode conversion (Q 2a ) Part 2 tangent mode [x c ] is output to the load controller 3 at the same time.

[0026]

number

[0027] Based on the input command value, the acceleration reference value (Equation 4) for the tangential mode is calculated and output. This acceleration reference value (Equation 4) is used to control the load applied to the actuator of the controlled object 10 along the tangent line, and the load controller 3 applies the desired load, such as a viscous load or a constant load.

[0028]

number

[0029] Figure 2(a) shows the block diagram for the case of viscous load, where "d / dt" indicates the time derivative, and "D v " indicates the virtual viscosity coefficient that acts as a load, and the acceleration reference value of Equation 4 is calculated as Equation 5.

[0030]

number

[0031] The position controller 4 receives a command value for the normal mode shown in Equation 3. The command value for Equation 3 is also determined in advance, and the mode conversion (Q 2a ) Normal mode [x d ] is output to the position controller 4 at the same time.

[0032]

number

[0033] Here, the acceleration reference value of Equation 6 is calculated and output based on the input command value. The acceleration reference value (Equation 6) is used to constrain the position control of the controlled object 10 on a straight trajectory.

[0034]

number

[0035] Here, by setting the normal mode command value (Equation 3) to zero and configuring position control, The position control is constrained to a linear trajectory of "y = ax". Figure 2(b) shows a block diagram when the position control is PD control (control that combines proportional action and differential action), and p ", "K d " indicates the proportional gain and the differential gain, respectively, and the acceleration reference value of Equation 6 is calculated as Equation 7.

[0036]

number

[0037] Mode inversion (Q 2a -1 The acceleration reference values ​​of Equation 4 and Equation 6 are input to unit 5. Here, both acceleration reference values ​​are expressed as Equation 8.

[0038]

number

[0039] Then, an inverse conversion of the mode is performed from the acceleration reference value of Equation 8 to the acceleration reference value of each actuator (Equation 9). Equation 10 is used for this inverse conversion.

[0040]

number

[0041]

number

[0042] The acceleration reference value of equation 9 is input to the acceleration control unit 6. Here, the torque command value (equation 11) of each actuator is calculated from the acceleration reference value of equation 9.

[0043]

number

[0044] FIG. 2(c) shows an example of the configuration of the acceleration control unit 6, in which the coordinate conversion unit 6a converts the acceleration reference value of Equation 9 into the angular acceleration reference value (Equation 12) of each axis of the motor. n " indicates the nominal inertia of each motor shaft.

[0045]

number

[0046] 2(d) shows the case where a disturbance observer is used. Here, "θ1, θ2" indicate the rotation angles of the motor after coordinate transformation, and a general disturbance observer described in Non-Patent Document 3 can be used as the disturbance observer 6b, and Equation 13 indicates the estimated value of the disturbance torque of each shaft of the motor.

[0047]

number

[0048] The acceleration control unit 6 outputs the torque command value of the equation 11 to the controlled object 10. Here, the torque command value input to the controlled object 10 is expressed as the torque input of the equation 14.

[0049]

number

[0050] The controlled object 10 outputs the position response (x1, x2) of the actuator from the torque input of Equation 14. The position response (x1, x2) of the actuator output here is converted into a mode (Q 2a ) unit 2, and the control of the controlled object 10 is repeated by the units 3 to 6.

[0051] According to the load control device 1 of this embodiment, mode conversion (Q 2a ) section and mode inversion (Q 2a -1 ) unit 5 can divide the hand motion of the controlled object into the tangential direction and the normal direction by mode conversion. In this case, it is possible to control the application of load on the motor's constrained trajectory in the tangential direction, and the trajectory constraint by the motor in the normal direction, and to control each independently.

[0052] In particular, the desired load is applied in the tangential direction by the load controller, and the normal direction can be fixed on the desired trajectory by incorporating position control that sets the normal mode to zero, ensuring freedom in load design. Also, there is no need for function superposition or weight adjustment as in Non-Patent Document 2 and Patent Document 1, which also makes it possible to reduce the computational load. [Example]

[0053] This embodiment is configured in substantially the same manner as embodiment 1. However, it differs in that the command value input to position controller 4 is not zero but a constant value. Specifically, the command value of equation 3 is rewritten as equation 15, and the command value of equation 15 is input to position controller 4 in FIG. 2(a).

[0054]

number

[0055] In the first embodiment, constraint to a trajectory of a straight line (y = ax) with a y-intercept of zero was assumed. In contrast, in this embodiment, the command value of the normal mode is set to a constant value instead of zero, so that a straight line (y = ax + b) with a non-zero y-intercept is also included. This makes it possible to simultaneously achieve trajectory constraint and load adjustment even for a straight line trajectory that does not pass through the origin. Equation 16 is used to derive the command value for constraint to a straight line with a constant y-intercept.

[0056]

number

[0057] Here, to be on the straight line "y=ax+b", it is sufficient to have "ax-y=-b". This point can be compared with equation 16 and used as the command value for equation 15. [Example]

[0058] In this embodiment, based on the second embodiment, the parameters a and b of the constraining straight line are updated each time as follows.

[0059] (1) First, the actuator position coordinate (x t ,y t ) and the point on the curve where the distance between the function curve and the point (x s ,f(x s )) is calculated by using the point (x s ,f(x s )) and the tangent to the point (x t ,y t) and calculate the distance to the point (x s ) is the solution.

[0060] (2) Next, the point (x s ,f(x s )) to find the equation of the tangent on the equation (17). The 1st and 0th order coefficients are substituted into the parameters a and b, respectively, and the control algorithm of the second embodiment is executed. Here, the parameters a and b are expressed by the equation (17).

[0061]

number

[0062] In the second embodiment, we dealt with constraints on a general straight line trajectory expressed by "y = ax." In contrast, in the third embodiment, we also deal with constraints on a nonlinear curve expressed by "y = f(x)." Here, we sequentially calculate the tangent to "y = f(x)" and perform control to constrain to that tangent, thereby realizing the trajectory constraint of "y = f(x)." This makes it possible to simultaneously achieve heartbeat constraint and load adjustment even for a curved trajectory expressed by "y = f(x)."

[0063] In this embodiment, (1) the position coordinates of the actuator (x t ,y t ) to the point on the curve (x s ,f(x s )), and (2) by constraining the tangent at that point and controlling the load in the tangential direction, constraint on the trajectory of the nonlinear function curve and load control are realized.

[0064] <Simulation example> As described above, according to the first embodiment, it is possible to simultaneously realize constraint control to a straight trajectory that passes through the origin and load control on the trajectory. Also, according to the second embodiment, it is possible to simultaneously realize constraint control to a straight trajectory that does not pass through the origin and load control on the trajectory. Furthermore, according to the third embodiment, it is possible to adopt a nonlinear curve as a constraint trajectory. Below, a simulation example applied to the fitness machine of Figs. 4 to 6 will be described.

[0065] (1) Example of fitness machine configuration As shown in FIG. 4, the fitness machine 20 includes a chair 23 on which a user (trainee) sits, left and right operating units (grips) 36 that the user grasps and operates, and mechanical units 21 and 22 that are movable along three axes (X-axis, Y-axis, and Z-axis). These mechanical units 21 and 22 are arranged on the left and right sides of the chair 23.

[0066] Here, an application example will be described in which the mechanical unit 21 in Fig. 5 is used as the controlled object 10. In Fig. 5, x indicates rotation around the X axis, y indicates linear motion along the Y axis, and z indicates rotation around the Z axis.

[0067] The mechanism 21 includes a linear motion mechanism 34 that linearly moves the grip 36 in the y direction, and a rotation mechanism that rotates the linear motion mechanism 34 in the x and y directions, each of which is equipped with motors 38 to 39. These motors 37 to 39 correspond to the actuators of the controlled object 10 in this simulation.

[0068] Specifically, it comprises a vertically long box-shaped housing 31 erected on a rectangular base, a bracket 30 rotatably supported on the housing 31, and a linear motion mechanism 34 rotatably supported on the bracket 30.

[0069] A motor 39 is housed in the upper part of the housing 31. The bracket 30 is supported on the shaft of this motor 39 so as to be rotatable in the x direction. This forms a rotation mechanism that rotates the linear motion mechanism 34 along the x axis, and the load of the motor 39 is applied to the rotation of the bracket 30 in the same direction.

[0070] A motor 38 is fixed to the upper end of the bracket 30. The rear end of the arm portion 34a of the linear motion mechanism 34 is journaled to the shaft of this motor 38 so as to be rotatable in the y direction. This forms a rotation mechanism that rotates the linear motion mechanism 34 in the y direction, and the load of the motor 38 is applied to the rotation of the arm portion 34a in the same direction.

[0071] The linear motion mechanism 34 includes a motor 37 attached to the rear end of the arm portion 34a, a slider (movable element) 35 to which a grip 36 is fixed, and a ball screw mechanism (not shown) that applies a load from the motor 37 to the linear motion of the slider 35 along the z direction.

[0072] (2) Simulation method In this simulation, the degree of freedom of the linear motion mechanism 34 driven by motor 37 in Figure 5 (linear motion degree of freedom in the y direction) was defined as "linear," the rotational degree of freedom in the z direction driven by motor 38 was defined as "rotary1," and the rotational degree of freedom in the x direction driven by motor 39 was defined as "rotary2."

[0073] As shown in Figure 6, the angle of "rotary1" was set to "90°" and the linear motion mechanism 34 was fixed vertically by position control. In addition, control verification of each embodiment was performed on the "two-dimensional xy plane" consisting of "linear" and "rotary2." At this time, the load controller 3 applied a constant force load, and the position controller 4, which constrains the trajectory, implemented position control so that "y = ax." In this case, (x, y) in Figure 6 corresponds to the position response (x1, x2) of the actuator in Figure 1.

[0074] (3) Simulation results First, the time-series results of the simulation of Example 1 will be described with reference to Figure 7. The results in Figure 7(a) show the position response of each degree of freedom. Here, "rotary1" is fixed at "90° (i.e., π / 2)," and it was confirmed that the degrees of freedom of "linear" and "rotary2," which are controlled in Example 1, change in a parabolic fashion. This is reasonable because, in theory, when a constant load is applied, the position response is parabolic with uniform acceleration motion.

[0075] The result of Figure 7(b) is the normal mode "x d " command value (Xdcmd: Equation 3) and its response value (xd: x d) is shown. Here, the error is almost zero in the plot of the range of "±1 mm", and the control target is fully achieved. The results in Figure 7(c) show the driving force and torque for each degree of freedom. All of them remain almost constant. Here, the value for "linear" is slightly large because a driving force is required to offset gravity.

[0076] Next, Figure 8 shows a plot on the "xy plane" of Example 1, where the horizontal axis "th2" represents the x-axis and the vertical axis "position" represents the y-axis. Here, the actual trajectory of the simulation almost overlaps with the desired trajectory, confirming the achievement of trajectory constraint.

[0077] Figure 9(a) shows a plot on the "xy plane" for Example 2, and Figure 9(b) shows the same plot for Example 3. In each figure, the horizontal axis "th2" indicates the x-axis, and the vertical axis "position" indicates the y-axis, and it was confirmed that the trajectory was constrained to a straight line with a y-intercept and to a nonlinear function curve (a quadratic function in this simulation). [Explanation of symbols]

[0078] 1...Load control device 2...Mode conversion (Q 2a ) part 3...Load controller 4...Position controller 5...Mode inverse conversion (Q 2a -1 ) part 6...Acceleration control section 6a... Coordinate conversion section 6b...Disturbance observer 10...Control object 20...Fitness machine 21,22... Mechanism section 23...Chair 30…Bracket 31...Case 34...Linear motion mechanism 35...Slider 36...Grip 37~39...Motor

Claims

1. An apparatus for controlling an actuator that controls a trajectory of a controlled object and applies a load to the trajectory, a mode conversion unit that calculates tangential and normal modes for a motion trajectory of a controlled object from the position response of the actuator using a mode conversion matrix; a load controller that calculates an acceleration reference value that controls a load applied to the operation of the controlled object along the tangent line based on the command value of the tangent mode; a position controller that calculates an acceleration reference value that constrains the position control of the controlled object to a linear trajectory based on the command value of the normal mode; a mode inverse conversion unit that performs an inverse conversion of a mode from the two acceleration reference values ​​to an acceleration reference value of each of the actuators; an acceleration controller that calculates a torque command for each of the actuators from an acceleration reference value of each of the actuators; Equipped with controlling the load on the constraint trajectory in the tangential direction, while controlling the constraint of the trajectory in the normal direction; A load control device characterized in that the control of the tangential direction and the control of the normal direction are performed independently of each other.

2. The mode conversion unit is Using the modal transformation matrix, the actuator position response [x １ ,x ２ ] to the tangential / normal mode [x c ,x d ] while calculating The position controller By performing position control that sets the command value (Equation 3) in the normal mode to zero, the movement of the controlled object is constrained to a trajectory on a straight line expressed by "y = ax".

2. The load control device according to claim 1. [Equation 3]

3. The mode conversion unit is Using the modal transformation matrix, the actuator position response [x １ ,x ２ ] to the tangential / normal mode [x c ,x d ] while calculating The position controller includes: By executing the position control of the command value shown in Equation 15, the movement of the controlled object is constrained to a trajectory on a straight line expressed by "y = ax + b" 2. The load control device according to claim 1. [Equation 15]

4. The position controller By sequentially updating the parameters "a, b" of the straight line, the movement of the controlled object is constrained to a nonlinear curved trajectory expressed by "y = f(x)".

4. The load control device according to claim 3.

5. The control object is The actuator includes: an electric motor for rotary motion about the x-axis; an electric motor for linear motion in the y-axis; Equipped with The mode conversion unit converts the position response [x, y] of each of the electric motors into the position response [x １ ,x ２ ] as 5. The load control device according to claim 2, wherein the load is applied by restricting the movement of each of the exercises.

Images