Force control system and method for robotic manipulators with at least six degrees of freedom

Abstract

The present invention relates to a force control system and method for robotic manipulators. The method comprises: initializing (102) the position (u) of a virtual guide (6) on a predetermined spline (5); obtaining (104) a position and orientation of an end effector (4); updating (108) the position (u) and speed (I) of the virtual guide (6) on the spline (5) based on the projection of the vector in the direction tangent (d t ) to the spline (5) from the virtual guide (6) to the end effector (4); obtaining (110) a reference system centered on the virtual guide (6) with one tangential axis (eT) and two normal axes (eN1, eN2) to the spline (5); calculating (112) a distance in rotation (DR) from the orientation of the end effector (4) to the orientation of the virtual guide (6); calculating (114) a distance in translation (d N 1, d N 2) on the normal axes from the end effector (4) to the virtual guide (6); calculating (116) a Cartesian impedance force in rotation (II) and a Cartesian impedance force in translation (III) on the normal axes (eN1, eN2) using the distance in rotation (DR) and the distance in translation (d N 1 d N 2); and applying (118) the forces to the robotic manipulator (1).

Description

[0001] DESCRIPTION

[0002] SYSTEM AND METHOD FOR FORCE CONTROL OF ROBOTIC MANIPULATORS WITH AT LEAST SIX DEGREES OF FREEDOM

[0003] Field of invention

[0004] The present invention falls within the field of robotic manipulators with an end effector for limbs and with at least six degrees of freedom. In particular, the invention relates to force control systems and methods for robotic manipulators.

[0005] Background of the invention

[0006] There is a growing demand for muscle rehabilitation, whether due to neurological conditions such as stroke or traumatic injuries. The aging population is one of the factors contributing to this increase. Pressure on the healthcare system is mounting, given the shortage of therapists and the significant physical demands of manual therapy. To facilitate therapy, exercise machines are used, most of which restrict the patient's range of motion through mechanisms, and can also vary the resistance. However, these machines are limited to a single type of movement and are rarely sensorized.

[0007] Force control in robotics is a constantly evolving area of ​​research, with numerous techniques and approaches developed to achieve precise and adaptable control of the forces exerted by robots in various applications:

[0008] Model-based control [1]: This technique uses dynamic models of the robot and its environment to calculate the required forces and apply them precisely. Classical control methods, such as proportional-derivative (PD) or proportional-integral-derivative (PID) control, can be used, as well as more advanced techniques, such as predictive control.

[0009] Hybrid force control [2]: Combines model-based control with force feedback control techniques to achieve more robust and adaptable control. This approach allows the robot to adjust its behavior in real time based on forces detected during interaction with the environment or with humans.

[0010] Force input control [3]: This approach focuses on controlling the force exerted by the robot on an object or surface, adapting to variations in resistance encountered during the interaction. It is commonly used in assembly, handling, and collaborative robotics applications.

[0011] Behavior-based control [4]: ​​This approach draws inspiration from the behavior of living beings to achieve adaptive and flexible force control in robots. Sensory feedback is used to adjust the robot's behavior based on environmental conditions and interactions with objects and people.

[0012] Force control applied to rehabilitation [5]: In the field of robotic rehabilitation, specific force control techniques have been developed to aid in the recovery from injuries and physical disabilities. These systems adapt the assistance provided by the robot according to the patient's needs and abilities.

[0013] Ongoing research in this field continues to advance towards more robust, adaptable, and intuitive control systems, with the aim of improving the interaction between robots and their environment, as well as their ability to collaborate safely and effectively with humans.

[0014] Patent document WO2019154911-A1 discloses a motion estimation-based control method that employs stochastic, not geometric, methods. Patent document CN116459113-A applies force only in 3-D.

[0015] However, none of the current systems include the 6-dimensional curve geometry (end effector positions and rotations) as a starting point for force control.

[0016] References

[0017] [1] Hogan, Neville. "Impedance control: An approach to manipulation." Proceedings of the 1984 IEEE International Conference on Robotics and Automation. IEEE, 1984.

[0018] [2] Khatib, Oussama, et al. "Control of manipulators in operational space." Robotics research. MIT Press, 1987. 229-244.

[0019] [3] Nakanishi, Jun, et al. "Dynamic surface control of robot manipulators using neural network approximation." IEEE Transactions on Industrial Electronics 47.1 (2000): 230-239.

[0020] [4] Siciliano, Bruno, et al. "A new approach to force control for manipulators." Journal of Robotic Systems 5.2 (1988): 229-249.

