Mechanical arm control method, device and equipment and storage medium

Abstract

The invention relates to a mechanical arm control method, device and equipment and a storage medium. A camera and a tool are arranged at the tail end of the mechanical arm, and the mechanical arm control method comprises the steps that target point recognition is conducted based on the camera, and first pose information of a target point relative to a base of the mechanical arm is determined; wherein the target point is a to-be-operated point on the target object; controlling the tool to move to a safety point according to the first pose information; wherein the distance between the safety point and the target point is greater than a set safety distance; calculating second pose information of the tool relative to the base; the first pose information is updated, trajectory planning is carried out according to the updated first pose information and second pose information, and a planned path is generated; and controlling the tool to move according to the planned path so as to operate the target point through the tool. According to the method provided by the invention, the movement of the target point can be tracked in real time, the tracking efficiency is improved, and the safe and stable operation of the tool on the target point is further ensured.

Description

Technical Field

[0001] The present invention relates to the technical field of robotic arm control, and particularly to a robotic arm control method, device, equipment, and storage medium. Background Art

[0002] In robotic arm-assisted medical surgeries, patients usually need to maintain a fixed posture to ensure the accuracy and safety of the surgery, such as robotic arm injections. However, since the injection process often lasts for a long time, patients may have difficulty maintaining a stable posture due to factors such as nervousness, fatigue, or other discomforts. This instability not only increases the patient's pain but may also lead to major problems during the surgery. For example, once the patient accidentally moves, the robotic arm may not be able to accurately track the injection point, directly affecting the surgical outcome and even resulting in surgical failure, bringing a series of safety hazards. Therefore, there is an urgent need to provide a method to improve the stability and tracking efficiency of robotic arms in medical surgeries. Summary of the Invention

[0003] To solve the above technical problems, embodiments of the present disclosure provide a robotic arm control method, device, equipment, and storage medium.

[0004] In a first aspect, embodiments of the present disclosure provide a robotic arm control method. A camera and a tool are configured at the end of the robotic arm. The method includes:

[0005] Based on the camera, identify a target point and determine first pose information of the target point relative to the base of the robotic arm; wherein the target point is a point to be operated on a target object.

[0006] According to the first pose information, control the tool to move to a safety point; wherein the distance between the safety point and the target point is greater than a set safety distance.

[0007] Calculate second pose information of the tool relative to the base.

[0008] Update the first pose information, and perform trajectory planning based on the updated first pose information and the second pose information to generate a planned path.

[0009] Control the tool to move along the planned path to operate on the target point through the tool.

[0010] Optionally, a flange is further configured at the end. The step of identifying a target point based on the camera and determining first pose information of the target point relative to the base of the robotic arm includes:

[0011] According to the state function of the robotic arm, determine a first homogeneous matrix of the flange relative to the base.

[0012] Determine a second homogeneous matrix of the camera relative to the flange according to the configuration information of the camera and the flange at the end;

[0013] When the target point is acquired by the camera, determine a third homogeneous matrix of the target point relative to the camera through a target recognition function;

[0014] Determine first pose information of the target point relative to the base of the robotic arm according to the first homogeneous matrix, the second homogeneous matrix, and the third homogeneous matrix.

[0015] Optionally, the determining first pose information of the target point relative to the base of the robotic arm according to the first homogeneous matrix, the second homogeneous matrix, and the third homogeneous matrix includes:

[0016] Multiply the first homogeneous matrix, the second homogeneous matrix, and the third homogeneous matrix to obtain a fourth homogeneous matrix; wherein, the fourth homogeneous matrix is a 4x4 matrix;

[0017] Determine a first sub-matrix formed by taking the element in the first row and first column of the fourth homogeneous matrix as the starting point as a 3x3 matrix, and at the same time determine a second sub-matrix formed by taking the element in the first row and fourth column of the fourth homogeneous matrix as the starting point as a 3x1 matrix;

[0018] Determine angle information according to the first sub-matrix and determine position information according to the second sub-matrix; wherein, the first pose information includes the position information and the angle information.

[0019] Optionally, the calculating second pose information of the tool relative to the base includes:

[0020] Determine a fifth homogeneous matrix of the tool relative to the flange according to the configuration parameters of the tool on the robotic arm;

[0021] Calculate the product of the first homogeneous matrix and the fifth homogeneous matrix to obtain a sixth homogeneous matrix of the tool relative to the base;

[0022] Extract a plurality of sub-matrices from the sixth homogeneous matrix and calculate second pose information according to the plurality of sub-matrices.

[0023] Optionally, before generating a planned path by performing trajectory planning according to the updated first pose information and the second pose information, the method further includes:

[0024] Calculate an error based on the updated first pose information, the second pose information, and the third pose information to obtain fourth pose information; wherein, the third pose information is that of the flange of the end effector relative to the base.

[0025] Performing trajectory planning based on the updated first pose information and the second pose information to generate a planned path, includes:

[0026] Performing trajectory planning based on the fourth pose information, the updated first pose information, and the second pose information to generate a planned path, where the planned path is a feasible path between the first pose and the second pose.

[0027] Optionally, calculating the error based on the updated first pose information, the second pose information, and the third pose information to obtain fourth pose information, includes:

[0028] Calculating a ratio of the modulus of the distance vector between the tool and the target point to a set pose threshold according to the second pose information and the updated first pose information to obtain a first value;

[0029] Calculating a product of the first value and the third pose information to obtain the fourth pose information.

[0030] Optionally, performing trajectory planning based on the fourth pose information, the updated first pose information, and the second pose information to generate a planned path, includes:

[0031] Generating a planned path starting from the second pose and ending at the first pose according to the fourth pose information, a set target speed, and a set motion threshold; wherein, the target speed is the speed when the tool moves to the target point, and the motion threshold characterizes the limitations on the moving speed and moving acceleration of the tool.

[0032] Optionally, when the tool completes the planned path, the method further includes:

[0033] Updating the first pose information and the second pose information;

[0034] Calculating the modulus of the distance vector between the tool and the target point according to the updated first pose information and the updated second pose information to obtain a second value;

[0035] If the second value is less than the set pose threshold, it is determined that the tool reaches the target point, and the tool is controlled to operate on the target point.

[0036] Second aspect, embodiments of the present disclosure provide a robotic arm control device. A camera and a tool are configured at the end of the robotic arm. The device includes:

