A method and device for controlling the telecentric motion of a serial robot arm

Abstract

The present disclosure relates to the technical field of robot motion control, and provides a telecentric motion control method and device for a serial mechanical arm. The method comprises the following steps: determining a telecentric motion range of a tool center point of an end tool of the serial mechanical arm; acquiring current pose information of the tool center point and an external resultant force applied to the end tool; calculating a target position coordinate and a target attitude angle of the tool center point in the telecentric motion range according to the current pose information and the external resultant force; and performing motion control on each joint of the serial mechanical arm according to the target position coordinate and the target attitude angle, so as to realize telecentric motion control of the serial mechanical arm. The present disclosure not only can realize accurate control of telecentric motion of the serial mechanical arm, expand the application range of the serial mechanical arm, but also can simplify the complexity of attitude control calculation, reduce the calculation amount, and reduce the cost of computing resources.

Description

Technical Field

[0001] This disclosure relates to the field of robot motion control technology, and in particular to a method and apparatus for controlling the telecentric motion of a serial robotic arm. Background Technology

[0002] Motion control of collaborative robotic arms involves various forms of telecentric motion. Currently, most telecentric motion control for collaborative robotic arms employs telecentric mechanisms, achieving control of the telecentric point through mechanical structure. This means manipulating the robotic arm via force control, thereby ensuring the accuracy of the telecentric point.

[0003] However, traditional telecentric motion control is achieved by changing the configuration of the robotic arm to a parallelogram configuration or its derivatives. That is, it relies on the mechanical structure itself to ensure the accuracy of the telecentric point. This not only severely limits the promotion and application of serial robotic arms in force-controlled telecentric motion control, but also makes the attitude control calculation of this method very complex, with a large amount of computation and high cost of computing resources. Summary of the Invention

[0004] In view of this, the present disclosure provides a method and apparatus for controlling the telecentric motion of a serial robotic arm, in order to solve the problem that the prior art achieves this by changing the configuration of the robotic arm to a parallelogram configuration or its derivative configuration, that is, relying on the mechanical structure itself to ensure the accuracy of the telecentric point. This not only seriously limits the widespread application of serial robotic arms in force-controlled telecentric motion control, but also the problem that the attitude control calculation of this method is very complex, the amount of calculation is large, and the required computing power resources are costly.

[0005] A first aspect of this disclosure provides a method for controlling the telecentric motion of a serial robotic arm, comprising:

[0006] Determine the telecentric motion range of the tool center point of the end effector of the tandem robotic arm;

[0007] Obtain the current pose information of the tool center point at the current position, and the external resultant force applied to the end tool of the serial robotic arm. The current pose information includes the current position coordinates and current attitude angle of the tool center point at the current position.

[0008] Based on the current pose information and the external force, calculate the target position coordinates and target attitude angle when the tool center point moves from the current position to the target position within the telecentric motion range;

[0009] Based on the target position coordinates and target posture angle, motion control is performed on each joint of the serial robotic arm to achieve telecentric motion control of the serial robotic arm.

[0010] A second aspect of this disclosure provides a telecentric motion control device for a serial robotic arm, comprising:

[0011] The determination module is configured to determine the telecentric motion range of the tool center point of the end effector of the tandem robotic arm;

[0012] The acquisition module is configured to acquire the current pose information of the tool center point at the current position, and the external resultant force applied to the end tool of the serial robotic arm. The current pose information includes the current position coordinates of the tool center point at the current position and the current attitude angle.

[0013] The calculation module is configured to calculate the target position coordinates and target attitude angle when the tool center point moves from the current position to the target position within the telecentric motion range, based on the current pose information and the external resultant force.

[0014] The control module is configured to perform motion control on each joint of the serial robotic arm based on the target position coordinates and target posture angle, so as to realize the telecentric motion control of the serial robotic arm.

[0015] A third aspect of this disclosure provides an electronic device including a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor executes the computer program to implement the steps of the method described above.

[0016] A fourth aspect of this disclosure provides a computer-readable storage medium storing a computer program that, when executed by a processor, implements the steps of the above-described method.

[0017] Compared with the prior art, the beneficial effects of this disclosed embodiment include at least the following: This disclosed embodiment determines the telecentric motion range of the tool center point of the end effector of the serial robotic arm; obtains the current pose information of the tool center point at the current position, and the external resultant force applied to the end effector of the serial robotic arm, the current pose information including the current position coordinates and current posture angle of the tool center point at the current position; calculates the target position coordinates and target posture angle of the tool center point when it moves from the current position to the target position within the telecentric motion range based on the current pose information and the external resultant force; and performs motion control on each joint of the serial robotic arm based on the target position coordinates and target posture angle to achieve telecentric motion control of the serial robotic arm. Compared with traditional control methods, it does not rely on the mechanical structure itself to ensure the accuracy of the telecentric point, but achieves precise control of the telecentric motion of the serial robotic arm through the above method. This not only expands the application range of serial robotic arms (such as medical collaborative serial robotic arms), but also greatly simplifies the complexity of posture control calculation, reduces the amount of calculation, and lowers the cost of computing resources. Attached Figure Description