[0021] [5] Marchal-Crespo, Laura, et al. "Neurorehabilitation robotics: Current status and future perspectives for improving function recovery after stroke." Journal of Neuroengineering and Rehabilitation 14.1 (2017): 1-22.

[0022] [6] Montesino, I. et al, “Extending Piecewise Bézier Fitting Methods to SO(3) and SE(3) for Robot Paths”, Junio 2023.

[0023] [7] Schneider, Philip. “An Algorithm for Automatically Fitting Digitized Curves” from "Graphics Gems", Academic Press, 1990.

[0024] [8] Peter E. Jupp et al. Fitting Smooth Paths to Speherical Data, 1987.

[0025] [9] Pierre-Yves Gousenbourger et al.; Data fitting on manifolds with composite Bézier-like curves and blended cubic splines; 2019.

[0026]

[0010] Chafik Samir et al.; A Gradient-Descent Method for Curve Fitting on Riemannian Manifolds; 2012.

[0027]

[0011] Ott, C, “Cartesian Impedance Control of Redundant and Flexible-Joint Robots”. Springer Tracts in Advanced Robotics, vol 49.

[0028] Description of the invention

[0029] The invention relates to a computer-implemented system and method for force control of robotic manipulators with at least six degrees of freedom. The method simulates the existence of a virtual guide in six dimensions (three positional and three rotational), facilitating the repetition of movements in 6-D, that is, considering not only the translational positions of the effector but also the rotational positions. This is especially useful in the field of robotic rehabilitation manipulators, allowing the creation of a personalized therapy machine for each patient and exercise.

[0030] A first aspect of the present invention relates to a method for force control of robotic manipulators with at least six degrees of freedom. The method comprises initializing the position of a virtual guide to a predetermined initial point of a spline associated with points of a special Euclidean group SE(3), including a translational position and a rotational position, and iteratively:

[0031] Obtain a translational position and a rotational position of an end effector of a robotic manipulator.

[0032] Obtain a vector from the translation position of the spline in the virtual guide to the translation position of the end effector, and project that vector in the direction tangent to the spline at the position of the virtual guide.

[0033] - Update the position and speed of the virtual guide on the spline based on the projected vector. Obtain a reference system centered on the position of the virtual guide with one axis tangent to the spline and two axes normal to the spline.

[0034] Calculate a rotational distance from the end effector's rotational position to the spline's rotational position in the virtual guide.

[0035] Calculate a translational distance on the normal axes from the translational position of the end effector to the translational position of the spline in the virtual guide.

[0036] Calculate, using respectively the rotational distance and the translational distance on the normal axes, a Cartesian impedance force in rotation and a Cartesian impedance force in translation on the normal axes with the axes of the reference system as the principal axes of the impedance.

[0037] - Apply the Cartesian impedance forces in rotation and translation to the robotic manipulator.

[0038] A second aspect of the present invention relates to a force control system for robotic manipulators with at least six degrees of freedom. The system includes a control unit and an actuator configured to implement the method described above. A third aspect of the present invention relates to a robotic manipulator with at least six degrees of freedom that incorporates said force control system.

[0039] A fourth aspect of the present invention relates to a program product comprising program instruction means for carrying out the described method when the program is executed on a processor. A fifth aspect of the present invention relates to a program support medium for storing said program product.

[0040] The present invention presents, among other novel aspects, the extension of impedance control to 6-dimensional curves, and the creation of splines from patient movement trajectories to generate said control.

[0041] Brief description of the drawings

[0042] Next, a series of drawings are briefly described that help to better understand the invention and that are expressly related to an embodiment of said invention that is presented as a non-limiting example thereof.

[0043] Figure 1 shows a robotic manipulator with n joints on which the force control of the present invention is applied.

[0044] Figure 2 shows the different positions and rotations of the end effector operated by a user along a path.

[0045] Figure 3 illustrates a flowchart of a force control method for robotic manipulators according to one embodiment.

[0046] Figures 4A-4C show the obtaining of the spline in a previous configuration stage.

[0047] Figure 5 shows another embodiment of the robotic manipulator force control method.

[0048] Figure 6 represents the initialization stage of the virtual guide position.

[0049] Figures 7 and 8 show the obtaining of the projection, in the direction tangent to the spline, of the vector that joins the virtual guide with the end effector.

[0050] Figures 9A, 9B, 9C, and 9D illustrate successive iterations of the robotic manipulator force control that make the virtual guide follow the end effector along the spline.

[0051] Figure 10 shows a reference system centered on the virtual guide.

