Track restraint device and track restraint method

Abstract

When constraining the trajectory of a controlled object on a figure (two-dimensional figure or three-dimensional space figure) defined by "SDF," mechanical characteristics such as elasticity and viscosity are imparted. The trajectory constraint device 1 converts the rotational angle position of the robot arm 10 into hand position information by inputting a signed distance command value, and calculates the signed distance from the constraint trajectory from that position information. When controlling the signed distance, the acceleration reference value of equation (3) is calculated using the position gain and velocity gain, which indicate virtual elasticity and viscosity. The calculation result of the unit vector applied to the partial derivative of the signed distance "XSD" is multiplied by the acceleration reference value to convert it into the acceleration reference value of equation (4). This acceleration reference value is converted into the acceleration reference value of equation (12), which rotates each axis of the robot arm 10. This acceleration reference value is multiplied by the inertia in the rotational angle direction of each axis to obtain the torque reference value for each axis, and the torque of each axis is controlled to constrain the trajectory of the robot arm 10.

Description

[Technical Field]

[0001] The present invention relates to a technique for constraining the movement of a controlled object to a specific trajectory. [Background technology]

[0002] "SDF (Signed Distance Function)" is often used in fields such as CG. In Patent Document 1, virtual objects are visually presented using "SDF" to provide an immersive experience using XR (Extended Reality). Compared to modeling with meshes, virtual objects can be visually presented using a simplified program.

[0003] Non-Patent Document 1 proposes a robot for use as a surgical simulator, and presents force sensations using "SDF" and "exponential" functions, as shown in equations (1) and (2) in the non-patent document. [Prior art documents] [Patent documents]

[0004] [Patent Document 1] Special Table 2023-504570 [Non-patent literature]

[0005] [Non-Patent Document 1] “Haptic-Assisted Collaborative Robot Framework for Improved Situational Awareness in Skull Base Surgery” Summary of the Invention [Problem to be solved by the invention]

[0006] As mentioned above, Patent Document 1 uses "SDF" to visually present virtual objects, which allows for implementation with simplified programming compared to modeling with meshes, but does not go so far as to present the force sensation of the virtual objects.

[0007] Similarly, Non-Patent Document 2 presents a force sensation using "SDF", but it is not possible to directly give the mechanical properties of virtual objects, such as elasticity and viscosity.

[0008] This invention has been made to solve these conventional problems, and its objective is to constrain the trajectory of the controlled object on a figure defined by "SDF" (a figure on a two-dimensional plane or a figure in three-dimensional space) and give it mechanical properties such as elasticity and viscosity. [Means for solving the problem]

[0009] (1) One aspect of the present invention is A device that restricts the movement of a controlled object to a trajectory determined from at least two axes, at least a rotation axis and a horizontal axis, an SDF block section which receives input of information converted from the operating position of the actuator of the controlled object to position information on a plane or space of a specific part that constrains the trajectory of the controlled object according to a command value of the signed distance "XSD", and calculates the signed distance "XSD" between the constrained trajectory and the converted position information; a first acceleration reference value calculation unit that calculates an acceleration reference value (absolute value) using a position gain and a velocity gain that indicate virtual elasticity and viscosity when controlling the signed distance "XSD"; a second acceleration reference value calculation unit that converts the acceleration reference value (absolute value) into an acceleration reference value for the workspace by multiplying the calculation result of the direction (unit vector) to be applied to the partial differential of the signed distance "XSD" by the acceleration reference value (absolute value); a third acceleration reference value calculation unit that converts the acceleration reference value for the workspace into an acceleration reference value for each of the axes (joint space) that moves each of the axes of the control object; a torque reference value calculation unit that calculates a torque reference value for each axis of the actuator to be controlled by multiplying the acceleration reference value in the joint space by the inertia in the direction of the movement; Equipped with The torque of each axis of the actuator is controlled based on the torque reference value, thereby constraining the trajectory of the controlled object.