[0018] To more clearly illustrate the technical solutions in the embodiments of this disclosure, the accompanying drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the accompanying drawings described below are only some embodiments of this disclosure. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0019] Figure 1 This is a schematic diagram illustrating one application scenario of an embodiment of this disclosure;

[0020] Figure 2 This is a flowchart illustrating a telecentric motion control method for a serial robotic arm provided in an embodiment of this disclosure;

[0021] Figure 3 This is a schematic diagram of the telecentric motion range of a tool center point in the telecentric motion control method for a serial robotic arm provided in this embodiment of the present disclosure;

[0022] Figure 4 This is a schematic diagram of the structure of a telecentric motion control device for a serial robotic arm provided in an embodiment of this disclosure;

[0023] Figure 5 This is a schematic diagram of the structure of an electronic device provided in an embodiment of this disclosure. Detailed Implementation

[0024] In the following description, specific details such as particular system architectures and techniques are set forth for illustrative purposes and not for limitation, so as to provide a thorough understanding of the embodiments of this disclosure. However, those skilled in the art will understand that this disclosure may also be implemented in other embodiments without these specific details. In other instances, detailed descriptions of well-known systems, apparatuses, circuits, and methods have been omitted so as not to obscure the description of this disclosure with unnecessary detail.

[0025] The following will describe in detail, with reference to the accompanying drawings, a telecentric motion control method and apparatus for a serial robotic arm according to an embodiment of the present disclosure.

[0026] Figure 1 This is a schematic diagram illustrating an application scenario of an embodiment of this disclosure. The application scenario may include a serial robotic arm 101 and a controller 102 connected to the serial robotic arm 101.

[0027] The serial robotic arm 101 can be a linkage mechanism including multiple joints 1011 and an end effector 1012. The center point of the end effector 1012 is its tool center point.

[0028] The serial robotic arm 101 can be connected to the controller 102 via a wired connection or a wireless connection. Wired connections include, but are not limited to, wired networks using coaxial cables, twisted pairs, and fiber optic cables; this embodiment does not limit the types of connections used. Wireless connections include, but are not limited to, Bluetooth, Near Field Communication (NFC), and Infrared; this embodiment does not limit the types of connections used.

[0029] The controller 102 can be a server that provides various services, such as a backend server that receives requests sent by the serial robotic arm 101 with which it has established a communication connection. This backend server can receive and analyze the requests sent by the serial robotic arm and generate processing results. The server can be a single server, a server cluster consisting of several servers, or a cloud computing service center. This embodiment of the disclosure does not limit this.

[0030] It should be noted that the server can be either hardware or software. When the server is hardware, it can be various electronic devices that provide various services to the serial robotic arms. When the server is software, it can be multiple software programs or software modules that provide various services to the serial robotic arms, or it can be a single software program or software module that provides various services to the serial robotic arms. This disclosure does not limit the scope of the embodiments.

[0031] In one embodiment, the method of this disclosure can be executed by a controller 102. The specific execution process is as follows: The controller 102 can first determine the telecentric motion range of the tool center point of the end effector of the serial robotic arm; then obtain the current pose information of the tool center point at the current position, and the external resultant force applied to the end effector of the serial robotic arm. The current pose information includes the current position coordinates and current posture angle of the tool center point at the current position; based on the current pose information and the external resultant force, calculate the target position coordinates and target posture angle when the tool center point moves from the current position to the target position within the telecentric motion range; based on the target position coordinates and target posture angle, perform motion control on each joint of the serial robotic arm to achieve telecentric motion control of the serial robotic arm. Compared with traditional control methods, the technical solution of this disclosure does not rely on the mechanical structure itself to ensure the accuracy of the telecentric point, but achieves precise control of the telecentric motion of the serial robotic arm through the above method. This not only expands the application range of serial robotic arms (such as medical collaborative serial robotic arms), but also greatly simplifies the complexity of posture control calculation, reduces the amount of calculation, and lowers the cost of computing resources.

[0032] In another embodiment, the method disclosed herein can be executed by a control chip (such as a microcontroller) embedded in a serial robotic arm. Except for the execution entity, the execution flow is essentially the same as that of the controller described above, and therefore will not be repeated here.