[0052] Figure 11 shows the obtaining of the rotational distance from the rotation position of the end effector to the rotation position of the spline at the virtual guide point.

[0053] Figure 12 shows the obtaining of the translation distance on the normal axes from the end effector to the virtual guide.

[0054] Figure 13 illustrates the calculation of the Cartesian impedance force on the tangent axis in an assisted motion.

[0055] Figure 14 illustrates the calculation of the Cartesian impedance force on the tangent axis in a resistive motion. Figure 15 illustrates the total resultant force in an assisted motion.

[0056] Figure 16 shows the resulting force and torque vector from the linear and rotational components.

[0057] Figure 17 shows the application of torques to the motors of the robotic manipulator joints.

[0058] Figure 18 shows a force control system for robotic manipulators with at least six degrees of freedom according to one embodiment.

[0059] Detailed description of the invention

[0060] The method of the present invention begins with recording a trajectory traced by a user's limb, which is gripped directly or via some type of splint to the end effector of the robotic manipulator. The user could be, for example, a patient in a muscle rehabilitation session, in the case of robotic rehabilitation manipulators. This trajectory is then processed to become a spline, or parametric curve.

[0061] During repetition of the trajectory by this or another user, a modified impedance algorithm generates an attractive force toward the nearest point on the curve, making the repetition as similar as possible to the recorded trajectory. Additionally, a resistive friction force is generated along the curve, or a support force, to assist movement. This results in a mechanism adapted to the exact trajectory required for each exercise and user.

[0062] The robotic manipulator must have at least six degrees of freedom, allowing it to freely modify the translational and rotational position of its end effector. There are no restrictions on the type of actuators it may have, as long as they enable the described movements. The robotic manipulator's sensors must allow it to measure and apply forces at the end effector. This includes manipulators with sensorized degrees of freedom that measure torque, as well as manipulators with an added force-torque sensor at the end effector.

[0063] Figure 1 shows a generic robotic manipulator 1 with n joints or degrees of freedom, where n > 6, to which the force control system and method of the present invention can be applied. The robotic manipulator 1 has a static base 2 to which a series of links are connected by joints 3, granting it freedom of movement (a torque is applied to each joint). The end effector 4 of the robotic manipulator 1 is manipulated (displacement and rotation) by a user's upper or lower limb, either directly or via some type of splint. The method simulates, using a parameterized curve with Lie groups, a mechanism that only allows the end effector 4 of the robotic manipulator 1 to move in one direction of translation and rotation.

[0064] In a preliminary setup stage, a user grasps the end effector or sensorized handle of the robotic manipulator, either by their own strength if they maintain a firm grip, or by using the sensorized splint that allows the patient's forearm to be attached. The sensor on the handle or splint detects contact with the user at all times, enabling the manipulator to react accordingly. Thus, the robotic manipulator detects the grip and compensates for its own gravity, allowing a therapist to freely move the patient, in the case of robotic rehabilitation manipulators, along a path defined by a specific exercise. Upon completion of this setup exercise, an ordered list of multiple (e.g., thousands) locations in 6D space (translation position and handle orientation) through which the end effector has passed will have been generated.These thousands of location captures define a trajectory, and are converted into a spline 5, as shown in Figure 2. As can be seen in the image, the trajectory made by the user is described by the translation and rotation position of the end effector 4.

[0065] Along this trajectory, defined as spline 5, the existence of two "virtual springs" is simulated. The first virtual spring is perpendicular to the curve and pushes the end effector 4 towards it, restricting the user's movement to that of the trajectory. When the user interacts with the system, the controller receives disturbances that affect both the translational position and the orientation of the end effector 4, so any disturbance affecting one also affects the other. Just as applying a vertical force along a diagonal line produces a displacement in both the direction of the force and the perpendicular to follow the diagonal trajectory, a translational displacement produces a rotational force to follow the trajectory, and vice versa. The second virtual spring travels along the axis of the curve (it is aligned with the longitudinal axis of spline 5 that defines the trajectory) and assists the user in performing the movement.Alternatively, in the case of resistive therapy, instead of a spring facilitating movement, a frictional force would be applied in the direction of the trajectory. Figure 3 shows the flowchart of a method 100 for force control of robotic manipulators 1 according to one embodiment. The method comprises initializing 102 the position of a virtual guide to an initial point of a predetermined spline, generated in a previous configuration stage. The spline is associated with points of a special Euclidean group SE(3) that include a translational (3D) position and a rotational (3D) position.

[0066] Next, a series of steps are executed iteratively. In particular:

[0067] Obtain 104 a translation position and a rotation position of an end effector 4 of a robotic manipulator.

[0068] Obtain 106 a vector from the translation position of the spline in the virtual guide to the translation position of the end effector 4, and project said vector in the tangent direction to the spline at the position of the virtual guide.

[0069] - Update 108 the position and speed of the virtual guide based on the projected vector.

[0070] Obtain 110 a reference system centered on the position of the virtual guide with one axis tangent to the spline and two axes normal to the spline.

[0071] Calculate 112 a rotational distance from the rotational position of the end effector 4 to the rotational position of the spline in the virtual guide.

[0072] Calculate 114, a translational distance along the normal axes from the translational position of end effector 4 to the translational position of the spline on the virtual guide. To do this, a vector is obtained from end effector 4 to the virtual guide, and this vector is projected onto the plane perpendicular to the spline on the virtual guide, obtaining a translational distance along each of the two axes normal to the spline.

[0073] Calculate a Cartesian rotational impedance force and a Cartesian translational impedance force about the normal axes, using the reference frame axes as the principal impedance axes. The Cartesian rotational impedance force is obtained as a function of the rotational distance. The Cartesian translational impedance force about the normal axes is also obtained as a function of the translational distance. At this stage, a Cartesian translational impedance force about the axis tangent to the spline is also calculated, although this force can be obtained in multiple ways, as will be explained later.

[0074] - Apply 118 Cartesian impedance forces in rotation and translation to the robotic manipulator, transforming them into joint torques 3 applied to the robotic manipulator 1.

[0075] In each iteration, a certain time interval is advanced (for example, a few milliseconds) and the torques to be applied to the joints 3 of the robotic manipulator 1 are recalculated, thus forcing the user to faithfully repeat the trajectory made in the previous 6D configuration stage (3 translation positions and 3 rotation positions).

[0076] Figure 4A shows the steps of the preliminary configuration stage, which generates the spline 5 shown in Figure 2. The configuration stage 130 includes recording, during the execution of a specific movement or exercise by a user's limb manipulating the end effector 4, translational and rotational positions of the end effector 4 as SE(3) points. The translational and rotational positions of the end effector 4 are received from the robotic manipulator 1 itself; specifically, they are captured by the robotic manipulator 1's joint position sensors, and the poses in the special Euclidean group SE(3), which includes the three recorded translations and rotations, are obtained using forward kinematics. This generates an ordered list of points along the movement path. Figure 4B represents an ordered sequence of N translational and rotational positions {Pi,Ri; P2,R2; ...; PN,RN} of the end effector 4 captured during the exercise or configuration movement at different times, for example every few milliseconds.

[0077] The configuration step 130 also includes generating a spline 5 from these SE(3) points. The resulting spline 5, shown in Figure 4C, approximates the captured path points as accurately as possible. This spline, defined by a function f(u), identifies values ​​from 0 to 1 (or from 0 to a maximum value UM) with points on the curve: f(u): ue IR -> T e SE( )

[0078] To obtain the spline 5, there are different nonlinear optimization methods, such as the one defined in [6], which uses Lie groups. To obtain curves based on trajectories, the algorithm described in [7] is preferably used, employing differential geometry to extend it to curves in nonlinear spaces, although other known algorithms can be used, such as those published in [8]-

[0010] . The flowchart in Figure 5 shows in more detail a possible implementation of the force control method 100 for robotic manipulators.

[0079] The step of initializing the position of the virtual guide 6 at the beginning of spline 5 is shown in Figure 6. Spline 5 was previously generated in the configuration step 130 and may be stored in memory, so that in the initialization process, method 100 may include accessing memory to retrieve spline 5. The virtual guide 6 is a point on spline 5 (only the position component u) that represents the mechanism moving along the path, that is, the end effector 4. The state of the virtual guide 6 is defined by two real numbers u and u, which represent its position and velocity in the one-dimensional space of the spline. During initialization, the position of the virtual guide 6 is defined as u=0, that is, the initial point of spline 5. During the exercise, the virtual guide 6 will move along spline 5 with increasing values ​​of u, until it reaches a maximum value u=u MFor example 1. Each value of u in spline 5 corresponds to specific translation and rotation position values ​​of the end effector 4 corresponding to the configuration stage 130.

