Trajectory constraint device and trajectory constraint method

Abstract

The present invention imparts mechanical characteristics such as elasticity and viscosity when constraining the trajectory of a controlled object onto a figure (a two-dimensional figure or a three-dimensional spatial figure) defined by a signed distance function (SDF). In a trajectory constraint device 1, a rotational angle position of a robot arm 10 is converted into end tip position information using a command value input for a signed distance, and the signed distance between the position information and a bound trajectory is calculated. When the signed distance is being controlled, an acceleration reference value expressed by formula (3) is calculated using a velocity gain and position gain representing virtual elasticity and viscosity. Said acceleration reference value is multiplied with a calculated result of a unit vector applied to a partial differential of the signed distance XSD, converting into an acceleration reference value expressed by formula (4). This acceleration reference value is converted into an acceleration reference value expressed by formula (12) to rotate each shaft of the robot arm 10. The inertia of the rotational angle direction of each shaft is then multiplied by this acceleration reference value, the torque of each shaft is controlled by determining a torque reference value for each shaft, and the trajectory of the robot arm 10 is constrained.

Description

Track restraint device, track restraint method

[0001] This invention relates to a technique for constraining the movement of a controlled object to a specific trajectory.

[0002] "SDF (Signed Distance Function)" is often used in fields such as computer graphics (CG). Patent Document 1 describes how SDF is used to visually present virtual objects in order to provide an immersive experience using XR (Extended Reality). Compared to modeling with a mesh, it is possible to visually present virtual objects with a simplified program.

[0003] Non-patent document 1 proposes a robot for surgical simulators, and as shown in equations (1) and (2) in the non-patent document, it presents force feedback using the functions "SDF" and "exponential".

[0004] JP 2023-504570

[0005] “Haptic-Assisted Collaborative Robot Framework for Improved Situational Awareness in Skull Base Surgery”

[0006] As mentioned above, Patent Document 1 uses "SDF" for the visual presentation of virtual objects, and while it enables implementation with simplified programming compared to modeling with a mesh, it does not provide force feedback for the virtual objects.

[0007] Similarly, Non-Patent Document 2 presents force perception using "SDF," but it cannot directly provide mechanical properties of a virtual object such as elasticity or viscosity.

[0008] This invention was made to solve the problems of the conventional approach, and aims to constrain a controlled object to a trajectory on a figure defined by "SDF" (a figure in a two-dimensional plane or a figure in three-dimensional space), thereby imparting mechanical properties such as elasticity and viscosity.

[0009] (1) One aspect of the present invention is a device for constraining the movement of a controlled object to a trajectory determined from at least two or more axial directions, including a rotation axis and a horizontal axis, comprising: an SDF block unit that receives information converted from the operating position of the actuator of the controlled object to position information on a plane or in space of a specific part that constrains the trajectory of the controlled object, according to a command value of signed distance "XSD", and calculates a 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 position gains and velocity gains that indicate virtual elasticity and viscosity when controlling the signed distance "XSD"; a second acceleration reference value calculation unit that converts the calculation result of the direction (unit vector) applied to the partial derivative of the signed distance "XSD" to an acceleration reference value for the working space by multiplying the acceleration reference value (absolute value) by the acceleration reference value; and a third acceleration reference value calculation unit that converts the acceleration reference value for the working space to an acceleration reference value for each axis (joint space) that operates each axis of the controlled object. The system includes a torque reference value calculation unit that calculates torque reference values ​​for each axis of the actuator to be controlled by multiplying the acceleration reference value of the joint space by the inertia of the direction of motion, and is 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 values.

[0010] (2) Another aspect of the present invention is a method by which a computer constrains the movement of a controlled object to a trajectory determined from at least two or more axial directions, including at least a rotation axis and a horizontal axis, the method comprising: inputting information converted from the operating position of the actuator of the controlled object to position information on a plane or in space of a specific part that constrains the trajectory of the controlled object, according to a command value of signed distance "XSD", and calculating a signed distance "XSD" between the constrained trajectory and the converted position information; calculating 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"; converting the calculation result of the direction (unit vector) applied to the partial derivative of the signed distance "XSD" to an acceleration reference value for the working space by multiplying the acceleration reference value (absolute value) by the acceleration reference value; and converting the acceleration reference value for the working space to an acceleration reference value for each axis (joint space) that operates each axis of the controlled object. The method is characterized by comprising the steps of: determining the torque reference value for each axis of the actuator to be controlled by multiplying the acceleration reference value of the joint space by the inertia of the direction of motion; and constraining the trajectory of the controlled object by controlling the torque of each axis of the actuator based on the torque reference value.

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