[0037] A target point recognition unit, configured to recognize a target point based on the camera, and determine first pose information of the target point relative to the base of the robotic arm; wherein the target point is an operation point on a target object;

[0038] A first motion unit, configured to control the tool to move to a safety point according to the first pose information; wherein the distance between the safety point and the target point is greater than a set safety distance;

[0039] A calculation unit, configured to calculate second pose information of the tool relative to the base;

[0040] A trajectory planning unit, configured to update the first pose information, and perform trajectory planning according to the updated first pose information and the second pose information to generate a planned path;

[0041] A second motion unit, configured to control the tool to move along the planned path to operate on the target point through the tool.

[0042] Third aspect, embodiments of the present disclosure provide an electronic device, including:

[0043] A memory;

[0044] A processor; and

[0045] A computer program;

[0046] wherein the computer program is stored in the memory and is configured to be executed by the processor to implement the method described in the first aspect.

[0047] Fourth aspect, embodiments of the present disclosure provide a computer-readable storage medium, on which a computer program is stored. When the computer program is executed by a processor, the steps of the method described in the first aspect are implemented.

[0048] The robotic arm control method provided by the present disclosure has a camera and a tool configured at the end of the robotic arm. The method includes: identifying a target point based on the camera to determine first pose information of the target point relative to the base of the robotic arm; wherein, the target point is a point to be operated on the target object; controlling the tool to move to a safety point according to the first pose information; wherein, the distance between the safety point and the target point is greater than a set safety distance; calculating second pose information of the tool relative to the base; updating the first pose information, and generating a planned path by performing trajectory planning according to the updated first pose information and the second pose information; controlling the tool to move along the planned path to operate on the target point through the tool. The method provided by this application can track the movement of the target point in real time, improve the tracking efficiency, and further ensure that the tool can perform safe and stable operations on the target point. Description of the Drawings

[0049] The drawings herein are incorporated into the specification and constitute a part of this specification, showing embodiments consistent with the present disclosure, and are used together with the specification to explain the principles of the present disclosure.

[0050] To more clearly illustrate the technical solutions in the embodiments of the present disclosure or in the prior art, the following will briefly introduce the drawings required for use in the description of the embodiments or the prior art. Obviously, for those of ordinary skill in the art, other drawings can also be obtained based on these drawings without creative efforts.

[0051] Figure 1 It is a schematic flowchart of a robotic arm control method provided by an embodiment of the present disclosure;

[0052] Figure 2 For Figure 1 It is a detailed flowchart of S101 in a robotic arm control method shown;

[0053] Figure 3 It is a schematic flowchart of another robotic arm control method provided by an embodiment of the present disclosure;

[0054] Figure 4 It is a schematic flowchart of still another robotic arm control method provided by an embodiment of the present disclosure;

[0055] Figure 5 It is a schematic flowchart of an online trajectory planning method based on median filtering provided by an embodiment of the present disclosure;

[0056] Figure 6 It is a schematic structural diagram of a robotic arm control device provided by an embodiment of the present disclosure;

[0057] Figure 7 It is a schematic structural diagram of an electronic device provided by an embodiment of the present disclosure. Detailed Embodiments

[0058] In order to more clearly understand the above-mentioned objects, features and advantages of the present disclosure, the solutions of the present disclosure will be further described below. It should be noted that, without conflict, the embodiments of the present disclosure and the features in the embodiments may be combined with each other.

[0059] In the following description, many specific details are set forth in order to fully understand the present disclosure, but the present disclosure may also be implemented in other ways different from those described herein; obviously, the embodiments in the specification are only a part of the embodiments of the present disclosure, rather than all of the embodiments.

[0060] Specifically, in the medical surgical scenario of robotic arm-assisted injection, most patients need to stay still. However, due to factors such as long injection time and patient nervousness, it is very difficult to maintain a posture. If the patient accidentally moves, the injection point will also change accordingly, and the robotic arm may not be able to track the injection point in real time, resulting in surgical failure and a series of safety problems. In addition, during other dynamic tracking processes, due to factors such as the tracking speed of the robotic arm, the surgical injection efficiency will also be affected.

[0061] To solve the above technical problems, the embodiments of the present disclosure provide a robotic arm control method, which relates to the field of robotic arm control. By real-time tracking of the target point, precise operation is ensured. At the same time, through trajectory planning, the speed of the robotic arm tracking can also be reasonably controlled, improving the tracking efficiency, and ensuring the operation accuracy, operation safety and operation stability of the tool. Specific details are described in detail through the following one or more embodiments.

[0062] The robotic arm control method provided by the embodiments of the present disclosure is applicable to the robotic arm control scenario. This method can be executed by a robotic arm control device, which can be implemented in software and / or hardware, and the device can be integrated in an electronic device. Among them, the electronic device may include, but is not limited to, mobile terminals such as smart phones, laptop computers, digital broadcast receivers, personal digital assistants (PDAs), tablet personal computers (Tablet PCs), portable multimedia players (PMPs), vehicle-mounted terminals (such as vehicle-mounted navigation terminals), wearable devices, etc., and fixed terminals such as digital TVs, desktop computers, and smart home devices.

[0063] Figure 1 FIG. is a schematic flow chart of a robotic arm control method provided by an embodiment of the present disclosure. A camera, a tool and a flange are configured at the end of the robotic arm, and specifically include the following steps as Figure 1 shown below:

[0064] Among them, the robotic arm includes a controller, an end effector, and a base, etc. The controller is the core control part of the robotic arm, used to control each component on the robotic arm. The base is the fixed or installation part of the robotic arm, providing the support structure and motion reference point of the robotic arm. The end effector is the component installed at the farthest end of the robotic arm, directly interacting with the environment or object. Among them, components such as a camera, a tool, and a flange are configured on the end effector. The camera is used to provide visual feedback, helping the robotic arm perceive the surrounding environment and feeding back the real-time captured images to the controller. The flange is a mechanical interface, usually a circular disc-shaped structure with bolt holes, used to connect the robotic arm and the end effector, and also supports the quick replacement of different end effectors. The tool can be understood as the end effector, such as an injection needle for medical use. The injection needle is connected to the robotic arm through the flange and is configured at the end of the robotic arm.

[0065] S101. Identify the target point based on the camera, and determine the first pose information of the target point relative to the base of the robotic arm;