[0010] (2) Another aspect of the present invention is A method in which a computer constrains the motion of a controlled object to a trajectory determined from at least two or more axial directions, at least a rotational axis and a horizontal axis, a step of converting information into position information on a plane or space of a specific part that constrains the trajectory of the controlled object from the operating position of the actuator of the controlled object according to a command value of the signed distance "XSD", and calculating the signed distance "XSD" from the constrained trajectory from the converted position information; When controlling the signed distance "XSD", a step of calculating an acceleration reference value (absolute value) using a position gain and a velocity gain that indicate virtual elasticity and viscosity; a step of converting the calculated direction (unit vector) of the partial differential of the signed distance "XSD" into an acceleration reference value for the workspace by multiplying the acceleration reference value (absolute value) by the calculated direction (unit vector) of the partial differential of the signed distance "XSD"; converting the acceleration reference value for the workspace into an acceleration reference value for each of the axes (joint space) that moves each of the axes of the control object; calculating a torque reference value for each axis of the actuator to be controlled by multiplying the joint space acceleration reference value by the inertia in the direction of the movement; constraining the trajectory of the controlled object by controlling the torque of each axis of the actuator based on the torque reference value; It is characterized by having: [Effects of the Invention]

[0011] According to the present invention, the controlled object can be constrained to a trajectory on a figure defined by "SDF" (a figure on a two-dimensional plane or a figure in three-dimensional space), and mechanical properties such as elasticity and viscosity can be given. [Brief explanation of the drawings]

[0012] [Figure 1] FIG. 2 is a diagram showing the relationship between the rotation axis and the tip position of the robot arm in the first embodiment. [Figure 2] A diagram showing the relationship between the robot arm's hand position and constraint trajectory on the yz plane. [Figure 3] Control block diagram of the same. [Figure 4] FIG. 10 is a control block diagram of a second embodiment. [Figure 5] FIG. 10 is a control block diagram of a fourth embodiment. [Figure 6] FIG. 11 is a perspective view of a fitness machine used in the simulation of Example 3. [Figure 7] The same is a configuration diagram of the simulation target. [Figure 8] 10(a) is a graph of a time series response showing the simulation results of Example 3, and FIG. 10(b) is a trajectory diagram on a two-dimensional plane of the same. DETAILED DESCRIPTION OF THE INVENTION

[0013] The following describes a trajectory constraint device (method) according to an embodiment of the present invention. This trajectory constraint device (method) constrains a specific position of a robot arm to be controlled (here, the hand position is used as an example) to a specific trajectory, and imparts mechanical properties such as elasticity and viscosity to the operator (user) to move within an appropriate trajectory.

[0014] In this case, by virtually controlling mechanical properties such as elasticity and viscosity inside and outside any boundary, it is possible to limit the range of motion of the robot arm's hand position and present the force sensation of a virtual object.

[0015] The main applications of the trajectory constraint device are expected to be motion teaching, a joystick (with a limited range of motion) for a game controller, force feedback in XR, etc. Details will be explained below based on Examples 1 to 4. [Example]

[0016] <Basic Concept> The trajectory constraint device of this embodiment calculates the absolute value of the reference value using SD control (PD control) of the signed distance, calculates the direction to be applied (unit vector) using the partial derivative of the signed distance, and controls the signed distance by multiplying these.

[0017] At this time, the control gain is the position gain "K p " and velocity gain "K d " are reflected as virtual elasticity and viscosity, respectively. Specifically, the rotation axis and hand position of the robot arm are within the range of motion shown in Figure 1, and are constrained to a circular orbit M on the yz plane shown in Figure 2.

