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 dynamics unit 7 of a virtual carriage of a load controller 1 assumes a virtual carriage traveling on a constrained locus and outputs an arc length parameter indicating a distance that the virtual carriage has traveled from a force for moving the virtual carriage in a tangential direction of the locus. A locus unit 2 for arc length parameter display outputs position coordinates of the virtual carriage, and a position control unit 3 outputs an acceleration reference value (formula 1) for synchronizing a position of a control target 10 with the position coordinates. An acceleration control unit 4 calculates torque command values (formula 2) of respective actuators of the control target 10 on the basis of the acceleration reference value (formula 1), and outputs the calculated respective torque command values (formula 2) to the control target 10.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 sought 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 that assumes a virtual cart traveling on a constrained trajectory, constrains the motion trajectory of a controlled object by synchronizing the position of the virtual cart with the position of a controlled object, and applies a load to the motion on the trajectory, a dynamics unit that outputs an arc length parameter that indicates a distance traveled by the virtual cart based on a force that moves the virtual cart in a tangential direction of the track; a trajectory unit to which the arc length parameters are input, which constrains the trajectory of the virtual cart to the trajectory of the arc length parameters, and which outputs position coordinates of the virtual cart; a position controller that receives position information of the controlled object and position coordinates of the virtual bogie and outputs an acceleration reference value for synchronizing the position of the controlled object with the position coordinates of the virtual bogie; an acceleration control unit that receives the acceleration reference values ​​of each actuator of the controlled object, calculates torque command values ​​for each actuator based on the input acceleration reference values, and outputs the calculated torque command values ​​to the controlled object.

[0011] (2) In one embodiment of the present invention, The dynamics unit calculates the arc length parameter s using Equation 6,

[0012]

number

[0013] M sv = inertia of the virtual cart F t = Force that moves the virtual cart in the tangential direction of the track (reaction force [-F t ] and the tangential unit vector of the orbit [e t ] can be calculated by the dot product.) The track section calculates the position coordinates of the virtual carriage based on a parameterized curve [x c (s),y c (s)] and output it. The position controller calculates the position [x p ,y p ] is the position coordinate of the virtual cart [x c (s),y c (s)], outputting the acceleration reference values ​​in the x and y directions shown in Equation 1,

[0014]

number

[0015] the acceleration control unit calculates a torque command value for each of the actuators shown in Equation 2 based on each of the acceleration reference values, and outputs each of the calculated torque command values ​​to the control target;

[0016]

number

[0017] The method is characterized in that the motion trajectory of the object to be controlled is constrained to a trajectory on a two-dimensional plane (x, y plane) expressed by a number line related to the arc length parameter s, and a desired load is applied.

[0018] (2) In another aspect of the present invention, The arc length parameter is calculated by equation (13):

[0019]

number

[0020] The track section calculates the position coordinates of the virtual carriage based on a parameterized curve [x c (s),y c (s),z c (s)] and output it. The position controller calculates the position [x p ,y p ,z p ] is the position coordinate of the virtual cart [x c (s),y c (s),z c (s)], output the acceleration reference values ​​in the x, y, and z directions shown in Equation 9.

[0021]

number

[0022] the acceleration control unit calculates a torque command value for each of the actuators shown in Equation 10 based on each of the acceleration reference values, and outputs each of the calculated torque command values ​​to the control target;

[0023]

number

[0024] The method is characterized in that the motion trajectory of the object to be controlled is constrained to a trajectory in three-dimensional space (x, y, z space) expressed by a number line related to the arc length parameter s, and a desired load is applied.

[0025] (4) Yet another aspect of the present invention is a force controller that directly applies a load to the controlled object; The force controller is characterized by outputting an acceleration reference value of the actuator corresponding to a desired load to the controlled object.

[0026] (5) The acceleration reference value output by the force controller is The calculation may be performed in accordance with a response value or an estimated value of a reaction force sensor of the controlled object.

[0027] (6) 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 position controller receives the position response "x, y" of each motor and calculates the position "x p ,y p It is characterized by being entered as ". [Effects of the Invention]

[0028] 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]