[0033] It should be noted that the specific type, quantity and combination of the serial robotic arm 101, controller 102 and the connection between them can be adjusted according to the actual needs of the application scenario, and this disclosure embodiment does not limit this.

[0034] Figure 2 This is a flowchart illustrating a telecentric motion control method for a serial robotic arm provided in an embodiment of this disclosure. Figure 2 The telecentric motion control method of the serial robotic arm can be derived from Figure 1 The controller 102 executes.

[0035] like Figure 2 As shown, the telecentric motion control method of this serial robotic arm includes:

[0036] Step S201: Determine the telecentric motion range of the tool center point of the end effector of the serial robotic arm.

[0037] In one embodiment, combined with Figure 3 First, the telecentric point R and initial point O for the telecentric motion control of the tandem robotic arm can be determined according to actual needs. For example, in hip replacement surgery, the center point of the acetabulum is the telecentric point R, and the initial point O is located on the center line of the acetabulum passing through the telecentric point R. The initial position of the tool centerpoint (TCP point) of the end effector of the tandem robotic arm entering the surgical area (i.e., the telecentric motion range of the tool centerpoint of the end effector of the tandem robotic arm) must fall on the initial point O. The line RO connecting the telecentric point R and the initial point O is used as the baseline for the telecentric motion control of the tandem robotic arm. The position of the initial point O can be flexibly set according to the actual situation, but it is necessary to ensure that the initial point O is within the telecentric motion range.