[0066] Among them, the target point is the point to be operated on the target object.

[0067] It can be understood that when identifying the target point of the target object based on the camera, the target point can be understood as the point to be operated by the tool, such as the injection point to be operated by the injection needle, and the target object can be the patient to be treated. After identifying the target point, determine the first pose information of the target point relative to the base. The first pose information reflects the position and pose of the target point in the base coordinate system. Among them, the base usually defines the position of the global coordinate system (also known as the world coordinate system or the base coordinate system), and the joint angles on the robotic arm and the position and pose of the end effector are all described relative to the base coordinate system.

[0068] S102. Control the tool to move to the safety point according to the first pose information;

[0069] Among them, the distance between the safety point and the target point is greater than the set safety distance;

[0070] It is understandable that, based on the above S101, the safe pose is determined according to the first pose information and the safe distance, and then the tool is controlled to move to this safe pose. The safe distance is, for example, 5 cm above the target point. Specifically, if the tip of the injection needle is 5 cm above the target point, 5 cm is the appropriate position of the tip of the needle from the target point to ensure the safety of the target object. The safe distance can be determined by the user according to their needs, and the safe distances corresponding to tools performing different tasks are different. Additionally, when safety is guaranteed, the tool can be controlled to move to the safe pose at a relatively fast speed. Subsequently, when the moving distance is approximately the safe distance, that is, when the tip of the needle approaches the target point, factors such as safety and tracking speed need to be fully considered when controlling the tool to move to the position where the target point is located.

[0071] S103. Calculate the second pose information of the tool relative to the base.

[0072] It is understandable that, based on the above S102, after the tool moves to the safe point, the second pose information of the tool relative to the base is calculated. Among them, the second pose information represents the position and orientation of the tool in the base coordinate system. That is, the target point and the tool are unified in the base coordinate system to accurately track the target point and accurately control the tool.

[0073] S104. Update the first pose information, and perform trajectory planning according to the updated first pose information and the second pose information to generate a planned path.

[0074] Among them, the planned path refers to the feasible path between the first pose and the second pose.

[0075] It is understandable that, based on the above S103, for the problem of the target point moving due to the movement of the target object, it is necessary to perform real-time recognition of the target point based on the camera and update the first pose information to ensure that the target object can be followed in real time and the target point information can be tracked in real time when the target object moves. In one possible situation, during the process of controlling the tool to move to the safe point, the target point moves slightly. In this case, real-time tracking can accurately locate the moved target point. In another situation, the target object may not move either, and the updated first pose information is the same as the first pose information determined in S101. Additionally, since the movement of the target point is slight, the distance from the tool will also fluctuate above and below the safe distance. Subsequently, the online path planning can be directly performed according to the updated first pose information and the second pose information to generate a planned path. Among them, the starting point of the planned path is at the second pose, the end point is at the first pose, the second pose information includes the relevant information at the second pose, and the updated first pose information includes the relevant information at the first pose. That is, the planned path refers to the planned trajectory for the tool to move to the target point.

[0076] S105. Control the tool to move along the planned path to operate on the target point through the tool.

[0077] It can be understood that, based on the above S104, control the tool to move along the planned path towards the target point until the tool (such as the tip of the needle) and the target point are in the same position, that is, the ultimate goal of the control is to make the first pose equal to the second pose, or the error between the two is within a small range. After the tool reaches the target point, the tool can be controlled to operate on the target point.

[0078] Optionally, when the tool completes the planned path, the method further includes:

[0079] Update the first pose information and the second pose information; calculate the modulus value of the tool and the target point on the distance vector according to the updated first pose information and the updated second pose information to obtain a second value; if the second value is less than the set pose threshold, it is determined that the tool reaches the target point, and control the tool to operate on the target point.

[0080] It can be understood that after the tool moves along the planned path and before operating on the target point, to avoid safety problems caused by the movement of the target point during the movement of the tool, it is also necessary to perform real-time pose judgment on the target point, that is, the first pose information needs to be updated at least for the third time, and the first pose information can also be updated in real time according to the period or in real time according to the event. At the same time, calculate the pose of the tool after moving along the planned path and update the second pose information. Subsequently, continue to calculate the error according to the updated first pose information and the updated second pose information to obtain an error result, where the error result includes a position error and an angle error. If the position error is less than the set position threshold and the angle error is less than the set angle threshold, turn off the real-time pose judgment of the target point and stop the online trajectory planning, and directly control the tool to operate on the target point. If either the position error or the angle error is greater than the set threshold, repeat the above process to continue following the target point and perform online trajectory planning.

[0081] The robotic arm control method provided by the embodiments of the present disclosure can track the pose information of the target point in real time when the target point moves. When the target point moves too large or too small, it can also track the target point in real time and perform online trajectory planning to prevent safety problems caused by too large or too small movement ranges, which not only improves the tracking efficiency but also ensures the safety, stability, and accuracy of the operation.

[0082] Based on the above embodiments, Figure 2 For Figure 1The schematic diagram of the refinement process of S101 in a robotic arm control method shown. Optionally, the target point is recognized based on the camera, and the first pose information of the target point relative to the base of the robotic arm is determined, specifically including as follows Figure 2 The following steps shown as follows:

[0083] S201. Determine the first homogeneous matrix of the flange relative to the base according to the state function of the robotic arm.

[0084] It can be understood that the state function of the robotic arm is a mathematical expression describing all state variables of the robotic arm at a certain moment, such as state variables such as joint angles, joint velocities, joint accelerations, and the pose of the end effector. According to the state function, the first homogeneous matrix of the flange relative to the base can be determined, where the first homogeneous matrix represents the position and orientation of the flange in the base coordinate system. Among them, the homogeneous matrix refers to a matrix used to represent affine transformations (such as translation, rotation, scaling, etc.) in computer graphics and robotics, usually a 4x4 matrix, which can represent translation, rotation, scaling, and their combinations. The following embodiments will be described in detail taking the homogeneous matrix as a 4x4 matrix as an example.

[0085] S202. Determine the second homogeneous matrix of the camera relative to the flange according to the configuration information of the camera and the flange at the end.

[0086] It can be understood that both the camera and the flange are configured at the end of the robotic arm. According to the configuration information, the homogeneous matrix of the camera relative to the flange can be determined, and specifically, the hand-eye calibration method can be used to determine it. Among them, the second homogeneous matrix represents the position and orientation of the camera in the flange coordinate system.