[0029] [Figure 1] FIG. 1 is a block diagram of a first embodiment. [Figure 2] (a) is a block diagram of the position controller, (b) is a block diagram of the dynamics part of the virtual cart, (c) is a block diagram of the acceleration control part, and (d) is a block diagram of the acceleration control part using a disturbance observer. [Figure 3] 1 is a conceptual diagram of a first embodiment. [Figure 4] (a) is a graph showing the time series data of orbital position, (b) is a graph in which the horizontal axis of (a) is changed to the arc length parameter, (c) is a graph plotting the data of (b), and (d) is a graph interpolated and resampled from (c). [Figure 5] FIG. 10 is a block diagram of a second embodiment. [Figure 6] (a) is a block diagram of the position controller, (b) is a block diagram of the Ouchi mix section of the virtual bogie, (c) is a block diagram of the acceleration control section, and (d) is a block diagram of the acceleration control section using a disturbance observer. [Figure 7] (a) is a Glock diagram of Example 3, and (b) is a block diagram of the force controller. [Figure 8] FIG. 2 is a perspective view of a fitness machine used in the simulation of Example 1. [Figure 9] FIG. [Figure 10] 10 is a diagram showing the operation of the simulation. [Figure 11] (a) is a graph showing the response position of each degree of freedom of the simulation target over time, (b) is a graph showing the response values ​​of the trajectory parameters of Example 1, and (c) is a graph showing the driving force and torque of each degree of freedom. [Figure 12] A graph showing the plot of the same simulation in the "xy plane." [Figure 13](a) is a graph showing the results of a lemniscate (polar coordinate equation) in the "xy plane," (b) is a graph showing the results of an asteroid (star shape) in the "xy plane," (c) is a graph showing the results of explicit functions such as quadratic functions in the "xy plane," and (d) is a graph showing the results of a single stroke of a star in the "xy plane." DETAILED DESCRIPTION OF THE INVENTION

[0030] 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.

[0031] 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]

[0032] A first embodiment of the load control device will be described with reference to Figures 1 to 3. In Figure 1, 1 indicates the load control device of this embodiment, and 10 indicates a controlled object. The load control device 1 is often mounted on the controlled object 10, and performs trajectory control and load control of the controlled object 10, which is configured to be freely movable, at any position.

[0033] As an example, we consider a robot with multiple degrees of freedom that can freely control the position and force of the end-effector, constraining the end-effector position to a trajectory on a two-dimensional plane and adjusting the load on the movement along the constrained trajectory.

[0034] <Configuration example> The load control device 1 is configured by a computer, and as shown in FIG. 1, it includes a track section 2 for displaying arc length parameters, a position controller 3, an acceleration controller 4, and a p " Block 5, "-e t " block section 6, and the virtual cart dynamics section (load adjustment section) 7 are implemented.

[0035] The arc length parameter s output from the dynamics unit 7 is input to the track unit 2. Assuming that a virtual cart travels on a constrained track, as shown in Fig. 3, this arc length parameter s indicates the distance traveled by the virtual cart on the track.