[0080] Next, the vector 8 from the virtual guide 6 to the end effector 4 of the robotic manipulator 1 is obtained (Figure 7), that is, the distance vector between the virtual guide 6 and the end effector 4 of the robotic manipulator 1. The vector 8 is projected 107 onto the tangent direction d t to spline 5 at the point of the virtual guide 6 (Figure 8) and its magnitude e (module of the projected vector 9 on the tangent d) is used t of the spline) to update the parameters (u,ú) of the virtual guide for the next iteration (instant t+1) by means of the following function, where At is the time distance between consecutive iterations t+1 and t, and where the virtual mass m, the elasticity constant ky, and the damping coefficient They are chosen to ensure the mechanism is stable throughout the iterations: u t+1 = u t + ú t ■ At

[0081] The position and velocity of the virtual guide are updated as a function of the projected vector. Figures 9A, 9B, 9C, and 9D illustrate how successive iterations (t, t+i, t+j, t+k) of the approximation step cause the virtual guide 6 to follow the end effector 4 along the spline 5. In the next step, a reference frame centered on the virtual guide 6 is obtained (Figure 10), using, for example, the Frenet-Serret equations. This reference frame consists of a first axis and T in the tangent direction d t to spline 5 and two axes normal to the tangent direction (a second axis 6NI in the direction of the curvature d c and a third axis (N2 cross product of the first and second axes).

[0082] The rotational component and the linear or translational component of the forces to be applied to the robotic manipulator 1 are calculated below.

[0083] With respect to the rotational component, the rotational distance DR from the rotational position R is calculated as 112 e t from end effector 4 to rotation position R t of spline 5 at the virtual guide point 6 (Figure 11). In a novel approach, the rotation component of the Cartesian impedance is unified in its application to splines. To achieve this, the rotational distance not to the rotation position R is calculated. o not from a target 10 (corresponding to a future position of the virtual spline guide), but to the rotation position R t corresponding to the point of spline 5 where the virtual guide 6 is currently positioned.

[0084] Figure 11 shows a graphical representation of the evolution of the spline's rotation component along the path, including the rotation position R. t for the current time t. Each point on sphere 11 represents a specific orientation or rotational position (3 rotations). Sphere 11 also shows the rotational position Ref of the end effector 4 at the current time t, and the resulting rotational distance DR, which is a distance vector defining the rotational distance between the rotational positions R. e fy Rt (the components of the rotating distance vector represent the axis of rotation between one orientation and another, and the magnitude of the components is the rotation in radians around the corresponding axis).

[0085] A Cartesian impedance force in rotation is then calculated as a function of this rotational distance DR, obtaining a torque vector T¡(r rx ,Try ,T rz ).

[0086] To do this, a modified Cartesian impedance is calculated. In the traditional Cartesian impedance, linear elasticity constants k are defined. x , k y , k z (N / m) and rotational elasticity k rx , k ry , k rz (N m / Rad) with which a matrix is ​​constructed:

[0087] The matrix is ​​multiplied by the error vector e composed of the linear distance errors e x , and y , and z and the errors in rotation and rx , and ry , and rz . The rotational error vector (e rx ,and ry and rz ) is obtained through the operation RrefRr = R <uff>where R ref is the rotation matrix of the reference and R r the robot's end-effector rotation matrix. This matrix is ​​transformed into the rotational error vector (e rx ,and ry and rz ) through:

[0088] To obtain the modified Cartesian impedance, a damping matrix D is multiplied by the vector of the differential of the errors é {é x , é y , é z , é rx , é ry , é rz To obtain the damping matrix D, the method described in

[0011] is followed, using the robot's Jacobian matrix. Finally, both resulting vectors are added to obtain the vector F (Cartesian impedance force) containing the forces f x , f y , f z and the torques T rx ,T ry ,T rz Cartesians.

[0089] From the Cartesian impedance force F, the robot torques necessary to generate a force and torque vector w ("wrench") at the end effector 4 are obtained using the inverse robot dynamics, T N = InvDyn(F), where T N is the vector of N torque pairs {^i, -> ^n} corresponding to the N degrees of freedom of the robotic manipulator 1 .

[0090] The present impedance method on curves builds on the Cartesian impedance using the reference systems and distances obtained from the spline as follows: ) The errors are calculated with respect to the position and rotation given by the spline 5 at the point where the virtual guide 6 is located:

[0091] Where p r is the position of the end effector 4 of the robotic manipulator 1, p v It is the position of the virtual guide 6, R r is the orientation of the robot's end effector, R v is the rotation given by spline 5 at the virtual guide point 6, and rotdiff is the rotation difference process described above, thus obtaining the six-dimensional error vector e. These errors are expressed in the canonical x, y, z reference system. The errors expressed on the axes of the virtual guide reference system are obtained. Let R be g The matrix that describes the rotation that transforms the canonical reference system into that of the virtual guide is defined by the following six-dimensional change-of-basis matrix:

[0092] The error vector and the differential error vector become:

[0093] 6 V = G T 1 e é v = G T 1 and

[0094] And the Jacobian matrix J used to obtain the damping matrix D is modified as follows, obtaining ] v .