[0087] S203. When the camera captures the target point, determine the third homogeneous matrix of the target point relative to the camera through the target recognition function.

[0088] It can be understood that when the target point is within the camera capture range, the third homogeneous matrix of the target point relative to the camera is determined through the target recognition function, where the third homogeneous matrix represents the position and orientation of the target point in the camera coordinate system.

[0089] S204. Determine the first pose information of the target point relative to the base of the robotic arm according to the first homogeneous matrix, the second homogeneous matrix, and the third homogeneous matrix.

[0090] It can be understood that the product of the first homogeneous matrix, the second homogeneous matrix, and the third homogeneous matrix is calculated, and then the first pose information of the target point relative to the base is obtained.

[0091] Optionally, determining the first pose information of the target point relative to the base of the robotic arm according to the first homogeneous matrix, the second homogeneous matrix, and the third homogeneous matrix can be specifically implemented through the following steps:

[0092] Multiply the first homogeneous matrix, the second homogeneous matrix, and the third homogeneous matrix successively to obtain a fourth homogeneous matrix; wherein, the fourth homogeneous matrix is a 4x4 matrix; determine the 3x3 matrix formed with the element in the first row and first column of the fourth homogeneous matrix as the starting point as the first sub-matrix, and at the same time determine the 3x1 matrix formed with the element in the first row and fourth column of the fourth homogeneous matrix as the starting point as the second sub-matrix; determine the angular information according to the first sub-matrix, and determine the position information according to the second sub-matrix; wherein, the first pose information includes the position information and the angular information.

[0093] It can be understood that the calculation method of the fourth homogeneous matrix is shown in formula (1).

[0094]

[0095] In the formula, represents the fourth homogeneous matrix of the target point relative to the base, represents the first homogeneous matrix of the flange relative to the base, represents the second homogeneous matrix of the camera relative to the flange, represents the third homogeneous matrix of the target point relative to the camera.

[0096] It can be understood that after obtaining the 4x4 fourth homogeneous matrix, divide the fourth homogeneous matrix into multiple sub-matrices. For example, determine the 3x3 matrix formed starting from the element in the first row and first column as the first sub-matrix, and at the same time determine the 3x1 matrix formed starting from the element in the first row and fourth column as the second sub-matrix. The fourth homogeneous matrix, the first sub-matrix, and the second sub-matrix are shown in formula (2).

[0097]

[0098] In the formula, T represents the 4x4 fourth homogeneous matrix, M1 represents the 3x3 first sub-matrix, and M2 represents the 3x1 second sub-matrix.

[0099] It is understandable that the first sub - matrix is the 3x3 part starting from the upper - left corner of the 4x4 fourth homogeneous matrix, which can be regarded as the rotation matrix R. Among them, the first sub - matrix contains all the rotation information and can be used to perform rotation operations on vectors without changing their positions. According to the first sub - matrix, the angle information can be determined, which can be used to perform rotation and other linear transformations. The second sub - matrix is the fourth column of the 4x4 fourth homogeneous matrix (excluding the last element because it is 1 in homogeneous coordinates), which actually represents the translation vector t and can be used to perform translation operations.

[0100] It is understandable that the first pose is represented as P face (X face , Y face , Z face , A face , B face , C face ), where the first sub - matrix characterizes the pose information of A face , B face , C face etc., and the second sub - matrix characterizes the position information of X face , Y face , Z face etc. The safe pose 5 cm above the target point is represented as P face1 (X face1 , Y face1 , Z face1 , A face1 , B face1 , C face1 ).

[0101] Optionally, calculating the second pose information of the tool relative to the base can be specifically achieved through the following steps:

[0102] According to the configuration parameters of the tool on the robotic arm, determine the fifth homogeneous matrix of the tool relative to the flange; calculate the product of the first homogeneous matrix and the fifth homogeneous matrix to obtain the sixth homogeneous matrix of the tool relative to the base; extract multiple sub - matrices from the sixth homogeneous matrix and calculate the second pose information according to the multiple sub - matrices.

[0103] It is understandable that according to the configuration parameters of the tool at the end, the fifth homogeneous matrix of the tool relative to the flange can be determined, where the fifth homogeneous matrix characterizes the position and orientation of the tool in the flange coordinate system. Calculate the product of the fifth homogeneous coordinate and the above - mentioned first homogeneous coordinate to obtain the sixth homogeneous matrix, as shown in formula (3) specifically.

[0104]

[0105] In the formula, The sixth homogeneous matrix representing the tool relative to the base The first homogeneous matrix representing the flange relative to the base The fifth homogeneous matrix representing the tool relative to the flange.

[0106] It can be understood that after obtaining the sixth homogeneous matrix, multiple sub - matrices are obtained according to the sub - matrix division method shown by the fourth homogeneous matrix, and then the second pose information is determined based on the multiple sub - matrices. The specific process will not be elaborated here. The first pose is represented as P tool (X tool , Y tool , Z tool , A tool , B tool , C tool ), which can be understood as the tool position. The ultimate goal of control is that P face and P tool are equal, that is, the target point and the tool position are the same, indicating that the tool reaches the target point.

[0107] A robotic arm control method provided by an embodiment of the present disclosure unifies the tool and the target point in the base coordinate system, facilitating real - time tracking of the target point. Subsequently, online trajectory planning is performed between the tool and the target point to accurately control the tool to move to the target point.

[0108] Based on the above - mentioned embodiment, Figure 3 is a schematic flow diagram of another robotic arm control method provided by an embodiment of the present disclosure. Optionally, before generating a planned path according to the updated first pose information and the second pose information for trajectory planning, the robotic arm control method specifically further includes steps as Figure 3 shown:

[0109] S301. Calculate the error according to the updated first pose information, the second pose information, and the third pose information to obtain the fourth pose information.

[0110] Among them, the third pose information is that of the flange configured at the end relative to the base.

[0111] It can be understood that to ensure safety, the error is calculated according to the updated first pose information, the second pose information, and the third pose information to obtain the fourth pose information. Among them, the third pose information characterizes the position and pose of the flange in the base coordinate system. According to the error calculation, it can be preliminarily determined whether to follow the plan and whether to scale according to the relative position between the target point and the tool. In the case where the change in the first pose information is large due to the movement of the target object, the relative position can be reduced first, and then the tracking plan can be carried out, which can effectively improve the control efficiency.