[0036] Specifically, the track section 2 is configured to calculate the position coordinates [x c (s),y c (s)], and the position controller 3 outputs the position coordinates [x c (s),y c This function can be defined by a formula, or it can refer to a table that converts the time series data of the constraint trajectory (position) into arc length parameters.

[0037] The position controller 3 receives the position coordinates [x c (s),y c (s)] and the actual hand position [x p ,y p ] will be entered.

[0038] Here, the robot's hand position [x p ,y p ] is the position coordinate of the virtual cart [x c (s),y c (s)] to perform position control in synchronization with the acceleration reference value (Equation 1) in the x and y directions. The calculated acceleration reference value (Equation 1) is used by the acceleration control unit 4 and the "M p " is output to block section 5.

[0039]

number

[0040] Figure 2(a) shows the case of position control using PD control. Here, the command value is expressed as the position coordinates [x c (s),y c (s)], and [d / dt] in the figure indicates the time derivative, [k p ][k d] indicate the proportional gain and the differential gain, respectively.

[0041] Acceleration control section 4 receives the acceleration reference value (Equation 1) output from position controller 3, and calculates the torque command value (Equation 2) of each actuator of control target 10 from the acceleration reference value (Equation 1) of each actuator.

[0042]

number

[0043] FIG. 2(c) shows the details of the acceleration control unit 4, where a coordinate conversion unit 4a converts the acceleration reference value (Equation 1) into an angular acceleration reference value (Equation 3) for each axis of the motor.

[0044]

number

[0045] Also, in the same figure, "J n " indicates the nominal function of each motor axis, and the torque command value (Equation 2) is calculated from the angular acceleration reference value (Equation 3) using this function. Figure 2(d) shows the case where the disturbance observer 4b is used, and the coordinate conversion unit 4c converts the hand position [x p ,y p ] is converted into the motor rotation angle [θ1, θ2] and output to the disturbance observer 4b. A general disturbance observer described in Non-Patent Document 3 can be used for this disturbance observer 4b. Here, Equation 4 shows the estimated value of the disturbance torque of each shaft of the motor.

[0046]

number

[0047] The converted torque command value (Equation 2) is output to the controlled object 10, and the robot's hand position [x p ,y p ], the trajectory constraint and load control are executed. At this time, the controlled object 10 is moved from the motor response position to the robot hand position [xp ,y p ] is calculated and output to the position controller 3 as needed, and the position control by the above-mentioned sections 3 and 4 is repeatedly executed.

[0048] "M p The block unit 5 calculates the hand position [x p ,y p ] inertia matrix "M p ", and the acceleration reference value (Equation 1) is input to calculate the gravitational force [F vs ] to "-e t " Block section 6 is output.

[0049] "-e t Block 6 is the tangential unit vector "e t " and gravitational force [F vs ] is used as input, and the force [F t ] is output to the dynamics unit 7. As shown in Equation 5, the unit vector "e t " is required.

[0050]

number

[0051] The dynamics unit 7 calculates the force [F t ] is entered. Here, the entered [F t ] to calculate the arc length parameter s. t The dynamics from [M v ]·[D v ]·[K v ] indicate the virtual mass, viscosity coefficient, and spring constant that apply the load, respectively.

[0052] The calculated arc length parameter s is output to the track section 2, and the track section 2 is set to the position coordinate [x c (s),y c (s)] is updated as needed and output to the position controller 3. As a result, the position controller 3 outputs the updated position coordinates [x c (s),y c (s)] to the hand position [x p ,y p ] is output to the acceleration control unit 4. Therefore, the position coordinate [x c (s),y c (s)] is updated, the acceleration reference value (Equation 1) is calculated, and position control and load control are performed.

[0053] <Control Principle> The control of this embodiment constrains the position of the robot's hand to a trajectory on an arbitrary two-dimensional plane (x, y plane) expressed by a number line related to the arc length parameter s, and applies a desired load.

[0054] That is, as described above, assuming the virtual cart shown in FIG. 3 traveling on the constraint trajectory, the actual hand position of the robot [x p ,y p ] is the position coordinate of the virtual cart [x c (s),y c In this case, the dynamics unit 7 can set the load that the designer wants to apply, such as a damper, on the trajectory constraint. Such control in this embodiment is realized by the following processing procedure (S01 to S03).

[0055] (1) Processing Procedure S01: First, the orbit unit 2 determines the orbit to be constrained by a parameterized curve [x c (s),y c (s)] and use the defined formula to find the arc length parameter s. c (s),y c(s)] may be defined by a mathematical formula, or a table may be referenced that converts the time series data of the position of the constraint orbit into the arc length parameter s (S01-1).

[0056] S02: Next, the dynamics unit 7 defines the dynamics of the virtual cart moving on the trajectory determined in S01. At this time, the designer may arbitrarily set the load he or she wishes to impart to the dynamics of the virtual cart. For example, in the case of a single-inertia system, Equation 6 may be used.

[0057]

number

[0058] [M in Equation 6 sv ] indicates the inertia of the virtual cart. A spring, a damper, or a combination of these may be used here. Also, [F t ] indicates the force that moves the virtual cart in the tangential direction of the track.

[0059] That is, the virtual cart is constrained to the track as shown in Figure 3, and the force that moves it [F t ] is the "reaction force -F" of "attractive force by position control" vs This is the orbital tangential component of [F t =-F vs ·e t ] and [reaction force - F vs ] and [unit vector e in the tangential direction of the orbit t ] and the inner product. c '(s),y c '(s)] like [e t ] is found by differentiating the parametric curve with respect to the arc length parameter s with respect to the arc length parameter s.