[0018] The terms in Figures 1 and 2 are defined as follows: · x-axis: The axis representing the horizontal direction (forward). · y-axis: The axis representing the horizontal direction (sideways) and the vertical axis of rotation. · Z axis: The axis representing the vertical direction, the horizontal axis of rotation. θ y : The angle of rotation around the y-axis (y-axis rotation). θ z : The angle of rotation around the z-axis (z-axis rotation). ·y,z: Position of the robot arm end effector on (projected onto) the yz plane. · D: Length from the rotation axis of the arm to the tip of the hand (fixed in this embodiment). x SD : Signed distance (distance between the robot arm's end-effector position and the constraint trajectory). Constraint trajectory: A trajectory that constrains the position of the robot arm's hand on the yz plane (in this embodiment, a trajectory on the circle M). a, b: The center position of the circle on the yz plane.

[0019] <Configuration example> In Figure 3, reference numeral 1 denotes a trajectory constraint device of this embodiment. This trajectory constraint device 1 constrains the motion trajectory of the hand position of a two-axis rotary robot arm 10 to be controlled on a circle M in the yz plane. This robot arm 10 is Horizontal rotation around the z-axis (z-axis rotation / horizontal rotation), Vertical rotation around the y-axis (y-axis rotation / vertical rotation), It is configured as a two-axis rotary mechanism that operates as follows.

[0020] The trajectory constraint device 1 is configured by a computer and includes the blocks shown in Figure 3. Here, bold variables in Figure 3 indicate vectors or matrices, and thin line variables indicate scalars. Similarly, bold arrows indicate vectors, and thin line arrows indicate scalars. Each block in Figure 3 (excluding the robot arm 10) is defined as follows:

[0021] (1) Forward kinematics: The rotation angle (θ y ,θ z ) position to the hand position on the yz plane ([yz] T ) (T is transpose).

[0022] (2) SDF2: A block that calculates the signed distance “XSD” between the end point position of the robot arm 10 and the constraint trajectory. Hereinafter, this will be referred to as SDF block part 2.

[0023] (3) d / dt: Time differential calculation block.

[0024] (4) Kp: Position gain that controls XSD, and is the virtual elasticity of the force that constrains the constraint trajectory.

[0025] (5) Kd: The velocity gain that controls XSD, which is the virtual viscosity of the force that constrains the constraint trajectory.

[0026] (6) Equation (1): A signed distance command value input to the trajectory constraint device 1. In the first embodiment, it is expressed as Equation (2).

number

number

[0027] (7) Acceleration reference value calculation unit 3: A block that receives the position gain "Kp" and the velocity gain "Kd" and calculates the acceleration reference value (scalar / absolute value) for constraining the hand position of the robot arm 10 to the constraint trajectory. This acceleration reference value is as shown in equation (3).

number

[0028] (8) Unit vector calculation unit 7: The signed distance "XSD" between the end-point position of the robot arm 10 and the constraint trajectory is converted into a unit vector "e" in the y-axis and z-axis directions. yz " A block that calculates ".

[0029] (9) Acceleration reference value calculation unit 4: The acceleration reference value of equation (3) and the unit vector "e yz " is input to the block. The unit vector "e yz " is multiplied to calculate the acceleration reference value (work / matrix) converted in the y-axis and z-axis directions. This acceleration reference value is as shown in equation (4).

number

[0030] (10) Acceleration reference value calculation unit 5: A block that calculates the acceleration reference value (joint space) of each axis of the actuator of the robot arm 10 shown in equation (12).

number

[0031] (11) Equation (5): Inverse Jacobian matrix for converting from the working space to the joint space. This is used by the acceleration reference value calculation unit 5 to calculate the acceleration reference value of Equation (12).

number

[0032] (12) Torque reference value calculation unit 6: Calculates the torque reference value τ to be applied to the y-axis and z-axis rotation axes of the robot arm from the acceleration reference value of equation (12). ref " (torque command value / matrix) calculation block.

[0033] (13) "J n ”: Inertia matrix of y-axis and z-axis rotation. Torque reference value “τ ref " is used to calculate

[0034] <Example of operation> An example of the operation of the trajectory constraint device 1 when the hand position of the robot arm 10 is constrained to a circular M trajectory on a two-dimensional plane (yz plane) will be described. Here, the operation of the SDF block unit 2, acceleration reference value calculation units 3 to 5, torque reference value calculation unit 6, and unit vector calculation unit 7 in Figure 3 will be mainly explained, and the relationship between the range of motion of the robot arm 10, the hand position, and the constraint trajectory will be as shown in Figure 1.

[0035] First, the trajectory constraint device 1 starts control by inputting the command value of equation (1). At this time, the horizontal rotation angle θz and the vertical rotation angle yz for the hand position of the robot arm 10 are acquired, and the hand position on the yz plane is converted and expressed as equation (6) (forward kinematics). The hand position information of equation (6) is input to the SDF block unit 2.

number

number

number

number

number

[0036] Then, the acceleration reference value calculation unit 4 calculates the acceleration reference value of the equation (3) and the unit vector "e" of the equation (9) as shown in the equation (11). yz " to obtain the acceleration reference value (work space) in the y-axis and z-axis directions shown in equation (4).

number

number

number

number

number

[0037] The trajectory constraint device 1 of the second embodiment will be described with reference to Fig. 4. This embodiment differs from the first embodiment in that a changeover switch 11 for selecting "with / without SD control" of the robot arm is provided between the acceleration reference value calculation unit 5 and the torque reference value calculation unit 6.

[0038] That is, when the "command value of equation (1) > 0", the changeover switch 11 is closed, the acceleration reference value of equation 12 is input to the torque reference value calculation unit, and SD control is performed. On the other hand, when the "command value of equation (1) ≦ 0", the changeover switch 11 is opened, so the acceleration reference value of equation 12 is not input to the torque reference value calculation unit, and SD control is not performed.

[0039] Here, when the "command value of equation (1) > 0", i.e., "XSD" is positive, the robot will be positioned outside the trajectory constraint figure (circle M in Figure 2). In this case, if SD control is applied, the robot will be pulled back to circle M when it is positioned outside circle M, which will present the force sensation of a virtual wall. The elasticity and viscosity of the wall can be adjusted with "Kp" and "Kd" in Figure 2. If the robot goes outside the trajectory constraint figure, a control input will be applied, which can be used to limit the range of motion, etc. [Example]

[0040] The trajectory constraint device 1 of this embodiment is configured to perform a switching operation opposite to that of embodiment 2. That is, when the "command value of equation (1)<0", the changeover switch 11 is closed, the acceleration reference value of equation 12 is input to the torque reference value calculation unit, and SD control is performed. On the other hand, when the "command value of equation (1)≧0", the changeover switch 11 is opened, the acceleration reference value of equation 12 is not input to the torque reference value calculation unit, and SD control is not performed.

[0041] Here, when the "command value of equation (1)<0", that is, when "XSD" is negative, the object will be located inside the trajectory constraint figure, in the case of circle M in Figure 2, it will be located inside circle M. In this case, when SD control is applied, the object will be repelled to the outside of circle M when it is located inside circle M, so the force sensation of a circular virtual object will be presented. The elasticity and viscosity of the virtual object can be adjusted with "Kp" and "Kd" in the figure. [Example]

[0042] A trajectory constraint device 1 according to a fourth embodiment will be described with reference to FIG. 5. This trajectory constraint device 1 aims to present a force sensation in a 3D space (such as a sphere). That is, the robot arm 10: Linear motion with the x-axis as the linear axis, and Horizontal rotation around the z-axis (z-axis rotation / horizontal rotation), Vertical rotation around the y-axis (y-axis rotation / vertical rotation), It is configured as a three-axis mechanism that operates as follows.

[0043] In this embodiment, the tip position of the robot arm 10 to be controlled is expressed in three-dimensional space [r, θ y ,θ z ], so the hand position transformed by forward kinematics becomes [x,y,z].

[0044] Therefore, the unit vectors output by the unit vector calculation unit 7 are three-dimensional, and the variables of each vector and matrix are expanded for three dimensions. The variable r in the three-dimensional space is the variable D in Figure 1 (corresponding to the length from the rotation axis of the arm to the tip of the hand).

[0045] The unit vector [e x ,e y ,e z ] is obtained by normalizing the partial differential value of equation (22) as shown in equations (18) and (19).

number

number

number

number

[0046] <Simulation example> As described above, the SD control in each embodiment achieves the following three functions: That is, by adopting the configuration of embodiment 1, it is possible to constrain the trajectory of the hand position of the robot arm 10 on a figure defined by SDF (for example, on the circle M in Fig. 2) and to impart mechanical properties such as elasticity and viscosity.

[0047] By adopting the configuration of Example 2, it is possible to present the force sensation of a virtual wall to prevent the user from going outside the figure, thereby restricting the range of motion. By adopting the configuration of Example 3, a repulsive force is generated when the user enters the inside of the figure, and it is possible to present the force sensation of a virtual object. By adopting the configuration of Example 4, it is possible to realize these functions not only on a two-dimensional plane but also in three-dimensional space. Below, we will explain a simulation example in which Example 3 is applied to the fitness machine of Figure 6.

[0048] (1) Example of fitness machine configuration As shown in FIG. 6, 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.

[0049] Here, an application example will be described in which the mechanical unit 21 in Fig. 7 is the robot arm 10 to be controlled, and the operating unit (grip) 36 is the hand position. In Fig. 7, x indicates linear motion along the X axis, y indicates rotation around the Y axis, and z indicates rotation around the Z axis.

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

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

[0052] 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 z 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.

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

[0054] 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 x-direction.

[0055] (2) Simulation method and results In this simulation, the rotational degree of freedom "θ1" driven by motor 38 and the rotational degree of freedom "θ2" driven by motor 39 in Fig. 7 were used. The linear degree of freedom "r" driven by motor 37 was not used and was fixed.

[0056] The rotational degrees of freedom "θ1, θ2" correspond to "θy, θz" in the embodiment, and the control of the third embodiment was executed on a two-dimensional plane formed by these, and the presentation of the force sensation of a virtual object of a circle M was simulated.

[0057] Figure 8(a) shows the time series response of this simulation, and Figure 8(b) shows the force response on the same two-dimensional plane. In Figure 8(a), the hand (operation unit 36) starts from the origin (0). At approximately 10 seconds, the hand contacts the virtual object circle M from the horizontal direction (θ2 side), generating a horizontal reaction force. At around 12 seconds and 13 seconds, the hand contacts circle M from above in the vertical direction (θ1 side), generating a vertical reaction force. From 15 seconds onward, the hand moves as if stroking the surface of circle M, confirming that the contour of circle M is realized from the trajectory shown in Figure 8(b). This confirms that the force sensation of a circular virtual object can be presented when constrained on the trajectory of circle M.

[0058] Other examples The present invention is not limited to the above-described embodiment, and can be modified and implemented within the scope of the claims. For example, the SD (trajectory constraint figure) may be another known shape. An example will be described below.

[0059] (1) SD of coordinates (x, y, z) on a two-dimensional line or a three-dimensional plane

number

[0060] (2) The distance to the three-dimensional line can be obtained by applying the calculation of equation (7). That is, (i) Regarding a cylinder of radius R (obtained by rotating and translating the cylinder in the z-axis direction with its center at x=y=0),

[0061] (ii) The SD is calculated between the input coordinates and the one where R = 0 (applying an inverse transformation to the above-mentioned cylindrical transformation to the input coordinates, and calculating the SD based on the transformed figure). In this case, however, the square root of equation (7) is 0 or more, and R is 0, so the SD is 0 or more.

[0062] (3) For other shapes, the SD can be calculated by using the rotation, translation, linear transformation, and other transformations of the shapes mentioned above. Regarding the combination of shapes, the SD can be calculated by selecting an appropriate shape depending on the position and calculating the SD of that shape. Alternatively, the SD can be determined by mathematically defining the minimum distance between the coordinates of a point on the shape and a certain coordinate, and the sign corresponding to "outside" or "inside." [Explanation of symbols]

[0063] 1…Trajectory restraint device 2...SDF block section 3~5...Acceleration reference value calculation section 6...Torque reference value calculation section 7...Unit vector calculation section 10...Robot arm

Claims

1. A device that constrains the movement of a controlled object to a trajectory determined from at least two or more axes, including at least a rotation axis and a horizontal axis, an SDF block section which receives input of information converted from the operating position of the actuator of the controlled object into position information on a plane or space of a specific portion that constrains the trajectory of the controlled object in accordance with a command value of the signed distance "XSD", and calculates the signed distance "XSD" from the constrained trajectory from the converted position information; a first acceleration reference value calculation unit that calculates an acceleration reference value (absolute value) using a position gain and a velocity gain that indicate virtual elasticity and viscosity when controlling the signed distance "XSD"; a second acceleration reference value calculation unit that converts the acceleration reference value (absolute value) into an acceleration reference value for workspace by multiplying a calculation result of a direction (unit vector) to be applied to the partial differential of the signed distance "XSD" by the acceleration reference value (absolute value); a third acceleration reference value calculation unit that converts the workspace acceleration reference value into an acceleration reference value of each of the axes (joint space) that moves each of the axes of the control object; a torque reference value calculation unit that calculates a torque reference value for each axis of the actuator to be controlled by multiplying the acceleration reference value for each axis by the inertia in the direction of the movement; Equipped with A trajectory constraint device, characterized in that the trajectory of the controlled object is constrained by controlling the torque of each axis of the actuator based on the torque reference value.

2. The control object is Vertical rotation around the horizontal (horizontal) y-axis. Horizontal rotation around the vertical z-axis; While it is configured as a two-axis mechanism that operates A command value for the distance "XSD" shown in formula (1) is input, [Equation 1] the SDF block unit receives position information of the specific part on the yz plane expressed using the horizontal rotation angle and the vertical rotation angle, and calculates a signed distance "XSD" between the position information and the constraint trajectory on the yz plane; The second acceleration reference value calculation unit converts the acceleration reference values ​​into acceleration reference values ​​in the workspace in the y-axis and z-axis directions, the third acceleration reference value calculation unit converts the acceleration reference value into an acceleration reference value in a joint space of two rotational axes that rotates the control object; the torque reference value calculation unit calculates a torque reference value for each axis by multiplying the acceleration reference value in the joint space by an inertia in the direction of each rotation angle; 2. The trajectory constraint device according to claim 1, wherein the specific portion of the controlled object is constrained to a trajectory on a graphic in a yz plane.

3. The command value of equation (1) is 3. The trajectory restraint device according to claim 2, wherein the formula (2) is satisfied. [Equation 2]

4. a changeover switch is provided between the third acceleration reference value calculation unit and the torque reference value calculation unit, The changeover switch is When the command value of equation (1) is greater than 0, the torque reference value calculation unit is closed and the acceleration reference value in the joint space is input. When the command value is equal to or less than 0, the torque reference value calculation unit is opened and the acceleration reference value in the joint space is not input.

3. The trajectory restraint device according to claim 2.

5. a changeover switch is provided between the third acceleration reference value calculation unit and the torque reference value calculation unit, The changeover switch is When the command value of the formula (1) is smaller than 0, the torque reference value calculation unit is closed and the acceleration reference value in the joint space is input. When the command value is greater than 0, the torque reference value calculation unit is opened and the acceleration reference value in the joint space is not input.

3. The trajectory restraint device according to claim 2.

6. The control object is Linear motion with the horizontal (forward) x-axis as the linear axis; Vertical rotation around the horizontal (horizontal) y-axis. Horizontal rotation around the vertical z-axis; While it is configured as a three-axis mechanism that operates A command value for the distance "XSD" shown in formula (1) is input, [Equation 1] The SDF block receives position information of the specific part in the xyz space expressed using the length of the linear motion, the horizontal rotation angle, and the vertical rotation angle, and calculates the signed distance "XSD" between the position information and the constraint trajectory in the xyz space; the second acceleration reference value calculation unit converts the acceleration reference values ​​into acceleration reference values ​​in the workspace in the directions of the x-axis, y-axis, and z-axis; the third acceleration reference value calculation unit converts the acceleration reference values ​​into three-axis joint space acceleration reference values ​​for moving the controlled object; the torque reference value calculation unit calculates a torque reference value for each axis by multiplying the acceleration reference value in the joint space by inertia in the length direction and in the direction of each rotation angle; 2. The trajectory constraint device according to claim 1, wherein the specific portion of the controlled object is constrained to a trajectory on a graphic in xyz space.

7. a changeover switch is provided between the third acceleration reference value calculation unit and the torque reference value calculation unit, The changeover switch is When the command value of equation (1) is greater than 0, the torque reference value calculation unit is closed and the acceleration reference value in the joint space is input. When the command value is smaller than 0, the torque reference value calculation unit is opened and the acceleration reference value in the joint space is not input.

7. The track restraint device of claim 6.

8. The computer A method for constraining the motion of a controlled object to a trajectory determined from at least two or more axes, at least a rotation axis and a horizontal axis, comprising: a step of converting information obtained by converting the operating position of the actuator of the controlled object into position information on a plane or in space of a specific portion that constrains the trajectory of the controlled object according to a command value of the signed distance "XSD", and calculating the signed distance "XSD" from the constrained trajectory from the converted position information; When controlling the signed distance "XSD", a step of calculating an acceleration reference value (absolute value) using a position gain and a velocity gain indicating virtual elasticity and viscosity; a step of converting the acceleration reference value for the workspace by multiplying the calculation result of the direction (unit vector) to be applied to the partial differential of the signed distance "XSD" by the acceleration reference value (absolute value); converting the workspace acceleration reference value into an acceleration reference value of each of the axes (joint space) that moves each of the axes of the control object; determining a torque reference value for each axis of the actuator to be controlled by multiplying the acceleration reference value for each axis by the inertia in the direction of the movement; constraining the trajectory of the controlled object by controlling the torque of each axis of the actuator based on the torque reference value; A trajectory constraint method comprising:

Images