[0112] Optionally, the error calculation is performed based on the updated first pose information, the second pose information, and the third pose information to obtain the fourth pose information, which can be specifically implemented through the following steps:

[0113] According to the second pose information and the updated first pose information, calculate the ratio of the modulus of the distance vector between the tool and the target point to the set pose threshold to obtain a first value; calculate the product of the first value and the third pose information to obtain the fourth pose information.

[0114] It can be understood that the calculation process of the fourth pose information is shown in formula (4).

[0115]

[0116] In the formula, the fourth pose information is represented as P delta (X delta , Y delta , Z delta , A delta , B delta , C delta ), the third pose information is represented as P flange (X flange , Y flange , Z flange , A flange , B flange , C flange ), where the calculated value of (X tool - X face ).norm() on the degree of freedom X represents the modulus of the distance vector between the tool and the target point, and the value of this modulus is denoted as the modulus value. There is a modulus value for each degree of freedom. The degree of freedom refers to the above X, Y, Z, A, B, C. The pose threshold includes the position threshold M max and the angle threshold N max .

[0117] It can be understood that when the modulus value is equal to the pose threshold, the first value is 1. The farther the tool and the target point are, the larger the first value. Taking X as an example, the calculated X delta is closer to the calculated value X flange of the flange position, the faster the tracking speed, and vice versa. Therefore, according to the relationship between the fourth pose information and the third pose information, the motion strategy of the tool can be specified, and this motion strategy can be fully considered when controlling the tool to move subsequently.

[0118] Optionally, after obtaining the fourth pose information, the method further includes:

[0119] Determine a motion strategy according to the fourth pose information and the pose threshold, where the motion strategy characterizes the association relationship between the distance between the tool and the target point and the moving speed.

[0120] It is understandable that through the error calculation between the tool and the target point, when the tool is far from the target point, the tool can quickly move to the vicinity of the target point. When the tool is close to the target point, the movement speed of the tool can be controlled. The motion strategy formulated accordingly improves safety while ensuring efficiency.

[0121] It is understandable that when calculating the error, the position error can be calculated according to the three degrees of freedom of XYZ, and the angle error can be calculated according to the three degrees of freedom of ABC. To ensure safety, if the position error is greater than M max , and the angle error is greater than N max , then the following planning is not performed. Otherwise, scaling is performed according to the relative position between the tool and the target point. After scaling, the error calculation is performed again. If the above conditions are met, the following planning is performed. When the position error between the target point and the tool is within the range of (M max ~M min ), it indicates that the farther the distance, the greater the following distance sent, and vice versa. When the attitude error between the two is within the range of (N max ~N min ), it indicates that the farther the distance, the greater the following distance sent, and vice versa.

[0122] S302. Perform trajectory planning according to the fourth pose information, the updated first pose information, and the second pose information to generate a planned path.

[0123] It is understandable that based on the above S301, online trajectory planning (OnlineTrajectoryPlanning) is performed according to the fourth pose information, the updated first pose information, and the second pose information to generate a planned path.

[0124] Optionally, the performing trajectory planning according to the fourth pose information, the updated first pose information, and the second pose information to generate a planned path can be specifically implemented through the following steps:

[0125] Generate a planned path starting from the second pose and ending at the first pose according to the fourth pose information, the set target speed, and the set motion threshold; wherein, the target speed refers to the speed when the tool moves to the target point, and the motion threshold characterizes the limitation on the moving speed and moving acceleration of the tool.

[0126] It is understandable that the fourth pose information, the target speed, and the motion threshold are used as the inputs for online trajectory planning to generate a planned path that starts moving from the second pose and ends at the first pose. Among them, the target speed refers to the speed when the tool moves to the target point. For safety and accuracy considerations, the target speed is 0, that is, the tool stops exactly at the target point. The motion threshold refers to the limitations on the moving speed and moving acceleration of the tool during the movement. Additionally, the current position and current speed of the tool can also be used as the inputs for online trajectory planning. Usually, at the start of the movement, both the current position and current speed of the tool are 0, that is, the tool starts moving from a stationary state towards the target point. In this case, the smooth start of the tool needs to be considered during trajectory planning to ensure the control smoothness and safety throughout the movement process.

[0127] A robotic arm control method provided by an embodiment of the present disclosure controls a tool to move towards a target point safely and accurately through online trajectory planning, which can ensure that the tool can track the target point in real time and ensure the tracking efficiency.

[0128] Based on the above embodiment, Figure 4 The following is a flowchart of another robotic arm control method provided by an embodiment of the present disclosure, which specifically includes the following steps:

[0129] 1) Identify the target point based on the camera; 2) Move the tool to 5 cm above the target point; 3) Determine the target point position in real time; 4) Judge whether the displacement between the target point and the current tool is greater than M max , and whether the angle is greater than N max ; 5) If the displacement is less than M max and / or the angle is less than N max , then perform online trajectory planning; 6) Based on step 5), after completing the online trajectory planning, judge whether the displacement between the target point and the current tool is less than M max , and whether the angle is less than N max ; 7) Based on step 6), if the displacement is less than M max and the angle is less than N max , then turn off the real-time determination of the target point position; 8) Based on step 7), control the tool to directly move to the target point; 9) Based on step 6), if the displacement is greater than or equal to M max and / or the angle is greater than or equal to N max , then continue to perform online trajectory planning; 10) Based on step 4), if the displacement is greater than or equal to M max and the angle is greater than or equal to N max , then do not perform online trajectory planning and control the tool to directly move to the target point.

[0130] It is understandable that the specific implementation steps of the above 1)-10) can refer to the above embodiment and will not be elaborated here.

[0131] Based on the above embodiments, Figure 5 is a schematic flowchart of an online trajectory planning method based on medium-pass filtering provided by an embodiment of the present disclosure. As Figure 5 shown, online trajectory planning mainly controls six degrees of freedom, namely X, Y, Z, A, B, and C. The first pose information and the second pose information are input into the OTG planning library, and the expected position, expected speed, expected acceleration, etc. of each control period are output. In fact, it refers to the control period when the tool moves to the target point. The complete planned path includes multiple control periods. Completing all control periods means that the tool has moved along the planned path. Among them, the control period Ti can be understood as the initial control period of the tool, and the control period Ti+1 is the next control period. Online trajectory planning generates the expected position and expected speed of the control period Ti+1 based on the fourth pose information, target speed, motion threshold, current position, and current speed at the control period Ti, that is, at the control period Ti+1, the tool is controlled to move from the current position to the expected position. Through online trajectory planning, the tool will always move in the direction of the target point, ensuring real-time tracking of the injection point of the target object.