[0038] In hip replacement surgery, the telecentric motion range (also known as the "telecentric motion region") of the end effector of a tandem robotic arm is typically the region remaining after the removal of the smaller spherical dome from the larger dome vertebral body. The larger dome vertebral body is defined as the point where the apex of the cone coincides with the center of the sphere (i.e., the telecentric point R), and its radius is d. max A cone with a semi-apex angle of θ, and a smaller cone whose vertex coincides with the center of the sphere (i.e., the centroid R) and has a radius of d. min A spherical cone with a semi-apex angle of θ on its conical surface is a solid bounded by a conical surface and a sphere. maxThis indicates the current position of the TCP point of the serial robotic arm within the telecentric motion region (e.g., Figure 3 The maximum distance d between point A and the centroid R in the diagram. min This indicates the current position of the TCP point of the serial robotic arm within the telecentric motion region (e.g., Figure 3 Let θ be the minimum distance between point A and the centroid R, and let θ be the angle between the line RA connecting the centroid R and point A and the line RO connecting the centroid R and the initial point O.

[0039] Step S202: Obtain the current pose information of the tool center point at the current position, and the external resultant force applied to the end tool of the serial robotic arm. The current pose information includes the current position coordinates of the tool center point at the current position and the current attitude angle.

[0040] Combination Figure 3 As an example, assume the tool center point of the tandem robotic arm is at the current position A. Establish a tool coordinate system A-XYZ with A as the origin, and the X-axis of the pose coordinates of point A points to the centroid R. Obtain the current pose information of the tool center point of the tandem robotic arm at point A. This pose information includes the current position coordinates p of point A. A (Including the x, y, and z coordinates of the tool's center point at point A) and the current pose r A (Including the rotation angle r of the tool center point at point A around the AX axis) x Rotation angle r about the AY axis y The rotation angle r about the AZ axis z ).

[0041] To fully determine the state of an object in three-dimensional space, three degrees of freedom in position and three degrees of freedom in orientation are required. The former determines the object's specific location in space, while the latter determines its orientation. The states of the object's six degrees of freedom are collectively referred to as its pose. For a robot, any point in space must be explicitly specified using these six parameters (i.e., the six degrees of freedom). For example, if a robot reaches the same point in space at the same position but with different orientations, it means the robot is reaching the same point with different orientations.

[0042] The external resultant force applied to the end effector of a tandem robotic arm typically refers to the external force acting on the end effector, which is collected by sensors on the end effector of the tandem robotic arm.

[0043] Step S203: Based on the current pose information and the external resultant force, calculate the target position coordinates and target attitude angle when the tool center point moves from the current position to the target position within the telecentric motion range.

[0044] In one embodiment, the admittance control equation can be constructed first based on the current pose information and the external resultant force; then, based on the admittance control equation, the target position coordinates of the target position are calculated when the tool center point moves from the current position to the target position within the telecentric motion range; then, based on the target position coordinates and the current pose information, the target attitude angle of the target position is determined when the tool center point moves from the current position to the target position within the telecentric motion range.

[0045] In some embodiments, the desired positive definite inertia matrix, the desired positive definite damping matrix, and the desired positive definite stiffness matrix can be obtained first; then, the admittance control equation can be constructed based on the desired positive definite inertia matrix, the desired positive definite damping matrix, the desired positive definite stiffness matrix, the current pose information, and the external resultant force.

[0046] Among them, the desired positive definite inertia matrix is ​​the positive definite matrix of the desired inertia coefficient. The desired positive definite damping matrix is ​​the positive definite matrix of the desired damping. The desired positive definite stiffness matrix is ​​the positive definite matrix of the desired stiffness coefficient. The inertia coefficient, damping coefficient, and stiffness coefficient are three commonly used control parameters in robot impedance control.

[0047] As an example, suppose the TCP point of the serial robotic arm is currently located at point A, and the pose coordinates of point A are q. A The target position is denoted as point B, and the pose coordinates of point B are q. B . This represents the pose deviation of the tool center point (i.e., TCP point) when it moves from point A to point B. The formula for calculating the pose deviation is shown in equation (1) below.

[0048]

[0049] In equation (1), p A This represents the position of the TCP point at point A, i.e., the x, y, z coordinates, r A This represents the attitude of the TCP endpoint at point A, i.e., r x ,r y ,r z p B This indicates the location of the TCP endpoint at point B, r B This indicates the TCP endpoint's position at point B.

[0050] pose coordinates of the current position (i.e., point A) Given the target point (i.e., point B), its pose coordinates. These are the parameters to be solved.

[0051] Based on the obtained expected positive definite inertia matrix M d Expected damping positive definite matrix D d Desired stiffness positive definite matrix K dand the external resultant force F applied to the end effector of the tandem robotic arm. ext The admittance control equation is constructed as shown in formula (2) below.

[0052]

[0053] In equation (2), for The first derivative of represents the velocity of the position deviation; for The second derivative of F represents the acceleration of the position deviation; ext It represents the resultant force from the outside.

[0054] Based on the admittance control equation, the target position coordinates can be calculated when the tool center point moves from the current position to the target position within the telecentric motion range. This can include the following steps:

[0055] The admittance control equation is transformed into a transfer function that characterizes the velocity of the position deviation of the tool's center point from its current position to the target position.

[0056] Discretize the transfer function to obtain the target position coordinates when the tool center point moves from the current position to the target position within the telecentric motion range.

[0057] As an example, if only position control is considered, the expected positive definite stiffness matrix K d If the value is 0, then the above formula (2) can be simplified into the following formula (3).

[0058]

[0059] Next, the differential equation of the above formula (2) is transformed into a transfer function that characterizes the velocity of the position deviation of the tool center point from the current position to the target position, as shown in the following formula (4).

[0060]

[0061] In equation (4), This indicates the speed of positional deviation.

[0062] It should be noted that the specific conversion calculation process for transforming differential equations into transfer functions can be found in relevant existing technologies, and will not be elaborated here.

[0063] Then, the velocity of the position deviation. Discretize the data, and the discretization formula is shown in formula (5) below.

[0064]

[0065] In equation (5) above, △t represents the time interval for collecting the current pose information of the serial robotic arm at its current position.

[0066] According to the above formula (5), the target position coordinates of the tool center point can be calculated when it moves from the current position to the target position (i.e., point B) within the telecentric motion range, as shown in the following formula (6).

[0067]

[0068] In some embodiments, the target attitude angle includes the target roll angle, the target pitch angle, and the target yaw angle; the current attitude angle includes the current yaw angle.

[0069] Based on the target position coordinates and current pose information, determine the target pose angle at the target position when the tool center point moves from the current position to the target position within the telecentric motion range. This may include:

[0070] Calculate the target roll angle when the tool's center point moves to the target position based on the target position coordinates;

[0071] Obtain the X-axis torque data when the tool's center point is at its current position;

[0072] Based on the current yaw angle of the tool center point at the current position, the X-axis torque data, and the target roll angle, calculate the target pitch angle and target yaw angle when the tool center point moves to the target position.

[0073] In some embodiments, based on the current yaw angle of the tool center point at the current position, the X-axis torque data, and the target roll angle, the target pitch angle and target yaw angle when the tool center point moves to the target position are calculated, including:

[0074] Based on the current yaw angle of the tool center point at the current position, the X-axis torque data, and the admittance control equation, calculate the target yaw angle when the tool center point moves to the target position;

[0075] Based on the target roll angle and the target yaw angle, calculate the target pitch angle when the tool center point is at the target position.

[0076] In some embodiments, the target pitch angle when the tool center point is at the target position is calculated based on the target roll angle and the target yaw angle, including:

[0077] The target's roll angle and yaw angle are calculated by vector cross product to obtain the target's pitch angle when the tool's center point is at the target's position.

[0078] As an example, the target pose angle r of the tandem robotic arm moving to the target position (i.e., point B) is...d The rotation matrix can be represented as R = {r x ,r y ,r z}

[0079] Combination Figure 3 To ensure that the TCP point (tool center point) of the tandem robotic arm performs telecentric motion within the defined telecentric motion range, the X-axis of the TCP point's pose coordinate system must always point to the telecentric point R. Taking point A as an example, for the TCP point to perform telecentric motion within the defined telecentric motion range, the X-axis of its pose coordinate system at point A must always point to the telecentric point R. Similarly, when the TCP point of the tandem robotic arm moves to point B, the X-axis of its pose coordinate system at point B must always point to the telecentric point R.

[0080] Therefore, the target roll angle (i.e., the angle of rotation around the X-axis) of the TCP point of the serial robotic arm at the target position (i.e., point B) can be calculated as a vector based on the target position coordinates of point B. The target roll angle r when the tool's center point moves to point B can be calculated using the following formula (7). x .

[0081]

[0082] z-axis of the default base coordinate system Within the plane formed by the z-axis and x-axis of the TCP point pose coordinate system, the target yaw angle r of the TCP point pose coordinate system can be calculated using the following formulas (8) and (9). y (i.e., the angle of rotation around the y-axis).

[0083]

[0084] r y =r y / |r y | (9).

[0085] Finally, the target pitch angle r when the tool's center point moves to the target position (point B) can be calculated using the following formula (10). z (i.e., the angle of rotation around the Z-axis).

[0086] r z =r x ×r y (10).

[0087] As another example, the target roll angle r when the tool center point of the tandem robotic arm moves to the target position (point B). xSame as the example above. The difference is the target pitch angle r when the tool center point of the tandem robotic arm moves to the target position (point B). z By acquiring the X-axis torque data of the tandem manipulator at its current position (point A) and the current yaw angle (i.e., the angle of rotation around the Z-axis) within its current attitude angle at that position, and substituting the X-axis torque data and the current yaw angle into the aforementioned admittance control equation, the target pitch angle r when the tandem manipulator moves to the target position (point B) can be calculated. z .

[0088] Next, based on the target roll angle r calculated above... x pitch angle r with respect to the target z , for r x and r z Perform the cross product calculation to obtain the target yaw angle r when the tool's center point moves to the target position (point B). y .

[0089] The force couple or torque that causes a machine component to rotate (including its tendency to rotate) is called rotational torque, or simply torque. X-axis torque data refers to the force couple or torque exerted on the tool center point of a series-connected robotic arm along the X-axis. This data can be acquired by sensors mounted on the robotic arm.

[0090] Furthermore, the rotation matrix R obtained above can be converted into Euler angles, and r can then be calculated. d .

[0091] By combining the above steps, the pose coordinates of the tool center point of the tandem robotic arm as it moves from the current position (point A) to the target position (point B) within the aforementioned telecentric motion range can be calculated.

[0092] Step S204: Based on the target position coordinates and target posture angle, perform motion control on each joint of the serial robotic arm to achieve telecentric motion control of the serial robotic arm.

[0093] Specifically, the joint angles of each joint of the serial robotic arm can be calculated first based on the target position coordinates and target posture angles; then, the motion control of each joint of the serial robotic arm can be performed based on the joint angles to achieve the telecentric motion control of the serial robotic arm.

[0094] In some embodiments, based on a preset inverse kinematics analysis algorithm for robotic arms, the joint angles of each joint of the serial robotic arm are calculated according to the target posture angle and the target position coordinates.

[0095] Specifically, based on the steps described above, the target attitude angle and target position coordinates of the tool's center point are calculated, i.e. The joint angles of each joint of the serially connected robotic arm can be calculated further using existing inverse kinematics analytical algorithms for robotic arms.

[0096] As an example, the joint angles mentioned above can be further calculated using the velocity Jacobi transformation to obtain the joint position and joint velocity of each joint when the serial robotic arm moves from the current position to the target position within the telecentric motion range. Then, based on the joint position and joint velocity of each joint, the telecentric motion control of the serial robotic arm can be realized, so that the tool center point of the serial robotic arm can move precisely to the target position and target posture angle.

[0097] By increasing the control frequency, if Δt in the above formula (5) is sufficiently small, the serial robotic arm can be considered to be in a state of telecentric motion. Typically, Δt ≤ 2ms.

[0098] The technical solution provided in this disclosure involves: determining the telecentric motion range of the tool center point of the end effector of a tandem robotic arm; acquiring the current pose information of the tool center point at its current position, and the external resultant force applied to the end effector of the tandem robotic arm, wherein the current pose information includes the current position coordinates and current attitude angle of the tool center point at its current position; constructing an admittance control equation based on the current pose information and the external resultant force; calculating the target position coordinates of the target position when the tool center point moves from its current position to the target position within the telecentric motion range based on the admittance control equation; and determining the position of the tool center point within the telecentric motion range based on the target position coordinates and the current pose information. When moving from the current position to the target position, the target posture angle of the target position is determined; based on the target position coordinates and the target posture angle, the joint angles of each joint of the serial robotic arm are calculated; based on the joint angles, the motion control of each joint of the serial robotic arm is performed to achieve the telecentric motion control of the serial robotic arm. Compared with traditional control methods, it does not rely on the mechanical structure itself to ensure the accuracy of the telecentric point. Instead, it achieves precise control of the telecentric motion of the serial robotic arm through the above method. This not only expands the application range of serial robotic arms (such as medical collaborative serial robotic arms), but also greatly simplifies the complexity of posture control calculation, reduces the amount of calculation, and lowers the cost of computing resources.

[0099] All of the above-mentioned optional technical solutions can be combined in any way to form the optional embodiments of this application, and will not be described in detail here.

[0100] The following are embodiments of the apparatus disclosed herein, which can be used to execute embodiments of the method disclosed herein. For details not disclosed in the apparatus embodiments of this disclosure, please refer to the embodiments of the method disclosed herein.

[0101] Figure 4 This is a schematic diagram of a telecentric motion control device for a serial robotic arm provided in an embodiment of this disclosure. Figure 4As shown, the telecentric motion control device of the tandem robotic arm includes:

[0102] The determination module 401 is configured to determine the telecentric motion range of the tool center point of the end effector of the serial robotic arm;

[0103] The acquisition module 402 is configured to acquire the current pose information of the tool center point at the current position, and the external resultant force applied to the end tool of the serial robotic arm. The current pose information includes the current position coordinates of the tool center point at the current position and the current posture angle.

[0104] The calculation module 403 is configured to calculate the target position coordinates and target attitude angle when the tool center point moves from the current position to the target position within the telecentric motion range, based on the current pose information and the external resultant force.

[0105] The control module 404 is configured to perform motion control on each joint of the serial robotic arm according to the target position coordinates and the target posture angle, so as to realize the telecentric motion control of the serial robotic arm.

[0106] The technical solution provided in this embodiment determines the telecentric motion range of the tool center point of the end effector of the serial robotic arm through a determining module 401; an acquiring module 402 acquires the current pose information of the tool center point at the current position and the external resultant force applied to the end effector of the serial robotic arm, the current pose information including the current position coordinates and current posture angle of the tool center point at the current position; a calculating module 403 calculates the target position coordinates and target posture angle of the tool center point when it moves from the current position to the target position within the telecentric motion range based on the current pose information and the external resultant force; and a control module 404 performs motion control on each joint of the serial robotic arm based on the target position coordinates and target posture angle to achieve telecentric motion control of the serial robotic arm. Compared with traditional control methods, it does not rely on the mechanical structure itself to ensure the accuracy of the telecentric point, but achieves precise control of the telecentric motion of the serial robotic arm through the above method. This not only expands the application range of serial robotic arms (such as medical collaborative serial robotic arms), but also greatly simplifies the complexity of posture control calculation, reduces the amount of calculation, and lowers the cost of computing resources.

[0107] In some embodiments, the computing module 403 may include:

[0108] The building unit is configured to construct admittance control equations based on the current pose information and the external resultant force;

[0109] The calculation unit is configured to calculate the target position coordinates of the target position when the tool center point moves from the current position to the target position within the telecentric motion range, based on the admittance control equation.

[0110] The determining unit is configured to determine the target pose angle of the target position when the tool center point moves from the current position to the target position within the telecentric motion range, based on the target position coordinates and the current pose information.

[0111] In some embodiments, the above-mentioned building unit can be specifically configured as follows:

[0112] Obtain the desired positive definite inertia matrix, the desired positive definite damping matrix, and the desired positive definite stiffness matrix;

[0113] Based on the desired positive definite inertia matrix, desired positive definite damping matrix, desired positive definite stiffness matrix, current pose information, and external resultant force, the admittance control equation is constructed.

[0114] In some embodiments, the above-mentioned computing unit may be specifically configured as follows:

[0115] The admittance control equation is transformed into a transfer function that characterizes the velocity of the position deviation of the tool's center point from its current position to the target position.

[0116] Discretize the transfer function to obtain the target position coordinates when the tool center point moves from the current position to the target position within the telecentric motion range.

[0117] In some embodiments, the target attitude angle includes the target roll angle, the target pitch angle, and the target yaw angle; the current attitude angle includes the current yaw angle. The aforementioned determining unit can be specifically configured as follows:

[0118] Calculate the target roll angle when the tool's center point moves to the target position based on the target position coordinates;

[0119] Obtain the X-axis torque data when the tool's center point is at its current position;

[0120] Based on the current yaw angle of the tool center point at the current position, the X-axis torque data, and the target roll angle, calculate the target pitch angle and target yaw angle when the tool center point moves to the target position.

[0121] In some embodiments, the target pitch angle and target yaw angle when the tool center point moves to the target position are calculated based on the current yaw angle, X-axis torque data, and target roll angle at the current position of the tool center point. Specifically, this includes:

[0122] Based on the current yaw angle of the tool center point at the current position, the X-axis torque data, and the admittance control equation, calculate the target yaw angle when the tool center point moves to the target position;

[0123] Based on the target roll angle and the target yaw angle, calculate the target pitch angle when the tool center point is at the target position.

[0124] In some embodiments, the control module 404 includes:

[0125] The angle calculation unit is configured as follows:

[0126] The target's roll angle and yaw angle are calculated by vector cross product to obtain the target's pitch angle when the tool's center point is at the target's position.

[0127] In some embodiments, the joint angles of each joint of the serially connected robotic arm can be calculated based on a preset inverse kinematics analysis algorithm of the robotic arm, according to the target posture angle and the target position coordinates.

[0128] It should be understood that the sequence number of each step in the above embodiments does not imply the order of execution. The execution order of each process should be determined by its function and internal logic, and should not constitute any limitation on the implementation process of the embodiments of this disclosure.

[0129] Figure 5 This is a schematic diagram of the electronic device 5 provided in an embodiment of this disclosure. Figure 5 As shown, the electronic device 5 of this embodiment includes: a processor 501, a memory 502, and a computer program 503 stored in the memory 502 and executable on the processor 501. When the processor 501 executes the computer program 503, it implements the steps in the various method embodiments described above. Alternatively, when the processor 501 executes the computer program 503, it implements the functions of each module / unit in the various device embodiments described above.

[0130] Electronic device 5 can be a desktop computer, laptop, handheld computer, cloud server, or other electronic device. Electronic device 5 may include, but is not limited to, processor 501 and memory 502. Those skilled in the art will understand that... Figure 5 This is merely an example of electronic device 5 and does not constitute a limitation on electronic device 5. It may include more or fewer components than shown, or different components.

[0131] The processor 501 may be a central processing unit (CPU), or other general-purpose processors, digital signal processors (DSPs), application-specific integrated circuits (ASICs), field-programmable gate arrays (FPGAs), or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components, etc.

[0132] The memory 502 can be an internal storage unit of the electronic device 5, such as a hard disk or RAM of the electronic device 5. The memory 502 can also be an external storage device of the electronic device 5, such as a plug-in hard disk, Smart Media Card (SMC), Secure Digital (SD) card, Flash Card, etc., equipped on the electronic device 5. The memory 502 can also include both internal and external storage units of the electronic device 5. The memory 502 is used to store computer programs and other programs and data required by the electronic device.

[0133] Those skilled in the art will clearly understand that, for the sake of convenience and brevity, the above-described division of functional units and modules is merely an example. In practical applications, the above functions can be assigned to different functional units and modules as needed, that is, the internal structure of the device can be divided into different functional units or modules to complete all or part of the functions described above. The functional units and modules in the embodiments can be integrated into one processing unit, or each unit can exist physically separately, or two or more units can be integrated into one unit. The integrated unit can be implemented in hardware or as a software functional unit.

[0134] If an integrated module / unit is implemented as a software functional unit and sold or used as an independent product, it can be stored in a computer-readable storage medium. Based on this understanding, all or part of the processes in the methods of the above embodiments can also be implemented by a computer program instructing related hardware. The computer program can be stored in a computer-readable storage medium, and when executed by a processor, it can implement the steps of the various method embodiments described above. The computer program may include computer program code, which can be in the form of source code, object code, executable files, or certain intermediate forms. A computer-readable medium may include: any entity or device capable of carrying computer program code, recording media, USB flash drives, portable hard drives, magnetic disks, optical disks, computer memory, read-only memory (ROM), random access memory (RAM), electrical carrier signals, telecommunication signals, and software distribution media, etc. It should be noted that the content included in a computer-readable medium may be appropriately added to or subtracted according to the requirements of legislation and patent practice in a jurisdiction. For example, in some jurisdictions, according to legislation and patent practice, computer-readable media may not include electrical carrier signals and telecommunication signals.

[0135] The above embodiments are only used to illustrate the technical solutions of this disclosure, and are not intended to limit it. Although this disclosure has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that modifications can still be made to the technical solutions described in the foregoing embodiments, or equivalent substitutions can be made to some of the technical features. Such modifications or substitutions do not cause the essence of the corresponding technical solutions to deviate from the spirit and scope of the technical solutions of the embodiments of this disclosure, and should all be included within the protection scope of this disclosure.

Claims

1. A method for controlling the telecentric motion of a serial robotic arm, characterized in that, include: Determine the telecentric motion range of the tool center point of the end effector of the tandem robotic arm; The current pose information of the tool center point at the current position is obtained, as well as the external resultant force applied to the end tool of the serial robotic arm. The current pose information includes the current position coordinates and current posture angle of the tool center point at the current position. Based on the current pose information and the external resultant force, the target position coordinates and target pose angle of the tool center point when it moves from the current position to the target position within the telecentric motion range are calculated. Based on the target position coordinates and the target posture angle, motion control is performed on each joint of the serial robotic arm to achieve telecentric motion control of the serial robotic arm; Based on the current pose information and the external resultant force, the target position coordinates and target pose angle of the tool center point as it moves from the current position to the target position within the telecentric motion range are calculated, including: Based on the current pose information and the external resultant force, construct the admittance control equation; Based on the admittance control equation, the target position coordinates of the target position are calculated when the tool center point moves from the current position to the target position within the telecentric motion range; Based on the target position coordinates and the current pose information, determine the target pose angle of the target position when the tool center point moves from the current position to the target position within the telecentric motion range; The target attitude angles include the target roll angle, the target pitch angle, and the target yaw angle; the current attitude angles include the current yaw angle. Based on the target position coordinates and the current pose information, determine the target pose angle of the target position when the tool center point moves from the current position to the target position within the telecentric motion range, including: Based on the target position coordinates, calculate the target roll angle when the tool's center point moves to the target position; Obtain the X-axis torque data when the tool's center point is at the current position; Based on the current yaw angle of the tool center point at the current position, the X-axis torque data, and the target roll angle, the target pitch angle and target yaw angle when the tool center point moves to the target position are calculated.

2. The method according to claim 1, characterized in that, Based on the current pose information and the external resultant force, the admittance control equations are constructed, including: Obtain the desired positive definite inertia matrix, the desired positive definite damping matrix, and the desired positive definite stiffness matrix; Based on the desired positive definite inertia matrix, desired positive definite damping matrix, desired positive definite stiffness matrix, current pose information, and external resultant force, the admittance control equation is constructed.

3. The method according to claim 1, characterized in that, Based on the admittance control equation, the target position coordinates of the target position are calculated when the tool center point moves from the current position to the target position within the telecentric motion range, including: The admittance control equation is converted into a transfer function that characterizes the velocity of the position deviation of the tool's center point from its current position to its target position. Discretize the transfer function to obtain the target position coordinates of the target position when the tool center point moves from the current position to the target position within the telecentric motion range.

4. The method according to claim 1, characterized in that, Based on the current yaw angle of the tool's center point at its current position, the X-axis torque data, and the target roll angle, the target pitch angle and target yaw angle are calculated when the tool's center point moves to the target position, including: Based on the current yaw angle of the tool center point at the current position, the X-axis torque data, and the admittance control equation, the target yaw angle when the tool center point moves to the target position is calculated. Based on the target roll angle and the target yaw angle, the target pitch angle when the tool's center point is at the target position is calculated.

5. The method according to claim 4, characterized in that, Based on the target roll angle and the target yaw angle, the target pitch angle when the tool's center point is at the target position is calculated, including: The target roll angle and the target yaw angle are calculated by vector cross product to obtain the target pitch angle when the tool center point is at the target position.

6. A telecentric motion control device for a serial robotic arm, characterized in that, The telecentric motion control device is used to implement the method as described in any one of claims 1 to 5, wherein the telecentric motion control device comprises: The determination module is configured to determine the telecentric motion range of the tool center point of the end effector of the tandem robotic arm; The acquisition module is configured to acquire the current pose information of the tool center point at the current position, and the external resultant force applied to the end tool of the serial robotic arm. The current pose information includes the current position coordinates and current posture angle of the tool center point at the current position. The calculation module is configured to calculate the target position coordinates and target posture angle of the tool center point when it moves from the current position to the target position within the telecentric motion range, based on the current pose information and the external resultant force. The control module is configured to perform motion control on each joint of the serial robotic arm according to the target position coordinates and the target posture angle, so as to realize the telecentric motion control of the serial robotic arm.

7. An electronic device comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that, When the processor executes the computer program, it implements the steps of the method as described in any one of claims 1 to 5.

8. A computer-readable storage medium storing a computer program, characterized in that, When the computer program is executed by a processor, it implements the steps of the method as described in any one of claims 1 to 5.

Images