[0012] Diagram showing the relationship between the rotation axis and end-effector position of the robot arm in Example 1. Diagram showing the relationship between the end-effector position of the robot arm on the yz plane and the constrained trajectory. Control block diagram of the same. Control block diagram of Example 2. Control block diagram of Example 4. Perspective view of the fitness machine used in the simulation of Example 3. Configuration diagram of the simulation target. (a) is a time-series response graph showing the simulation results of Example 3, and (b) is a trajectory diagram in the two-dimensional plane.

[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 the robot arm to be controlled (here, the end-effector position as an example) to a specific trajectory, and provides mechanical properties such as elasticity and viscosity to allow the operator (user) to move within an appropriate trajectory.

[0014] In this case, by virtually applying mechanical properties such as elasticity and viscosity through control both inside and outside an arbitrary boundary, it is possible to limit the range of motion of the robot arm's end-effector and present a sense of force from a virtual object.

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

[0016] ≪Basic Concept≫ In this embodiment, the trajectory constraint device calculates the absolute value of the reference value by SD control (PD control) of the signed distance, calculates the direction of application (unit vector) by partial derivative of the signed distance, and controls the signed distance by multiplying these two values.

[0017] At that time, the control gain is the position gain "K ｐ " and speed gain "K ｄ These are reflected as virtual elasticity and viscosity, respectively. Specifically, the rotation axis and end-effector position of the robot arm are within the range of motion shown in Figure 1 and are constrained to a circular orbit M in the yz plane shown in Figure 2.

[0018] The terms used 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 axis of rotation in the vertical direction. • z-axis: The axis representing the vertical direction, and the axis of rotation in the horizontal direction. • θ ｙ θ: The angle of rotation around the y-axis (y-axis rotation). ｚ : 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 arm's rotation axis to the end-effector (fixed in this embodiment). • x ＳＤ: Signed distance (distance from the end position of the robot arm to the constrained trajectory). - Constrained trajectory: A trajectory that constrains the end position of the robot arm on the yz plane (a trajectory on circle M in this embodiment). - a, b: Center positions of the circle on the yz plane.

[0019] <<Configuration Example>> 1 in FIG. 3 shows the trajectory constraint device of this embodiment. This trajectory constraint device 1 constrains the operating trajectory of the end position of the two-axis rotating robot arm 10 to be controlled on circle M in the yz plane. This robot arm 10 is configured as a two-axis rotating mechanism that operates with a horizontal rotation (z-axis rotation / horizontal direction rotation) with the z-axis as the rotation axis and a vertical rotation (y-axis rotation / vertical direction rotation) with the y-axis as the rotation axis.

[0020] The trajectory constraint device 1 is constituted by a computer and includes each block in FIG. 3. Here, the bold variables in FIG. 3 indicate vectors or matrices, and the thin-line variables indicate scalars. Similarly, the thick-line arrows indicate vectors, and the thin-line arrows indicate scalars. Also, each block in FIG. 3 (excluding the robot arm 10) is defined as follows.

[0021] (1) Forward kinematics: A conversion formula (T is transpose) that converts the rotation angle (θ ｙ , θ ｚ ) position of the end position of the robot arm 10 to the end position ([yz] Ｔ ) on the yz plane.

[0022] (2) SDF2: A block that calculates the signed distance "XSD" between the end position of the robot arm 10 and the constrained trajectory. Hereinafter, it is referred to as the SDF block unit 2.

[0023] (3) d / dt: A time differential operation block.

[0024] (4) Kp: A position gain that controls XSD, which is the virtual elasticity of the force that constrains to the constrained trajectory.

[0025] (5) Kd: A speed gain that controls XSD, which is the virtual viscosity of the force that constrains to the constrained trajectory.

[0026] (6) Equation (1): The command value of the signed distance input to the trajectory constraint device 1. In Example 1, it is Equation (2).

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

[0028] (8) Unit vector calculation unit 7: A block that calculates the unit vectors "e ｙｚ " in the y-axis and z-axis directions from the signed distance "XSD" between the tip position of the robot arm 10 and the constraint trajectory.

[0029] (9) Acceleration reference value calculation unit 4: A block that receives the acceleration reference value of Equation (3) and the unit vector "e ｙｚ ". By multiplying the acceleration reference value of Equation (3) by the unit vector "e ｙｚ ", an acceleration reference value (working / matrix) converted in the y-axis and z-axis directions is calculated. This acceleration reference value shall be as shown in Equation (4).

[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).<00​​​​​​​​​​​​​​​​​​<Example of Operation> This section describes an example of the operation of the trajectory constraint device 1 when the end-effector position of the robot arm 10 is constrained to a circular M-trajectory in a two-dimensional plane (yz plane). Here, the operation of the SDF block section 2, acceleration reference value calculation sections 3-5, torque reference value calculation section 6, and unit vector calculation section 7 in Figure 3 will be explained, and the relationship between the range of motion of the robot arm 10, the end-effector position, and the constraint trajectory will be as shown in Figure 1.

[0035] First, the trajectory constraint device 1 starts control upon input of the command value given by equation (1). At this time, the horizontal rotation angle θz and the vertical rotation angle yz of the end-effector position of the robot arm 10 are acquired, and the end-effector position on the yz plane is converted and expressed as equation (6) (forward kinematics). This end-effector position information from equation (6) is input to the SDF block unit 2. Next, the SDF block 2 determines the signed distance "XSD" of the constrained trajectory from the end-effector position information in equation (6). Here, the constrained trajectory is defined as circle M in Figure 2, that is, a circle with radius R at the center position (a, b), and is calculated using equation (7). The XSD control (SD control) is performed using the calculation result of equation (7). In Figure 2, the XSD control is configured as PD control, and the acceleration reference value of equation (3), which is the PD output of the acceleration reference value calculation unit 3, is obtained by equation (8). At this time, the acceleration reference value calculation unit 3 acquires the control gains "Kp (position gain)" and "Kd (velocity gain)" and uses them in the calculation of equation (8). When the trajectory of this embodiment is constrained, the command value of "XSD" shown in equation (1) above should be set to "0" as shown in equation (2). Here, since the acceleration reference value calculated by equation (8) is a scalar value on the yz plane, the acceleration reference value calculation unit 4 converts it to the acceleration reference value in the y-axis and z-axis directions (workspace) shown in equation (4). At that time, the unit vectors "e" in the y-axis and z-axis directions are used for the conversion. ｙｚ This value is calculated from equations (9) and (10) by the unit vector calculation unit 7 and input to the acceleration reference value calculation unit 4. When L = 0, [e ｙ , e ｚ Let ] = [0,0].

[0036] Then, as shown in equation (11), the acceleration reference value calculation unit 4 uses the acceleration reference value from equation (3) and the unit vector "e" from equation (9) ｙｚ By multiplying by , we obtain the acceleration reference values ​​(workspace) in the y-axis and z-axis directions shown in equation (4). Next, the acceleration reference value calculation unit 5 converts the acceleration reference value in equation (4). Here, it converts it to the acceleration reference value of the two rotation axes (joint space) shown in equation 12, which actually rotates the robot arm. Equation (13) is used for this conversion. Equation (5) in equation (13) is the inverse of the Jacobian matrix, which is shown in equation 14. Equation (13) yields the acceleration reference value for the joint space in equation (12). Subsequently, the torque reference value calculation unit 6 calculates the acceleration reference value in equation 12 by adding "θ" as shown in equation (15). ｙ , θ ｚ The torque reference value "τ" for each axis of the actuator is obtained by multiplying by the inertia in the direction of "τ". ｒｅｆ Calculate ". In equation 15, "J ｎ " is "θ ｙ , θ ｚ The nominal function of the joint space in the direction can be shown and expressed by equation (16). Based on the calculation results of equation (15), the torque of each rotation axis of the actuator of the robot arm 10 is controlled, thereby enabling trajectory constraint on the circle M in the yz plane as shown in Figure 2. At this time, the control gains "Kp (position gain)" and "Kd (velocity gain)" correspond to virtual elasticity and viscosity, respectively, and mechanical properties such as elasticity and viscosity can be provided. Note that the torque reference value "τ ｒｅｆ Gravity compensation and disturbance compensation such as friction may be added to this.

[0037] The trajectory restraint device 1 of Embodiment 2 will be described based on Figure 4. This embodiment differs from Embodiment 1 in that a switch 11 for "SD control on / off" of the robot arm is provided between the acceleration reference value calculation unit 5 and the torque reference value calculation unit 6.

[0038] In other words, if the command value in equation (1) is greater than 0, the changeover switch 11 is closed and the acceleration reference value in equation 12 is input to the torque reference value calculation unit, and SD control is executed. On the other hand, if the command value in equation (1) is less than or equal to 0, the changeover switch 11 is opened, so the acceleration reference value in equation 12 is not input to the torque reference value calculation unit, and SD control is not executed.

[0039] Here, when the command value in equation (1) > 0, i.e., when XSD is positive, the object will be located outside the trajectory constraint figure (circle M in Figure 2). In this case, applying SD control will result in a control that pulls the object back into circle M when it is located outside of circle M, thus presenting a sense of force from a virtual wall. The elasticity and viscosity of the wall can be adjusted using Kp and Kd in Figure 2. Since a control input is applied when the object deviates from the trajectory constraint figure, it can be used for range of motion limitations, etc.

[0040] The trajectory restraint device 1 in this embodiment is configured to perform a switching operation opposite to that of Embodiment 2. That is, when the command value in equation (1) is less than 0, the changeover switch 11 is closed, the acceleration reference value in equation 12 is input to the torque reference value calculation unit, and SD control is executed. On the other hand, when the command value in equation (1) is greater than or equal to 0, the changeover switch 11 is opened, the acceleration reference value in equation 12 is not input to the torque reference value calculation unit, and SD control is not executed.

[0041] Here, when the command value in equation (1) is less than 0, i.e., when XSD is negative, the object will be located inside the trajectory constraint figure, or inside circle M in the case of circle M in Figure 2. In this case, applying SD control will cause the object to bounce back to the outside of circle M when it is located inside circle M, thus presenting a force sensation of a circular virtual object. The elasticity and viscosity of the virtual object can be adjusted using Kp and Kd in the figure.

[0042] Based on Figure 5, the trajectory constraint device 1 of Embodiment 4 will be explained. This trajectory constraint device 1 aims to provide force feedback in 3D space (such as a sphere). That is, the robot arm 10 is configured as a three-axis mechanism that performs linear motion with the x-axis as the linear axis of motion, horizontal rotation with the z-axis as the axis of rotation (z-axis rotation / horizontal rotation), and vertical rotation with the y-axis as the axis of rotation (y-axis rotation / vertical rotation).

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

[0044] Therefore, the unit vector output by the unit vector calculation unit 7 becomes three-dimensional, and the variables of each vector and matrix are extended for three dimensions. The variable r in three-dimensional space corresponds to variable D in Figure 1 (corresponding to the length from the arm's rotation axis to the end of the arm).

[0045] The unit vector [e ｘ , e ｙ , e ｚ This is obtained by normalizing the value of the partial derivative of equation (22), as shown in equations (18) and (19). When L = 0, [e ｘ , e ｙ , e ｚ Let ] = [0,0,0]. Then, the acceleration reference value calculation unit 4 calculates the acceleration reference value of equation (3), which is a scalar value in xyz space, and the unit vector [e ｘ , e ｙ , e ｚ By multiplying by ], it is converted into acceleration reference values ​​(equation (20)) for the workspace in the x, y, and z directions. Furthermore, the acceleration reference value calculation unit 5 converts the acceleration reference values ​​(equation (12)) of the joint space of the three axes (x axis, y axis, z axis) that operate the robot arm 10. In addition, the torque reference value calculation unit 6 converts the acceleration reference values ​​of the joint space [D, θ ｙ , θ ｚ By multiplying by the inertia in the direction, the three-axis torque reference value "τ" is obtained. ｒｅｆ This determines the position of the end-effector of the robot arm 10 and constrains it to a figure in xyz space. The changeover switch 11 in this embodiment operates in the same manner as in embodiment 2.

[0046] <<Simulation Examples>> As described above, the SD control in each embodiment realizes the following three functions. That is, by adopting the configuration of Embodiment 1, the trajectory of the end-effector position of the robot arm 10 can be constrained to the figure defined in the SDF (for example, on the circle M in Figure 2), and mechanical properties such as elasticity and viscosity can be given.

[0047] By adopting the configuration of Example 2, the force sensation of a virtual wall can be presented to prevent the user from going outside the shape, thereby limiting the range of motion. By adopting the configuration of Example 3, a repulsive force is generated when the user enters the inside of the shape, allowing the force sensation of a virtual object to be presented. By adopting the configuration of Example 4, these functions can be realized not only in a two-dimensional plane but also in three-dimensional space. Below, a simulation example applying Example 3 to the fitness machine shown in Figure 6 will be described.

[0048] (1) Example of Fitness Machine Configuration As shown in Figure 6, the fitness machine 20 comprises a chair 23 on which the user (trainer) sits, left and right operating parts (grips) 36 that the user grasps and operates, and three movable mechanisms 21 and 22 that correspond to the three axes (X axis, Y axis, and Z axis), and these mechanisms 21 and 22 are located on the left and right sides of the chair 23.

[0049] Here, we will describe an application example in which the mechanism 21 in Figure 7 is the robot arm 10 to be controlled, and the operating part (grip) 36 is the end-effector position. In Figure 7, x represents linear motion along the X-axis, y represents rotation around the Y-axis, and z represents rotation around the Z-axis.

[0050] The mechanism 21 includes a linear motion mechanism 34 that moves the grip 36 in the x direction, and a rotary mechanism that rotates the linear motion mechanism 34 in the y and z directions, with motors 38 to 39 attached to each. In this simulation, these motors 37 to 39 correspond to the actuators of the robot arm 10.

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

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

[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 pivotally supported on the shaft of the motor 38 so as to be rotatable in the y-direction. This constitutes a rotation mechanism that rotates the linear motion mechanism 34 in the y-direction, and the motor 38 applies a load to the rotation of the arm portion 34a in the same direction.

[0054] The linear motion mechanism 34 includes a motor 37 mounted on the rear end of the arm portion 34a, a slider (movable element) 35 to which the 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 Figure 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 Embodiment 3 was executed in the two-dimensional plane composed of these degrees of freedom to simulate the presentation of force feedback for a virtual object of circle M.

[0057] Figure 8(a) shows the time-series response of this simulation, and Figure 8(b) shows the same simulation in a two-dimensional plane. In Figure 8(a), the end-effector position (operating part 36) starts from the origin (0), and around 10s, it makes contact with the virtual object circle M from the lateral direction (θ2 side), generating a lateral reaction force. Around 12s and 13s, it makes contact with the circle M from above in the vertical direction (θ1 side), generating a vertical reaction force, and thereafter from 15s onwards, it moves as if stroking the surface of the circle M, confirming that the contour of the circle M can be realized from the trajectory in Figure 8(b). This confirms that when constrained to the trajectory of the circle M, the force sensation of a circular virtual object can be presented.

[0058] <Other Examples> The present invention is not limited to the embodiments described above, and can be implemented by modifying the claims within the scope described. For example, the SD (orbital constraint figure) may be any other known shape. An example is described below.

[0059] (1) SD of coordinates (x, y, z) for a line in two dimensions or a plane in three dimensions Here, (a_x, a_y, a_z) are unit vectors and normal vectors pointing "outward" from the line or plane, and (x_0, y_0, z_0) correspond to the coordinates of a point on the line or plane.

[0060] (2) The distance to a line in three dimensions can be obtained by applying the calculations related to equation (7). That is, (i) with respect to a cylinder of radius R (obtained by a transformation that rotates and translates a cylinder in the z-axis direction with x=y=0 as the center),

[0061] (ii) The SD can be obtained by finding the SD between the input coordinates and the case where R = 0 (by applying the inverse transformation to the above cylinder transformation to the input coordinates and calculating the SD based on the transformed figure). However, in this case, the value inside the square root of equation (7) is 0 or greater, and R is 0, so the SD will be 0 or greater.

[0062] (3) For other shapes as well, the standard deviation (SD) can be determined by using rotational transformations, translational transformations, linear transformations, and other various transformations of the shapes described above. Regarding combinations of shapes, this can be handled by selecting an appropriate shape according to its position and determining the SD with respect to 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 whether it is "outside" or "inside".

[0063] 1...Trajectory constraint 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 for constraining the movement of a controlled object to a trajectory determined from at least two or more axial directions, including at least a rotation axis and a horizontal axis, comprising: an SDF block unit that receives information converted from the operating position of the actuator of the controlled object to position information on the plane or in space of a specific part that constrains the trajectory of the controlled object, according to a command value of signed distance "XSD", and calculates a 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 position gains and velocity gains that indicate virtual elasticity and viscosity when controlling the signed distance "XSD"; a second acceleration reference value calculation unit that converts the calculation result of the direction (unit vector) applied to the partial derivative of the signed distance "XSD" to an acceleration reference value for the working space by multiplying the acceleration reference value (absolute value) by the acceleration reference value; and a third acceleration reference value calculation unit that converts the acceleration reference value for the working space to an acceleration reference value for each axis (joint space) that operates each axis of the controlled object. A trajectory constraint device comprising: a torque reference value calculation unit that calculates torque reference values ​​for each axis of the actuator to be controlled by multiplying the acceleration reference value of the joint space by the inertia of the direction of motion, wherein the trajectory of the controlled object is constrained by controlling the torque of each axis of the actuator based on the torque reference values.

2. The controlled object is configured as a two-axis mechanism whose operation involves vertical rotation with the horizontal y-axis as the axis of rotation and horizontal rotation with the vertical z-axis as the axis of rotation, while the command value of the distance "XSD" shown in equation (1) is input, The SDF block 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 constrained trajectory on the yz plane. The second acceleration reference value calculation unit converts this into acceleration reference values ​​of the working space in the y-axis and z-axis directions. The third acceleration reference value calculation unit converts this into acceleration reference values ​​of the joint space of the two rotation axes that rotate the controlled object. The torque reference value calculation unit obtains torque reference values ​​for each axis by multiplying the acceleration reference values ​​of the joint space by the inertia in the direction of each rotation angle. The trajectory constraint device according to claim 1, characterized in that the specific part of the controlled object is constrained to a trajectory on a figure in the yz plane.

3. The trajectory restraint device according to claim 2, characterized in that the command value of formula (1) is formula (2).

4. A changeover switch is provided between the third acceleration reference value calculation unit and the torque reference value calculation unit, wherein the changeover switch is closed when the command value of equation (1) > 0, so that the acceleration reference value of the joint space is input to the torque reference value calculation unit, and is opened when the command value ≤ 0, so that the acceleration reference value of the joint space is not input to the torque reference value calculation unit, as described in claim 2.

5. The trajectory restraint device according to claim 2, wherein a changeover switch is provided between the third acceleration reference value calculation unit and the torque reference value calculation unit, and the changeover switch is closed when the command value of formula (1) < 0, so that the acceleration reference value of the joint space is input to the torque reference value calculation unit, and is open when the command value > 0, so that the acceleration reference value of the joint space is not input to the torque reference value calculation unit.

6. The controlled object is configured as a three-axis mechanism whose operation consists of linear motion with the x-axis in the horizontal direction (forward) as the axis of motion, vertical rotation with the y-axis in the horizontal direction (sideways) as the axis of rotation, and horizontal rotation with the z-axis in the vertical direction as the axis of rotation, while the command value of the distance "XSD" shown in equation (1) is input, The SDF block is input to the position information of the specific part in xyz space, expressed using the length of the linear motion, the horizontal rotation angle, and the vertical rotation angle, and calculates a signed distance "XSD" between the position information and the constrained trajectory in xyz space. The second acceleration reference value calculation unit converts it into acceleration reference values ​​of the working space in the x-axis, y-axis, and z-axis directions. The third acceleration reference value calculation unit converts it into acceleration reference values ​​of the three-axis joint space that operates the controlled object. The torque reference value calculation unit obtains torque reference values ​​for each axis by multiplying the acceleration reference values ​​of the joint space by the inertia in the length direction and the direction of each rotation angle, and the trajectory constraint device according to claim 1 is characterized in that the specific part of the controlled object is constrained to a trajectory on a figure in xyz space.

7. A changeover switch is provided between the third acceleration reference value calculation unit and the torque reference value calculation unit, wherein the changeover switch is closed when the command value of formula (1) > 0, so that the acceleration reference value of the joint space is input to the torque reference value calculation unit, and is opened when the command value < 0, so that the acceleration reference value of the joint space is not input to the torque reference value calculation unit, as described in claim 6.

8. A method by which a computer constrains the movement of a controlled object to a trajectory determined from at least two or more axial directions, including at least a rotation axis and a horizontal axis, comprising: a step of inputting information converted from the operating position of the actuator of the controlled object to position information on a plane or in space of a specific part that constrains the trajectory of the controlled object, according to a command value of signed distance "XSD", and calculating the signed distance "XSD" between the constrained trajectory and the converted position information; a step of calculating an acceleration reference value (absolute value) using position gains and velocity gains that indicate virtual elasticity and viscosity when controlling the signed distance "XSD"; a step of converting the calculation result of the direction (unit vector) applied to the partial derivative of the signed distance "XSD" to an acceleration reference value for the working space by multiplying the acceleration reference value (absolute value) by the acceleration reference value; a step of converting the acceleration reference value for the working space to an acceleration reference value for each axis (joint space) that operates each axis of the controlled object; and a step of obtaining a torque reference value for each axis of the actuator of the controlled object by multiplying the acceleration reference value of the joint space by the inertia of the direction of movement. A trajectory constraint method characterized by comprising the step of constraining the trajectory of the controlled object by controlling the torque of each axis of the actuator based on the torque reference value.

Images