[0095] Jv = Gr 1 ]

[0096] Using the matrix ] v the damping matrix D is obtained v expressed on the axes of the virtual guide's reference system.

[0097] Finally, the vector of torques and forces is obtained in the reference system of the guide F v , and by means of the matrix G T The equivalent vector F in the canonical reference frame is obtained. Using the robot's inverse dynamics, this vector F is transformed into pairs T. N of the robot. 1 Ks + D 6 = 1 F

[0098] F = G T F V T N = InvDyn(F)

[0099] In this equation K no longer represents the elasticities on the x,y,z axes but on the e axes T (tangential to the curve), 6NI ye N 2 (normal to the curve).

[0100] 3) In the case of guiding force, the first value of e v (which represents the distance with the guide on the tangential axis) is replaced by the distance along spline 5 between u v (the value of the parameter corresponding to the virtual guide) yu t (the value of the target position parameter at that instant):

[0101] In the case of resistive force, the value of F is substituted V1 , which corresponds to the force on the tangential axis, therefore V1 ■ d , where is the chosen friction constant (N / (m / s)).

[0102] Obtaining the linear or translational component is divided into two parts: the normal component and the tangential component. To obtain the normal translational component, the translational distance along the normal axes from the end effector 4 to the virtual guide 6 is calculated by projecting the vector 13 from the virtual guide 6 to the end effector 4 onto the plane perpendicular to the spline 5 at the position of the virtual guide 6 (Figure 12). A Cartesian impedance force in translation along the two axes normal to the spline is then calculated as a function of the projected vector 14 (normal distances d N1 yd Nz ). Of the normal distances (d N1 , d Nz ) forces are obtained on the normal axes (f N1 , f N2l ), using for example the Cartesian impedance algorithm described in

[0011] , which employs Hooke's Law (f NÍ = -k N ■ d N - c N ■ d N¡ ) to calculate the force that a damped spring would exert in that position. Each of the forces (f N1 , f N2l ) is calculated by a product of the respective distance (d N1 , d Nz ) by a normal translational elasticity constant k N (obtained by hand or adjusted automatically by a machine) and the product of its derivative (d N1 , d Nz ) by a damping value in normal translation c N which is calculated to ensure stable control. The tangential component of the translational Cartesian impedance force can be obtained in various ways. The method may include checking a selection of the type of force (elastic or frictional) to be applied to the virtual guide, depending on whether a trajectory-assisting force (elastic force) or a resistive force (frictional force) is desired in the tangential direction. The selection can be defined, for example, through a user interface. Alternatively, the type of force to be applied can be predefined.

[0103] In the case of wanting to perform an assistance movement in which the user is helped to move the end effector 4 of the robotic manipulator 1 to a target 10, the distance along the spline 5 from the virtual guide 6 to the target 10 is calculated (tangential distance d, Figure 13). The target 10 is defined as a u value obj (t) of the spline that should be reached at time t. The method may include defining one or more objectives 10 to be met at different times. The definition of these objectives can be based, for example, on some input parameter (e.g., exercise execution speed). Alternatively, or additionally, in configuration step 130, different target values ​​can be defined along the path that the user must follow during the exercise replication; for example, at time t=2s, objective 10 is set at the midpoint of the path (u=0.5), and at time t=3.25s, objective 10 is set at the end of the path (u=1), which would force the user to execute the second part of the path faster than the first half. The definition of the objectives is therefore closely linked to the desired execution speed of the exercise in different sections of it.

[0104] Next, the Cartesian impedance force f is calculated. dt on the tangent axis using the tangential distance d, defined by d = u obj - u. If in the example in Figure 13 u ob j=0.55 yu=0.43, then d=0.12. For this, the Cartesian impedance algorithm described in

[0011] can be used. This control uses Hooke's Law (f dt = -k dt ■ d - c dt ■ the) to calculate the force that a damped spring would exert in that position. The force on the tangent axis f dt It is calculated by a product of the distance d and a tangential translational elasticity constant k dt (obtained by hand or automatically adjusted) and the product of its derivative d by a tangential translation damping value c dt which is calculated to ensure stable control.