[0060] S03: The position controller 3 also calculates the position [x c (s),y c (s)] is input as a command value, and the robot's hand position [x p ,y p], the acceleration reference value (Equation 1) is calculated, and the acceleration control unit 4 and "M p " is output to block section 5. p The block unit 5 controls the attractive force [F vs At this time, the acceleration reference value (Equation 1) output from the position controller 3 is in the acceleration dimension, but the gravitational force [F vs ] is a vector value of the force dimension, so the inertia matrix of the hand position in the xy direction, M p " is converted into a force multiplied by the acceleration reference value (Equation 1).

[0061] By defining the trajectory to be constrained in this way, determining the dynamics of the virtual cart, and synchronizing the position of the virtual cart with the position of the robot's actual hand, it is possible to simultaneously achieve load adjustment and trajectory constraint.

[0062] In this case, in this embodiment, there is no need to generate a potential field that guides the robot to the hand position, as in Patent Document 1 and Non-Patent Document 2, so it is easy to reduce the load calculation for the trajectory-constrained control object and ensure the degree of freedom in load design.

[0063] (2) Details of S01-1 The method for converting time series data into data related to the arc length parameter s (S01-1) will be explained below. This method can be performed as follows:

[0064] S11: First, the graph of the time series data of the position of the constraint trajectory is re-plotted with the horizontal axis = arc length parameter s and the vertical axis = position coordinate. In this case, the arc length parameter s is calculated using Equation 7. Note that Equation 8 in Equation 7 represents the first-order derivative with respect to time.

[0065]

number

[0066]

number

[0067] S12: Interpolate (for example, linearly interpolate) between the plot samples in the graph created in S11 to make it a continuous function.

[0068] S13: To make it easier to calculate position coordinates from the arc length parameter s (to make it easier to refer to a table), the position values ​​when the arc length parameter s is changed at regular intervals are recorded (resampling). This is used as the data for the "orbit displayed as arc length parameters" in the orbit section 2. This data is used by performing interpolation such as linear interpolation.

[0069] The graphs for each of the steps (S11 to S13) will be explained with reference to Figure 4. Figure 4(a) shows a graph of the time-series data, while Figures 4(b) and (c) show graphs after S11 has been executed. Here, the graph in Figure 4(c) is not evenly spaced, as the horizontal axis is the arc length parameter (the distance the virtual cart has moved along the track). When the speed is high, the graph is coarse, and when the speed is low, the graph is dense.

[0070] Figure 4(d) shows the result of resampling the graph in Figure 4(c) at equal intervals by S12 and S13. According to this graph, the graph is displayed at equal intervals, so the position coordinates [x c (s),y c (s)] can be easily calculated. [Example]

[0071] A second embodiment of the load control device 1 will be described with reference to Figures 5 and 6. As shown in Figure 5, this embodiment is configured almost identically to the first embodiment. However, it differs in that the corresponding vectors and inertia matrices are expanded from second to third order, the position of the robot's hand is constrained on a trajectory in three-dimensional space (x, y, z space), and a desired load is applied.

[0072] That is, the track section 2 is a three-dimensional position coordinate [x c (s),y c(s), z(s)] to the position controller 3. In addition, the position controller 3 outputs the position coordinates [x c (s),y c (s),z(s)] and the robot's hand position [x p ,y p ,z p ], the acceleration reference values ​​in the x, y and z directions (Equation 9) are calculated.

[0073]

number

[0074] As shown in FIG. 6(a), the acceleration control unit 4 calculates the torque command value (Equation 10) of each actuator of the controlled object 10 from the acceleration reference value (Equation 10) of each actuator.

[0075]

number

[0076] As shown in FIG. 6(c), the coordinate conversion unit 4a calculates the angular acceleration reference value (Equation 11) of each motor shaft, and as shown in FIG. 6(d), the coordinate conversion unit 4c calculates the coordinate [x p ,y p ,z p ] is converted into the motor rotation angle [θ1, θ2, θ3] and output to the disturbance observer 4b.

[0077] Also, "-e t Block 6 is a unit vector [e t ] is calculated, and the dynamics unit 7 in FIG. 6(b) calculates the arc length parameter s using Equation 13.

[0078]

number

[0079]

number

[0080]

number