[0132] Figure 6 is a schematic structural diagram of a robotic arm control device provided by an embodiment of the present disclosure. The robotic arm control device provided by an embodiment of the present disclosure can execute the processing flow provided by the embodiment of the robotic arm control method. As Figure 6 shown, the robotic arm control device 600 includes a target point recognition unit 601, a first motion unit 602, a calculation unit 603, a trajectory planning unit 604, and a second motion unit 605. A camera and a tool are configured at the end of the robotic arm, where:

[0133] The target point recognition unit 601 is configured to recognize the target point based on the camera and determine the first pose information of the target point relative to the base of the robotic arm; wherein, the target point is the point to be operated on the target object;

[0134] The first motion unit 602 is configured to control the tool to move to a safe point according to the first pose information; wherein, the distance between the safe point and the target point is greater than the set safe distance;

[0135] The calculation unit 603 is configured to calculate the second pose information of the tool relative to the base;

[0136] The trajectory planning unit 604 is configured to update the first pose information and perform trajectory planning according to the updated first pose information and the second pose information to generate a planned path;

[0137] A second motion unit 605 for controlling the tool to move along the planned path to operate on the target point through the tool.

[0138] Optionally, the target point recognition unit 601 is configured to:

[0139] Determine a first homogeneous matrix of the flange relative to the base according to the state function of the robotic arm;

[0140] Determine a second homogeneous matrix of the camera relative to the flange according to the configuration information of the camera and the flange at the end;

[0141] When the target point is captured by the camera, determine a third homogeneous matrix of the target point relative to the camera through a target recognition function;

[0142] Determine first pose information of the target point relative to the base of the robotic arm according to the first homogeneous matrix, the second homogeneous matrix, and the third homogeneous matrix.

[0143] Optionally, the target point recognition unit 601 is configured to:

[0144] Multiply the first homogeneous matrix, the second homogeneous matrix, and the third homogeneous matrix to obtain a fourth homogeneous matrix; wherein, the fourth homogeneous matrix is a 4x4 matrix;

[0145] Determine a first sub-matrix formed by taking the element in the first row and first column of the fourth homogeneous matrix as the starting point as a 3x3 matrix, and at the same time determine a second sub-matrix formed by taking the element in the first row and fourth column of the fourth homogeneous matrix as the starting point as a 3x1 matrix;

[0146] Determine angle information according to the first sub-matrix and determine position information according to the second sub-matrix; wherein, the first pose information includes the position information and the angle information.

[0147] Optionally, the calculation unit 603 is configured to:

[0148] Determine a fifth homogeneous matrix of the tool relative to the flange according to the configuration parameters of the tool on the robotic arm;

[0149] Calculate the product of the first homogeneous matrix and the fifth homogeneous matrix to obtain a sixth homogeneous matrix of the tool relative to the base;

[0150] Extract a plurality of sub-matrices from the sixth homogeneous matrix and calculate second pose information according to the plurality of sub-matrices.

[0151] Optionally, the robotic arm control device 600 is further configured to:

[0152] Perform error calculation based on the updated first pose information, the second pose information, and the third pose information to obtain the fourth pose information; wherein, the third pose information is of the flange of the end configuration relative to the base.

[0153] Optionally, the trajectory planning unit 604 is configured to:

[0154] Perform trajectory planning based on the fourth pose information, the updated first pose information, and the second pose information to generate a planned path, where the planned path refers to a feasible path between the first pose and the second pose.

[0155] Optionally, the robotic arm control device 600 is further configured to:

[0156] Calculate the ratio of the modulus of the distance vector between the tool and the target point to the set pose threshold according to the second pose information and the updated first pose information to obtain a first value;

[0157] Calculate the product of the first value and the third pose information to obtain the fourth pose information.

[0158] Optionally, the trajectory planning unit 604 is configured to:

[0159] Generate a planned path starting from the second pose and ending at the first pose according to the fourth pose information, the set target speed, and the set motion threshold; wherein, the target speed refers to the speed when the tool moves to the target point, and the motion threshold characterizes the limitations on the moving speed and moving acceleration of the tool.

[0160] Optionally, the robotic arm control device 600 is further configured to:

[0161] Update the first pose information and the second pose information;

[0162] Calculate the modulus of the distance vector between the tool and the target point according to the updated first pose information and the updated second pose information to obtain a second value;

[0163] If the second value is less than the set pose threshold, determine that the tool reaches the target point and control the tool to operate on the target point.

[0164] Figure 6 The robotic arm control device in the illustrated embodiment can be used to execute the technical solutions of the above method embodiment, and its implementation principle and technical effects are similar, which will not be elaborated here.

[0165] Figure 7 The structural schematic diagram of the electronic device provided by the embodiment of the present disclosure. Specifically refer to the following Figure 7, which shows a schematic structural diagram of the electronic device 700 suitable for implementing the embodiments of the present disclosure. The electronic device 700 in the embodiments of the present disclosure may include, but is not limited to, mobile terminals such as mobile phones, laptop computers, digital broadcast receivers, PDAs (Personal Digital Assistants), PADs (Tablet Computers), PMPs (Portable Multimedia Players), vehicle terminals (such as in-vehicle navigation terminals), wearable electronic devices, etc., and fixed terminals such as digital TVs, desktop computers, smart home devices, etc. Figure 7 The electronic device shown is merely an example and should not impose any limitations on the functions and usage scope of the embodiments of the present disclosure.

[0166] As Figure 7 shown, the electronic device 700 may include a processing device 701 (such as a central processing unit, a graphics processing unit, etc.), which may perform various appropriate actions and processes according to the program stored in the read-only memory (ROM) 702 or the program loaded from the storage device 708 into the random access memory (RAM) 703 to implement the robotic arm control method of the embodiments as described in the present disclosure. In the RAM 703, various programs and data required for the operation of the electronic device 700 are also stored. The processing device 701, the ROM 702, and the RAM 703 are connected to each other through a bus 704. The input / output (I / O) interface 705 is also connected to the bus 704.