[0105] If resistive force is to be applied, friction is calculated (Figure 14). To do this, the velocity v of the end effector 4 is obtained and projected onto the tangent direction d. t to spline 5 at the position of the virtual guide 6, and a force f is calculated as 146 dt on the tangent axis opposite to said projected velocity v dt multiplied by a friction parameter (f dt =-nv dt ).

[0106] Once the Cartesian forces or impedances on the normal axes (f) have been calculated N1 yf N2 , step 120) and on the tangential axis (f dt , step 144 or 146), all these forces are added together to obtain the resultant force f t (Figure 15, corresponding to the case of elastic assistance force with a target 10).

[0107] Next, the force vectors ft(f) are concatenated x ,F y ,F z ) and torque T¡(r rx ,T ry ,T rz ') (Figure 16), and a force and torque vector w ("wrench") is obtained, which is the resultant of the linear and rotational components:

[0108] Finally, the force and torque vector w is transformed into torques The force and torque vector w is converted from joints 3 and applied to robotic manipulator 1. In the example in Figure 17, robotic manipulator 1 has seven joints and, therefore, seven degrees of freedom. Using the inverse dynamics of robotic manipulator 1, the force and torque vector w is converted into an actuation of the motors of robotic manipulator 1, allowing the simulation of impedance behavior on curves.

[0109] Figure 18 illustrates a schematic of the force control system 60 for robotic manipulators with at least six degrees of freedom according to one embodiment. The force control system 60 comprises a control unit 60 configured to receive information from the robotic manipulator 1 (including the iterative reception of the translational position P1 and rotational position R1 of the end effector 4) and to perform the calculations of the Cartesian impedance forces to be applied thereto, according to the steps described above. In the embodiment shown in Figure 18, the control unit 60 comprises a data processing unit (e.g., a CPU 64) and a memory 66 where, among other things, the spline 5 (obtained previously in the configuration stage) and the data used for the calculations are stored.

[0110] An actuator 68 is responsible for applying the calculated Cartesian impedance forces to the robotic manipulator 1, converting them into torques { 1, 2, applied to the motors 70 of joints 3. Alternatively, this conversion to torques can be performed instead by the robotic manipulator 1 itself (in that case, the actuator 68 would directly send the force and torque vector w to the robotic manipulator 1, which converts it into the torques In one embodiment, the control unit 62 is configured to record as SE(3) points a plurality of translation (Pi; P2; PN) and rotation (R1; R2; RN) positions of the end effector 4 during the execution of a movement in a previous setup stage (130), generate the spline 5 from said SE(3) points; and store the spline 5 in memory 66. Alternatively, the control unit 62 can receive the spline 5 and store it in memory 66 (said spline 5 may have been previously generated by a different data processing unit, using data obtained during the execution of an exercise on the same robotic manipulator 1 or on a similar one).

[0111] The force control system 60 can be incorporated internally in the robotic manipulator 1 itself or be, as shown in the example in Figure 18, an external element to it, in communication (wired or wireless) through a communication interface (not shown in the figure).< / uff>

Claims

CLAIMS 1. A force control method for robotic manipulators with at least six degrees of freedom, characterized in that it comprises initializing (102) the position (u) of a virtual guide (6) to a predetermined initial point of a spline (5) associated with points of a special Euclidean group SE(3) including a translation position and a rotation position, and iteratively: obtaining (104) a translation position (P1) and a rotation position (R1) of an end effector (4) of a robotic manipulator (1); obtaining (106) a vector from the translation position of the spline (5) in the virtual guide (6) to the translation position (P1) of the end effector (4), and projecting said vector in the tangent direction (d f) to the spline (5) at the position of the virtual guide (6); update (108) the position (u) and velocity (ú) of the virtual guide (6) on the spline (5) as a function of the projected vector; obtain (110) a reference system centered at the position (u) of the virtual guide (6) with a tangent axis (er) to the spline (5) and two normal axes (6NI, e N 2) to spline (5); calculate (112) a rotation distance (DR) from the rotation position ( e / ) from the end effector (4) to the rotation position (R t ) of the spline (5) in the virtual guide (6); calculate (114) a translational distance (d N1 , d Nz ) on the normal axes from the translation position of the end effector (4) to the translation position of the spline (5) in the virtual guide (6); calculate (116), using the rotation distance (DR) and the translation distance (d N1 , d Nz) on the normal axes (6NI , eN2), a Cartesian impedance force in rotation (f ) and a Cartesian impedance force in translation (f N1 , f N2l ) on the normal axes (6NI , 6N2) with the axes of the reference system as the principal axes of the impedance; and apply (118) the Cartesian impedance forces in rotation and translation to the robotic manipulator (1).

2. The method of claim 1, characterized in that the spline (5) is obtained in a configuration step (130) comprising: registering (132) as SE points(3) a plurality of translation (Pi; P2; ... ; PN) and rotation (R1; R2; ... ; RN) positions of the end effector (4) during the execution of a movement; and generating (134) the spline (5) from said SE points(3).

3. The method according to any of the preceding claims, characterized in that it comprises calculating a Cartesian impedance force in translation (f dt) on the tangent axis (er) to the spline (5).

4. The method according to claim 3, wherein calculating a Cartesian impedance force in translation (f dt ) on the tangent axis (er) comprises: obtaining a target position (u obj ) of the virtual guide (6) for the current time (t); calculate (142) a distance (d) along the spline (5) from the position (u) of the virtual guide (6) to the target position (u obJ ); and calculate (144) the Cartesian impedance force in translation (f dt ) on the tangent axis (er) using said distance (d).

5. The method according to claim 3, wherein calculating a Cartesian impedance force in translation (f dt ) on the tangent axis (er) comprises: obtaining the velocity (v dt ) of the end effector 4 on the tangent axis (er); and calculate (146) the Cartesian impedance force in translation (f dt) on the tangent axis (er) as a force opposite to said velocity (v dt ) multiplied by a friction parameter W- 6. A force control system for robotic manipulators with at least six degrees of freedom, characterized in that it comprises: a control unit (62) configured to: initialize the position (u) of a virtual guide (6) to a predetermined initial point of a spline (5) associated with points of a special Euclidean group SE(3) including a translation position and a rotation position, and iteratively: obtain a translation position (P1) and a rotation position (R1) of an end effector (4) of a robotic manipulator (1); obtain a vector from the translation position of the spline (5) in the virtual guide (6) to the translation position (P1) of the end effector (4), and project said vector in the tangent direction (d f) to the spline (5) at the position of the virtual guide (6); update the position (u) and velocity (ú) of the virtual guide (6) on the spline (5) as a function of the projected vector; obtain a reference system centered on the position (u) of the guide virtual (6) with a tangent axis (er) to the spline (5) and two normal axes (6NI , e N 2) to the spline (5); calculate a rotational distance (DR) from the rotational position (Ref) of the end effector (4) to the rotational position (R t ) of the spline (5) in the virtual guide (6); calculate a translational distance (d N1 , d Nz ) on the normal axes from the translation position of the end effector (4) to the translation position of the spline (5) in the virtual guide (6); calculate, using the rotation distance (DR) and the translation distance (d N1 , d Nz) on the normal axes (6NI , eN2), a Cartesian impedance force in rotation (f^) and a Cartesian impedance force in translation ( vi. v2>) en l° s normal axes (6NI, e N 2) with the axes of the reference system as the main axes of the impedance; and an actuator (68) configured to apply the Cartesian impedance forces in rotation and translation to the robotic manipulator (1).

7. The system of claim 6, wherein the control unit (62) is configured to: register as SE points(3) a plurality of translation (Pi; P2; ; PN) and rotation (R1; R2; ... ; RN) positions of the end effector (4) during the execution of a movement in a setup step (130); generate the spline (5) from said SE points(3); and store the spline (5) in a memory (66).

8. The system according to any of claims 6 to 7, characterized in that the control unit (62) is configured to calculate a translational Cartesian impedance force (f dt ) on the tangent axis (er) to the spline (5).

9. The system according to claim 8, wherein the control unit (62) is configured to: obtain a target position (or obj ) of the virtual guide (6) for the current time (t); calculate a distance (d) along the spline (5) from the position (u) of the virtual guide (6) to the target position (or obJ ); and calculate the Cartesian impedance force in translation ( / dt ) on the tangent axis (er) using said distance (d).

10. The system according to claim 8, wherein the control unit (62) is configured to: obtain the speed (v dt ) of the end effector 4 on the tangent axis (er); and calculate the Cartesian impedance force in translation ( / dt ) on the tangent axis (er) as a force opposite to said velocity (v dt ) multiplied by a friction parameter ( / z).

11. A robotic manipulator with at least six degrees of freedom, incorporating the force control system (60) of any of claims 6 to 10.

12. A program product comprising program instruction means for carrying out the method of any of claims 1 to 5 when the program is executed on a processor.

13. A program support medium, which stores the program product according to claim 12.

Images