[0081] According to this embodiment, even when the robot's hand position moves in three dimensions, it is possible to apply a desired load while constraining the robot's hand position on a certain trajectory, as in the first embodiment.

[0082] At this time, "-e t In block 6, the gravitational force [F vs ] to move the virtual cart in the tangential direction of the track [F t ] is calculated. This is a calculation of an inner product, which converts multidimensional vectors into scalar values, so it can be calculated in the same way in two dimensions as in the first embodiment. Furthermore, since the track section 2 first defines the position coordinates of the virtual bogie, there is no major problem in expanding from two dimensions to three dimensions.

[0083] As a result, even when extending the motion on a two-dimensional plane (Example 1) to three-dimensional motion, only the dimensions of the vector variables are extended, and the rest can be implemented within the same framework. Therefore, according to this example, it is possible to easily implement control that constrains the position of the robot's hand on a trajectory in three-dimensional space (x, y, z space) and applies a desired load. [Example]

[0084] A third embodiment of the load control device 1 will be described with reference to Fig. 7. In this embodiment, as shown in Fig. 7(a), a force controller 8 is provided in parallel with the position controller 3. This force controller 8 controls the force (load) applied directly to the end effector position of the robot. This force controller 8 may be configured to input a constant force without feedback, or to feed back a sensor response or estimated value of the reaction force.

[0085] FIG. 7(b) shows the details of the force controller 8, and [F cmd] indicates the command value in two or three dimensions, and [F res ] is the command value [F cmd ], and the response value or estimate of the reaction force is the same dimension as [C f ] indicates the force control gain, and when it is difficult to obtain the response value or estimated value of the reaction force, [F res =0] may be assumed.

[0086] In Examples 1 and 2, the load force generated by the dynamics unit 7 is applied to the robot's hand through the position controller 3. With this structure, the bandwidth of the load force is limited by the bandwidth of the position controller 3, making it difficult to present a sharp force sensation over a wide bandwidth.

[0087] Therefore, in this embodiment, as shown in FIG. 7(a), the force controller 8 is arranged in parallel with the position controller 3, and the output of the force controller 8 is directly applied to the tip of the robot. The acceleration reference value vector (Equation 14), which is the output of the force controller 8, is expressed as [F res ] (sensor response value / estimated value of reaction force) can be obtained, it is calculated by Equation 15. Note that [C f ] is a matrix of force gains.

[0088]

number

[0089]

number