[0167] Generally, the following devices may be connected to the I / O interface 705: an input device 706 including, for example, a touch screen, a touchpad, a keyboard, a mouse, a camera, a microphone, an accelerometer, a gyroscope, etc.; an output device 707 including, for example, a liquid crystal display (LCD), a speaker, a vibrator, etc.; a storage device 708 including, for example, a magnetic tape, a hard disk, etc.; and a communication device 709. The communication device 709 may allow the electronic device 700 to communicate with other devices wirelessly or wiredly to exchange data. Although Figure 7 the electronic device 700 with various devices is shown, it should be understood that it is not required to implement or have all the shown devices. More or fewer devices may be alternatively implemented or had.

[0168] In particular, according to an embodiment of the present disclosure, the processes described above with reference to the flowcharts can be implemented as computer software programs. For example, an embodiment of the present disclosure includes a computer program product that includes a computer program carried on a non-transitory computer-readable medium. The computer program includes program code for performing the methods shown in the flowcharts, thereby implementing the robotic arm control method as described above. In such an embodiment, the computer program can be downloaded and installed from a network via a communication device 709, or installed from a storage device 708, or installed from a ROM 702. When the computer program is executed by a processing device 701, the above-described functions defined in the method of the embodiment of the present disclosure are performed.

[0169] It should be noted that the computer-readable medium described above in the present disclosure can be a computer-readable signal medium, a computer-readable storage medium, or any combination of the two. A computer-readable storage medium can be, for example, but not limited to, an electrical, magnetic, optical, electromagnetic, infrared, or semiconductor system, apparatus, or device, or any combination of the above. More specific examples of the computer-readable storage medium can include, but are not limited to: an electrical connection having one or more wires, a portable computer disk, a hard disk, a random access memory (RAM), a read-only memory (ROM), an erasable programmable read-only memory (EPROM or flash memory), an optical fiber, a portable compact disk read-only memory (CD-ROM), an optical storage device, a magnetic storage device, or any suitable combination of the above. In the present disclosure, the computer-readable storage medium can be any tangible medium that contains or stores a program that can be used by or in conjunction with an instruction execution system, apparatus, or device. In the present disclosure, a computer-readable signal medium can include a data signal propagated in a baseband or as part of a carrier wave, which carries computer-readable program code. Such a propagated data signal can take various forms, including but not limited to electromagnetic signals, optical signals, or any suitable combination of the above. The computer-readable signal medium can also be any computer-readable medium other than the computer-readable storage medium, which can send, propagate, or transmit a program for use by or in conjunction with an instruction execution system, apparatus, or device. The program code contained on the computer-readable medium can be transmitted using any appropriate medium, including but not limited to: wires, optical cables, RF (radio frequency), etc., or any suitable combination of the above.

[0170] In some embodiments, the client and the server can communicate using any currently known or future-developed network protocol such as HTTP (HyperText Transfer Protocol), and can be interconnected with digital data communication in any form or medium (e.g., a communication network). Examples of communication networks include local area networks ("LANs"), wide area networks ("WANs"), the Internet (e.g., the Internet), and end-to-end networks (e.g., ad hoc end-to-end networks), as well as any currently known or future-developed networks.

[0171] The above computer-readable medium can be included in the above electronic device; or it can exist separately without being assembled into the electronic device.

[0172] Optionally, when the above one or more programs are executed by the electronic device, the electronic device can also perform the other steps described in the above embodiments.

[0173] Computer program code for performing the operations of the present disclosure can be written in one or more programming languages or combinations thereof. The above programming languages include, but are not limited to, object-oriented programming languages such as Java, Smalltalk, C++, and also include conventional procedural programming languages such as the "C" language or similar programming languages. The program code can be executed entirely on the user's computer, partially on the user's computer, executed as a stand-alone software package, partially on the user's computer and partially on a remote computer, or entirely on a remote computer or server. In the case of a remote computer, the remote computer can be connected to the user's computer through any type of network - including a local area network (LAN) or a wide area network (WAN) - or, alternatively, can be connected to an external computer (e.g., by using an Internet service provider to connect through the Internet).

[0174] The flowcharts and block diagrams in the accompanying drawings illustrate the architecture, functionality, and operation of possible implementations of systems, methods, and computer program products according to various embodiments of the present disclosure. In this regard, each block in the flowchart or block diagram may represent a module, a segment of code, or a portion thereof that contains one or more executable instructions for implementing the specified logical function. It should also be noted that, in some alternative implementations, the functions noted in the blocks may occur in a different order than that noted in the accompanying drawings. For example, two consecutive blocks shown may actually be executed substantially in parallel, or they may sometimes be executed in the reverse order, depending on the functionality involved. It should also be noted that each block in the block diagrams and / or flowcharts, and combinations of blocks in the block diagrams and / or flowcharts, can be implemented by a dedicated hardware-based system that performs the specified functions or operations, or by a combination of dedicated hardware and computer instructions.

[0175] The units described in the embodiments of the present disclosure can be implemented in software or in hardware. In some cases, the name of the unit does not constitute a limitation on the unit itself.

[0176] The functions described above herein can be performed, at least in part, by one or more hardware logic components. By way of example, and without limitation, exemplary types of hardware logic components that may be used include: field programmable gate arrays (FPGAs), application specific integrated circuits (ASICs), application specific standard products (ASSPs), systems on a chip (SOCs), complex programmable logic devices (CPLDs), and the like.

[0177] In the context of the present disclosure, a machine-readable medium may be a tangible medium that can contain or store a program for use by or in connection with an instruction execution system, apparatus, or device. A machine-readable medium may be a machine-readable signal medium or a machine-readable storage medium. A machine-readable medium may include, but is not limited to, electronic, magnetic, optical, electromagnetic, infrared, or semiconductor systems, apparatus, or devices, or any suitable combination of the foregoing. More specific examples of a machine-readable storage medium would include an electrical connection based on one or more wires, a portable computer diskette, a hard disk, a random access memory (RAM), a read-only memory (ROM), an erasable programmable read-only memory (EPROM or Flash memory), an optical fiber, a portable compact disc read-only memory (CD-ROM), an optical storage device, a magnetic storage device, or any suitable combination of the foregoing.

[0178] It should be noted that in this document, relational terms such as "first" and "second" are only used to distinguish one entity or operation from another entity or operation, and do not necessarily require or imply any actual relationship or order between these entities or operations. Moreover, the term "comprising", "including" or any other variant thereof is intended to cover non-exclusive inclusion, such that a process, method, article or gateway comprising a series of elements not only includes those elements but also includes other elements not expressly listed, or further includes elements inherent to such process, method, article or gateway. Without further limitation, an element defined by the statement "comprising an..." does not exclude the presence of additional identical elements in the process, method, article or gateway comprising the said element.

[0179] The above are only specific embodiments of the present disclosure, enabling those skilled in the art to understand or implement the present disclosure. Various modifications to these embodiments will be obvious to those skilled in the art, and the general principles defined herein can be implemented in other embodiments without departing from the spirit or scope of the present disclosure. Therefore, the present disclosure will not be limited to these embodiments described herein, but rather will conform to the widest scope consistent with the principles and novel features disclosed herein.

Claims

1. A robot arm control method, characterized in that: The end of the robotic arm is provided with a camera and a tool, and the method comprises: Recognize the target point based on the camera, and determine the first position information of the target point relative to the base of the robotic arm; wherein the target point is a point to be operated on the target object; Controlling the tool to move to a safety point according to the first posture information; wherein the distance between the safety point and the target point is greater than a set safety distance; Calculating second position information of the tool relative to the base; Updating the first posture information, and performing trajectory planning according to the updated first posture information and the second posture information to generate a planned path; The tool is controlled to move according to the planned path, so as to operate the target point through the tool.

2. The method according to claim 1, characterized in that The end is also provided with a flange, and the target point recognition based on the camera is performed to determine the first position information of the target point relative to the base of the robotic arm, including: Determining a first homogeneous matrix of the flange relative to the base according to a state function of the robotic arm; Determine a second homogeneous matrix of the camera relative to the flange according to configuration information of the camera and the flange at the end; When the camera captures the target point, determining a third homogeneous matrix of the target point relative to the camera through a target recognition function; The first position information of the target point relative to the base of the robotic arm is determined according to the first homogeneous matrix, the second homogeneous matrix and the third homogeneous matrix.

3. The method according to claim 2, characterized in that Determining the first position information of the target point relative to the base of the robotic arm according to the first homogeneous matrix, the second homogeneous matrix and the third homogeneous matrix includes: Multiplying the first homogeneous matrix, the second homogeneous matrix and the third homogeneous matrix to obtain a fourth homogeneous matrix; wherein the fourth homogeneous matrix is ​​a 4x4 matrix; A 3x3 matrix formed by taking the first row and the first column of the fourth homogeneous matrix as a starting point is determined as a first submatrix, and a 3x1 matrix formed by taking the first row and the fourth column of the fourth homogeneous matrix as a starting point is determined as a second submatrix; Angle information is determined according to the first sub-matrix, and position information is determined according to the second sub-matrix; wherein the first posture information includes the position information and the angle information.

4. The method according to claim 2, characterized in that: The calculating the second position information of the tool relative to the base includes: Determining a fifth homogeneous matrix of the tool relative to the flange according to configuration parameters of the tool on the robot arm; Calculating the product of the first homogeneous matrix and the fifth homogeneous matrix to obtain a sixth homogeneous matrix of the tool relative to the base; A plurality of sub-matrices are extracted from the sixth homogeneous matrix, and second posture information is calculated based on the plurality of sub-matrices.

5. The method according to claim 1, characterized in that Before performing trajectory planning according to the updated first pose information and the second pose information to generate a planned path, the method further includes: Perform error calculation according to the updated first pose information, the second pose information and the third pose information to obtain fourth pose information; wherein the third pose information is the flange configured at the end relative to the base; The performing trajectory planning according to the updated first pose information and the second pose information to generate a planned path includes: Trajectory planning is performed according to the fourth posture information, the updated first posture information and the second posture information to generate a planned path, wherein the planned path refers to a feasible path between the first posture and the second posture.

6. The method according to claim 5, characterized in that The step of performing error calculation according to the updated first pose information, the second pose information and the third pose information to obtain fourth pose information comprises: Calculate the ratio of the modulus value of the tool and the target point on the distance vector to the set posture threshold value according to the second posture information and the updated first posture information to obtain a first value; Calculate the product of the first value and the third posture information to obtain fourth posture information.

7. The method according to claim 5, characterized in that The performing trajectory planning according to the fourth posture information, the updated first posture information and the second posture information to generate a planned path includes: According to the fourth posture information, the set target speed and the set motion threshold, a planned path starting from the second posture and ending at the first posture is generated; wherein the target speed refers to the speed when the tool moves to the target point, and the motion threshold represents the limitation on the moving speed and moving acceleration of the tool.

8. The method according to claim 1, characterized in that In the case where the tool completes the planned path, the method further includes: Updating the first posture information and the second posture information; Calculate the modulus of the distance vector between the tool and the target point according to the updated first pose information and the updated second pose information to obtain a second value; If the second value is less than the set posture threshold, it is determined that the tool has reached the target point, and the tool is controlled to operate on the target point.

9. A robot arm control device, characterized in that: The end of the robotic arm is provided with a camera and a tool, and the device comprises: A target point recognition unit, used to perform target point recognition based on the camera, and determine the first position information of the target point relative to the base of the robotic arm; wherein the target point is a point to be operated on the target object; A first motion unit, used for controlling the tool to move to a safety point according to the first posture information; wherein the distance between the safety point and the target point is greater than a set safety distance; A calculation unit, used for calculating second position information of the tool relative to the base; A trajectory planning unit, used to update the first posture information, and perform trajectory planning according to the updated first posture information and the second posture information to generate a planned path; The second motion unit is used to control the tool to move according to the planned path, so as to operate the target point through the tool.

10. An electronic device, characterized in that: include: Memory; processor; as well as Computer programs; Wherein, the computer program is stored in the memory and is configured to be executed by the processor to implement the robot arm control method as described in any one of claims 1 to 8.

11. A computer-readable storage medium having a computer program stored thereon, characterized in that: When the computer program is executed by a processor, the steps of the robot arm control method according to any one of claims 1 to 8 are implemented.

Images