[0090] On the other hand, [F res If it is difficult to obtain [F res =0] and can be approximated by Equation 16.

[0091]

number

[0092] <Simulation example> As described above, according to the first embodiment, it is possible to simultaneously realize constraint control to the trajectory of the parameter display on a two-dimensional plane and load control on the trajectory. According to the second embodiment, it is also possible to handle the trajectory of the parameter display in three-dimensional space. According to the third embodiment, it is also possible to present a sharp force sensation in a wide frequency range. Below, a simulation example in which the first embodiment is applied to the fitness machine of Figs. 8 to 10 will be described.

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

[0094] Here, an application example will be described in which the mechanism unit 21 in Fig. 9 is a robot that is the control target 10, and the operation unit 36 ​​is the hand position. In Fig. 9, x indicates rotation around the X axis, y indicates linear motion along the Y axis, and z indicates rotation around the Z axis.

[0095] 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.

[0096] 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.

[0097] 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.

[0098] 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.

[0099] 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.

[0100] (2) Simulation method In this simulation, the degree of freedom of the linear motion mechanism 34 driven by motor 37 in Figure 9 (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."

[0101] 10, the angle of "rotary1" was set to "90°" and the linear motion mechanism 34 was fixed vertically by position control. In addition, the control of Example 1 was verified on the "two-dimensional xy plane" composed of "linear" and "rotary2."

[0102] The dynamics section 7 is a single-inertia system. t = constant" and the constrained trajectory is a spiral trajectory on a two-dimensional plane. In this case, the position response [x, y] of the motors 38 and 39 in Figure 10 is the same as the hand position [x p ,y p ] is assumed to correspond to

[0103] (3) Simulation results First, the time series results of the simulation will be explained based on Fig. 11. Fig. 11(a) shows the position response of each degree of freedom, Fig. 11(b) shows the response value of the arc length parameter s, and Fig. 11(c) shows the driving force and torque of each degree of freedom. Each response transitioned smoothly up to the end point of the spiral, and reasonable changes were obtained.

[0104] Next, Figure 12 shows the plot in the "xy plane," where the actual trajectory of the simulation almost overlaps with the desired trajectory, confirming the achievement of the trajectory constraint.

[0105] It is also possible to handle various other orbits expressed as parameters [x(t), y(t)]. For example, Figure 13 shows other results on the "xy plane," where (a) shows the result of a lemniscate (polar coordinate equation), (b) shows the result of an asteroid (star shape), (c) shows the result of explicit functions such as quadratic functions, and (d) shows the result of a star drawn in one stroke. [Explanation of symbols]

[0106] 1...Load control device 2...Arc length parameterized orbital section 3...Position controller 4...Acceleration control section 4a, 4c... Coordinate conversion section 4b...Disturbance observer 5… “M p " Block section 6... "-e t " Block section 7...Virtual cart dynamics section 8...Force controller 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. A device that constrains the motion trajectory of a controlled object by imagining a virtual cart traveling on a constrained trajectory and synchronizing the position of the virtual cart with the position of a controlled object, and applies a load to the motion on the trajectory, a dynamics unit that outputs an arc length parameter that indicates a distance traveled by the virtual cart based on a force that moves the virtual cart in a tangential direction of the track; a trajectory unit to which the arc length parameters are input, which constrains the trajectory of the virtual cart to the trajectory of the arc length parameters, and which outputs position coordinates of the virtual cart; a position controller that receives position information of the controlled object and position coordinates of the virtual bogie and outputs an acceleration reference value that synchronizes the position of the controlled object with the position coordinates of the virtual bogie; an acceleration control unit that receives the acceleration reference values ​​of the actuators of the control target, calculates torque command values ​​for the actuators based on the received acceleration reference values, and outputs the calculated torque command values ​​to the control target; A load control device comprising:

2. The dynamics section includes: The arc length parameter s is calculated using Equation 6. [Equation 6] M sｖ = inertia of the virtual cart F t = Force that moves the virtual cart in the tangential direction of the track (reaction force [-F t ] and the unit vector tangential to the orbit [e t ] can be calculated by the dot product.) The track section calculates the position coordinates of the virtual carriage based on a parameterized curve [x c (s), y c (s)] and output it. The position controller calculates the position [x p ,y p ] is the position coordinate [x c (s), y c (s)], outputting the acceleration reference values ​​in the x and y directions shown in Equation 1, [Equation 1] the acceleration control unit calculates torque command values ​​for the actuators shown in Equation 2 based on the acceleration reference values, and outputs the calculated torque command values ​​to the control targets; [Equation 2] The motion trajectory of the object to be controlled is constrained to a trajectory on a two-dimensional plane (x, y plane) expressed by a number line related to the arc length parameter s, and a desired load is applied.

2. The load control device according to claim 1.

3. The arc length parameter is calculated by equation (13): [0013] The track section calculates the position coordinates of the virtual carriage based on a parameterized curve [x c (s), y c (s), z c (s)] and output it. The position controller calculates the position [x p ,y p , z p ] is the position coordinate [x c (s), y c (s), z c (s)], output acceleration reference values ​​in the x, y and z directions shown in Equation 9, [Equation 9] the acceleration control unit calculates torque command values ​​for the actuators shown in Equation 10 based on the acceleration reference values, and outputs the calculated torque command values ​​to the control target; [Equation 10] The motion trajectory of the object to be controlled is constrained to a trajectory in three-dimensional space (x, y, z space) expressed by a number line related to the arc length parameter s, and a desired load is applied.

2. The load control device according to claim 1.

4. a force controller that directly applies a load to the controlled object; The force controller outputs an acceleration reference value of the actuator corresponding to a desired load to the controlled object.

2. The load control device according to claim 1.

5. The acceleration reference value output by the force controller is 5. The load control device according to claim 4, wherein the load control signal is calculated in accordance with a response value or an estimated value of a reaction force sensor of the controlled object.

6. 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 position controller receives the position response "x, y" of each of the electric motors and calculates the position "x ｐ ,y ｐ will be entered as 3. The load control device according to claim 2.

Images