Mechanical arm path planning and calibration method and device based on random tree

Abstract

The invention provides a mechanical arm path planning and calibration method and device based on a random tree, and the method comprises the steps: constructing a kinematics equation of a mechanical arm based on kinematics parameters of the mechanical arm; selecting at least one intermediate node based on the initial position and the target position of the mechanical arm; the distance between the intermediate node and any obstacle is greater than a threshold value; parallel planning is carried out on paths between the intermediate node and any two adjacent paths in the initial position and the target position; and summarizing the paths planned in parallel, and obtaining and executing the motion trail of the mechanical arm. According to the path planning method and device, the intermediate node is selected, and the adjacent paths in the intermediate node, the initial position and the target position are planned in parallel, so that the path planning speed is greatly increased.

Description

Technical Field

[0001] The present application relates to the technical field of surgical robots, and in particular to a method and device for robot arm path planning and calibration based on a random tree. Background Art

[0002] In a navigation surgical robot, the surgical robot performs surgical planning within the reachable area and the robotic arm performs the corresponding planned operations. However, the current planning speed of the robotic arm's motion trajectory is slow. Summary of the invention

[0003] The problem that this application solves is that the planning speed of the robot arm's motion trajectory is slow.

[0004] To solve the above problems, the first aspect of the present application provides a robot path planning and calibration method based on a random tree, comprising:

[0005] Based on the kinematic parameters of the robot arm, the kinematic equation of the robot arm is constructed;

[0006] Based on the starting position and the target position of the robot arm, at least one intermediate node is selected; the distance between the intermediate node and any obstacle is greater than a threshold;

[0007] Parallel planning is performed on the paths between the intermediate node and any two adjacent ones of the starting position and the target position;

[0008] The parallel planned paths are summarized to obtain and execute the robot arm motion trajectory.

[0009] The second aspect of the present application provides a robot path planning and calibration device based on a random tree, which includes:

[0010] An equation building module, which is used to build the kinematic equations of the robot arm based on the kinematic parameters of the robot arm;

[0011] A node selection module, which is used to select at least one intermediate node based on the starting position and the target position of the robot arm; the distance between the intermediate node and any obstacle is greater than a threshold;

[0012] A path planning module, which is used to plan the paths between the intermediate node and any two adjacent ones of the starting position and the target position in parallel;

[0013] The trajectory determination module is used to summarize the parallel planned paths, obtain and execute the robot arm motion trajectory.

[0014] A third aspect of the present application provides an electronic device, comprising: a memory and a processor;

[0015] The memory is used to store programs;

[0016] The processor, coupled to the memory, is configured to execute the program to:

[0017] Based on the kinematic parameters of the robot arm, the kinematic equation of the robot arm is constructed;

[0018] Based on the starting position and the target position of the robot arm, at least one intermediate node is selected; the distance between the intermediate node and any obstacle is greater than a threshold;

[0019] Parallel planning is performed on the paths between the intermediate node and any two adjacent ones of the starting position and the target position;

[0020] The parallel planned paths are summarized to obtain and execute the robot arm motion trajectory.

[0021] A fourth aspect of the present application provides a computer-readable storage medium having a computer program stored thereon, wherein the program is executed by a processor to implement the above-mentioned random tree-based robot arm path planning and calibration method.

[0022] In the present application, the speed of path planning is greatly improved by selecting intermediate nodes and performing parallel planning on adjacent paths between the intermediate nodes and the starting position and the target position. BRIEF DESCRIPTION OF THE DRAWINGS

[0023] Figure 1 is a flow chart of a robot path planning and calibration method based on a random tree according to an embodiment of the present application;

[0024] Figure 2 A flowchart of adjacent node planning of a robot path planning and calibration method based on a random tree according to an embodiment of the present application;

[0025] Figure 3 A flowchart of the end calibration of the robot path planning and calibration method based on random tree according to an embodiment of the present application;

[0026] Figure 4 is a structural block diagram of a robot path planning and calibration device based on a random tree according to an embodiment of the present application;

[0027] Figure 5 is a structural block diagram of an electronic device according to an embodiment of the present application. DETAILED DESCRIPTION

[0028] In order to make the above-mentioned purposes, features and advantages of the present application more obvious and easy to understand, the specific embodiments of the present application are described in detail below in conjunction with the accompanying drawings. Although the exemplary embodiments of the present application are shown in the accompanying drawings, it should be understood that the present application can be implemented in various forms and should not be limited by the embodiments described herein. On the contrary, these embodiments are provided in order to enable a more thorough understanding of the present application and to fully convey the scope of the present application to those skilled in the art.

[0029] It should be noted that, unless otherwise specified, the technical terms or scientific terms used in this application should have the common meanings understood by technicians in the field to which this application belongs.

[0030] In response to the above problems, the present application provides a new random tree-based robot path planning and calibration solution, which can effectively implement force guidance and solve the problem of the robot joint space and the operator's inability to intuitively guide the joint angle to a more appropriate angle.

[0031] The present application embodiment provides a method for robot path planning and calibration based on a random tree. The specific scheme of the method is as follows: Figure 1-Figure 2 As shown, the method can be performed by a random tree-based robot path planning and calibration device, which can be integrated into electronic devices such as computers, servers, computers, server clusters, and data centers. Figure 1 As shown, it is a flow chart of a robot path planning and calibration method based on a random tree according to an embodiment of the present application; wherein the robot path planning and calibration method based on a random tree includes:

[0032] S101, constructing a kinematic equation of the robot arm based on the kinematic parameters of the robot arm;

[0033] Among them, the kinematic equation of the robotic arm is a mathematical model that describes the relationship between the force and motion of the robotic arm during movement. It can be divided into forward dynamics and inverse dynamics. The forward dynamics problem is to calculate the motion (acceleration, velocity and position) of each joint of the robotic arm when the torque or force of each joint of the robotic arm is known; the inverse dynamics problem is to calculate the joint torque or force required to achieve this motion when the motion (position, velocity and acceleration) of each joint of the robotic arm is known.

[0034] The kinematic parameters of the robotic arm include the length of the robotic arm connecting rod and the pitch angle of the rotary joint.

[0035] Analyzing the robotic arm, it can be seen that the robotic arm is formed by connecting several rods (the place where two rods are connected is a joint), and these rods in the robotic arm are called connecting rods. Each connecting rod can rotate around a certain axis based on the husband connecting rod (the previous connecting rod) (the joint drives the connecting rod to rotate through the motor rotation), and this axis is only relative to the husband connecting rod. This results in that once multiple connecting rods are moving, the connecting rod can only have a direct spatial conversion relationship with the "previous level connecting rod" and the "next level connecting rod", that is, when the rotation angle of the first connecting rod (the pitch angle of the rotating joint) is determined, the specific position of the first connecting rod can be calculated according to the spatial coordinates of the base, and then the position of the end robotic arm can be obtained by calculating backward one level at a time, which is the end position of the loading and unloading ratio.

[0036] In summary, it can be seen that in the motion state of the robot arm, the coordinates of the tool at the end of the robot arm can be determined by knowing the posture of the robot arm (corresponding data / rotation data of each joint). Conversely, if the coordinates of the tool at the end of the robot arm (expected posture) are known, the posture of the robot arm (expected joint angle of each joint) can be determined by inverse kinematics solution.

[0037] S102, based on the starting position and the target position of the robot arm, selecting at least one intermediate node; the distance between the intermediate node and any obstacle is greater than a threshold;

[0038] In this application, the starting position refers to the current end effector position of the robot arm and its corresponding joint angle.

[0039] In this application, the target position refers to the end effector position that the robot arm needs to reach and its corresponding joint angle, which is usually the target point for the robot arm to perform the task.

[0040] In the present application, an intermediate node refers to a transition point selected between a starting position and a target position, which is used to decompose a complex motion path.

[0041] In this application, when selecting the intermediate node, the distance from any obstacle is greater than the safety threshold to ensure that the robotic arm will not collide with the obstacle during movement; the position of the intermediate node should enable the robotic arm to smoothly transition to the next node without sudden changes in joint angles or path discontinuity.

[0042] S103, performing parallel planning of paths between the intermediate node and any two adjacent ones of the starting position and the target position;

[0043] In this application, the path between an intermediate node and any two adjacent ones of the starting position and the target position is to take the starting position and the target position as intermediate nodes, and arrange them with the remaining intermediate nodes in a preset order (intermediate nodes include the corresponding order when selected), and there is a path between any two adjacent intermediate nodes; there are paths between (N-1) adjacent nodes between N intermediate nodes (including the starting position and the target position). These paths can be planned in parallel.

[0044] In other words, the path from the starting position to the first intermediate node, between adjacent intermediate nodes, and from the last intermediate node to the target position is segmented, and each segmented path can be calculated independently and in parallel, reducing the overall calculation burden and overall calculation time of path planning.

[0045] S104, summarizing the parallel planned paths, obtaining and executing the robot arm motion trajectory.

[0046] In this application, all segmented planned paths are summarized to generate a complete robot arm motion trajectory; ensure that the connection between the segmented paths is smooth to avoid sudden movement changes or joint jitter of the robot arm at the nodes.

[0047] In the present application, the speed of path planning is greatly improved by selecting intermediate nodes and performing parallel planning on adjacent paths between the intermediate nodes and the starting position and the target position.

[0048] In the present application, by decomposing the starting position and the target position into multiple small segments through intermediate nodes, these segments can be planned separately, thereby reducing the complexity of the global planning and reducing the planning time.

[0049] In this application, parallel computing technology is used to simultaneously plan different segmented paths. While each path segment is independently planned, the connection with its adjacent path segments is considered to ensure the continuity and smoothness of the overall path.

[0050] In one embodiment, the aggregated overall path is smoothed to eliminate sharp corners and discontinuous parts in the path.

[0051] In one embodiment, the intermediate nodes are a first intermediate node, a second intermediate node, and a third intermediate node arranged in sequence; the second intermediate node is at the same distance from the starting position and the target position, the first intermediate node is at the same distance from the starting position and the second intermediate node, and the third intermediate node is at the same distance from the target position and the second intermediate node.

[0052] In this application, too many intermediate nodes will increase the complexity of path planning, while too few intermediate nodes may result in an uneven path or poor obstacle avoidance effect or too long calculation time.

[0053] In this application, three intermediate nodes are set, which can not only improve the planning speed, but also significantly improve the flexibility and obstacle avoidance capability of path planning.

[0054] In this application, if you plan a path directly from the starting position to the target position, you may encounter many obstacles. By setting three intermediate nodes, the complex path can be decomposed into four simple sub-paths, so that each sub-path only needs to consider local obstacle avoidance problems, thereby finding a feasible path faster.

[0055] It should be noted that the path planning between adjacent intermediate nodes is similar, and the path planning of the first intermediate node and the second intermediate node is used as an example for detailed description below.

[0056] In one embodiment, in combination Figure 2 As shown, the path planning of the first intermediate node and the second intermediate node includes:

[0057] S301, constructing a first tree from a first intermediate node to a second intermediate node, and a second tree from a second intermediate node to the first intermediate node;

[0058] In this application, a dual tree structure is constructed: the first tree: starting from the first intermediate node (starting point), expanding to the second intermediate node (target point), recording the nodes and paths in the exploration process. The second tree: starting from the second intermediate node (target point), expanding to the first intermediate node (starting point), recording the nodes and paths in the exploration process.

[0059] In this application, the structure of the tree usually includes nodes and edges. The nodes represent the state of the robot at a certain position (including position and posture), and the edges represent the feasible motion path between two nodes. The root nodes of the tree are the starting point and the target point respectively. The expansion process of the tree is to continuously add new nodes and edges in space, and finally form a tree with multiple branches.

[0060] S302, calling a random function to generate a random sampling point near the first tree, and expanding the first tree;

[0061] In the present application, a random sampling point is generated using a random function in the surrounding space of the first tree (the tree starting from the first intermediate node). The generation of this sampling point is usually limited to certain boundaries and distributions, and can be uniformly distributed or generated according to a specific probability distribution.

[0062] In the present application, the node closest to the random sampling point in the first tree (called the nearest neighbor node) is found, and then a certain step length is extended from the nearest neighbor node toward the random sampling point to generate a new node and add it to the first tree.

[0063] In this application, the generation and expansion of random sampling points can improve the global search capability of the algorithm and avoid falling into a local optimal solution.

[0064] S303, when no obstacle is encountered, taking the second tree as the target, and continuing to expand the first tree with the current step length until an obstacle is encountered;

[0065] In the present application, if no obstacle is encountered, the first tree will continue to extend in the direction of the second tree until an obstacle is encountered or a node of the second tree is touched.

[0066] In the present application, during the expansion process, the step size of each movement is the same to maintain the smoothness of the path.

[0067] In this application, during the expansion process, each time a new node is generated, obstacle detection is required. If the new node collides with an obstacle, the expansion is stopped and the node is marked as an infeasible node.

[0068] In the present application, by expanding the second tree as the target, the distance between the two trees can be effectively shortened, the connection process between the two trees can be accelerated, and the efficiency of path planning can be improved.

[0069] S304, taking the latest node of the first tree as the target point, controlling the second tree to expand with a corresponding step length until an obstacle is encountered or the target point is reached;

[0070] In this application, the second tree (the tree starting from the second intermediate node) will be expanded with the latest node of the first tree as the target point. The expansion method is similar to the first tree, that is, starting from the node closest to the target point in the second tree, moving a specified step length toward the target point to generate a new node.

[0071] In this application, each time a new node is generated, it is necessary to determine whether the node collides with an obstacle. If an obstacle is encountered, the expansion stops; if the target point can be reached, it means that the two trees are successfully connected.

[0072] In the present application, this method of bidirectional expansion and alternating expansion can effectively increase the chance of two trees meeting each other and improve the probability of finding a feasible path.

[0073] S305: Summarize the extended positions of the first tree and the second tree to obtain the planned paths of the first intermediate node and the second intermediate node.

[0074] In the present application, when the nodes of the first tree and the second tree meet, it indicates that a feasible path from the first intermediate node to the second intermediate node has been found.

[0075] In the present application, the path from the starting node to the encounter node in the first tree is concatenated with the path from the target node to the encounter node in the second tree to form a complete path.

[0076] In the present application, the construction of the dual-tree structure can enable the algorithm to find a feasible path between the start node and the target node in a shorter time, and improve the efficiency of path planning through bidirectional search.

[0077] Preferably, the summarized path is also optimized by path simplification: removing unnecessary intermediate nodes and reducing the path length; and path smoothing: using B-spline curves or other interpolation methods to smooth the path, avoiding sharp turns, and improving the stability of the robot movement.

[0078] In one implementation, the path planning of the first intermediate node and the second intermediate node further includes:

[0079] When both the first tree and the second tree encounter obstacles, a random function is called to generate a random point that is not at the obstacle, and the random point is used as the target point to control the first tree to expand;

[0080] If the first tree avoids the obstacle, continue to expand the first tree with the corresponding step size until the latest node of the second tree is reached;

[0081] If the first tree fails to avoid the obstacle, the second tree is controlled to expand;

[0082] If the second tree avoids the obstacle, the second tree is continuously expanded with the corresponding step length until the latest node of the first tree is reached;

[0083] If the second tree also fails to avoid the obstacle, the random function is called again to generate a random point that is not at the obstacle.

[0084] In this application, when the first tree and the second tree cannot continue to advance during the expansion process (due to encountering obstacles or other restrictions), it means that the current path segment may not have a direct feasible solution and a new strategy needs to be introduced. In this case, the ordinary expansion method may cause the algorithm to fall into an infinite loop or a local optimal solution.

[0085] In this application, a random function is used to generate a random sampling point in the global search space, and the point should meet the following conditions: the point is not inside any obstacle, that is, the generated point is in free space; the point is at a reasonable distance from the latest nodes of the two trees and can effectively guide path expansion.

[0086] In this application, during the dual-tree expansion process, if both trees cannot continue to advance, the chance of connecting the two trees is effectively increased by introducing new random points; the dual-tree alternating expansion strategy can make full use of the information of the two trees and avoid blind spots and bottlenecks in a single expansion path.

[0087] In one embodiment, after constructing the kinematic equation of the robotic arm based on the kinematic parameters of the robotic arm, the method further includes:

[0088] Generate a calibration rod in the working space of the robot arm;

[0089] The sixth axis of the robotic arm is recalibrated based on the calibration rod.

[0090] In the present application, the calibration rod is a tool used for calibrating a robotic arm, and can be a physical entity or a virtual reference point.

[0091] In the present application, by accurately calibrating the sixth axis, the posture deviation caused by robot arm installation error, joint offset or kinematic model error can be eliminated, thereby improving the precise positioning capability of the end effector.

[0092] In one embodiment, in combination Figure 3 As shown, the recalibrating the sixth axis of the robotic arm based on the calibration rod includes:

[0093] S501, taking the vertex of the calibration rod as a fixed point, moving the robotic arm until the end point of the robotic arm coincides with the fixed point, and recording the position and posture of the robotic arm;

[0094] In this application, the end of the robot arm (usually the tool center point, TCP) is moved to the vertex position of the calibration rod so that the end point coincides with the vertex of the calibration rod; at this time, the actual position of the end effector of the robot arm is consistent with the fixed point of the calibration rod.

[0095] It should be noted that if the end of the end tool of the robot arm is calibrated, the end of the end tool can also be used to coincide with the vertex of the calibration rod.

[0096] In this application, all joint angles and end postures (positions) of the robot arm at this position are recorded, including the rotation angle of the sixth axis. The recorded position information will be used for subsequent inverse kinematics solution and calibration.

[0097] It should be noted here that the length of the sixth axis in the present application is actually the equivalent length of the sixth axis when the end tool is installed and the end tool and the sixth axis are regarded as one body.

[0098] S502, obtaining four robot arm positions when the end point of the robot arm coincides with the fixed point;

[0099] S503, solving the length of the sixth axis according to the four robot arm postures and the inverse kinematics of the robot arm.

[0100] In the four different positions, the end point has the same position but different postures. Based on the kinematic model of the robot arm, a set of equations containing the length of the sixth axis can be established; by solving the set of equations containing the four positions, the length of the sixth axis can be accurately determined.

[0101] In the present application, accurate calibration of the length of the sixth axis can reduce the posture deviation caused by the length error during the rotation of the end effector of the robot arm, and ensure the accurate alignment and operation of the end tool in different postures.

[0102] In the present application, by accurately calibrating the length of the sixth axis, the deviation of the kinematic model can be corrected and the accuracy of the overall model can be improved.

[0103] In the present application, the length of the sixth axis is determined in order to accurately describe the offset of the end point (such as the tool center point) relative to the sixth axis during the end rotation of the robot arm, thereby improving the posture control accuracy of the robot arm.

[0104] In one embodiment, the calculation formula for the length of the sixth axis of the robotic arm is:

[0105]

[0106] Among them, d3 is the length of the third axis, d6 is the length of the sixth axis, and p x 、p y is the translation of the robot arm on the x and y axes, a x 、a y is the rotation angle of the robot arm around the x and y axes, s1 is the abbreviation of sinθ1, c1 is the abbreviation of cosθ1, and θ1 is the first joint angle of the robot arm.

[0107] An embodiment of the present application provides a random tree-based robotic arm path planning and calibration device, which is used to execute the random tree-based robotic arm path planning and calibration method described in the above content of the present application. The random tree-based robotic arm path planning and calibration device is described in detail below.

[0108] like Figure 4 As shown, the random tree-based robot path planning and calibration device includes:

[0109] An equation building module 101, which is used to build a kinematic equation of the robot arm based on the kinematic parameters of the robot arm;

[0110] A node selection module 102 is used to select at least one intermediate node based on the starting position and the target position of the robot arm; the distance between the intermediate node and any obstacle is greater than a threshold;

[0111] A path planning module 103, which is used to plan the paths between the intermediate node and any two adjacent ones of the starting position and the target position in parallel;

[0112] The trajectory determination module 104 is used to summarize the parallel planned paths, obtain and execute the robot arm motion trajectory.

[0113] In one embodiment, the intermediate nodes are a first intermediate node, a second intermediate node, and a third intermediate node arranged in sequence; the second intermediate node is at the same distance from the starting position and the target position, the first intermediate node is at the same distance from the starting position and the second intermediate node, and the third intermediate node is at the same distance from the target position and the second intermediate node.

[0114] In one implementation, the path planning module 103 is further configured to:

[0115] Construct a first tree from the first intermediate node to the second intermediate node, and a second tree from the second intermediate node to the first intermediate node; call a random function to generate a random sampling point near the first tree, and expand the first tree; if no obstacles are encountered, take the second tree as the target, and continue to expand the first tree with this step size until an obstacle is encountered; take the latest node of the first tree as the target point, and control the second tree to expand with the corresponding step size until an obstacle is encountered or the target point is reached; summarize the location points of the expansion of the first tree and the second tree to obtain the planned path of the first intermediate node and the second intermediate node.

[0116] In one implementation, the path planning module 103 is further configured to:

[0117] When both the first tree and the second tree encounter obstacles, the random function is called to generate a random point that is not in the obstacle, and the random point is used as the target point to control the first tree to expand; if the first tree avoids the obstacle, the first tree is continuously expanded with the corresponding step size until the latest node of the second tree is reached; if the first tree does not avoid the obstacle, the second tree is controlled to expand; if the second tree avoids the obstacle, the second tree is continuously expanded with the corresponding step size until the latest node of the first tree is reached; if the second tree also does not avoid the obstacle, the random function is called again to generate a random point that is not in the obstacle.

[0118] In one embodiment, the system further comprises: a calibration module, which is used to:

[0119] A calibration rod is generated in the working space of the robot arm; and the sixth axis of the robot arm is recalibrated based on the calibration rod.

[0120] In one embodiment, the calibration module is further used to:

[0121] Taking the vertex of the calibration rod as the fixed point, move the robotic arm until the end point of the robotic arm coincides with the fixed point, and record the robotic arm posture; obtain the four robotic arm postures when the end point of the robotic arm coincides with the fixed point; solve the sixth axis length based on the four robotic arm postures and the inverse kinematics of the robotic arm.

[0122] In one embodiment, the calculation formula for the length of the sixth axis of the robotic arm is:

[0123]

[0124] Among them, d3 is the length of the third axis, d6 is the length of the sixth axis, and p x 、p y is the translation of the robot arm on the x and y axes, a x 、a y is the rotation angle of the robot arm around the x and y axes, s1 is the abbreviation of sinθ1, c1 is the abbreviation of cosθ1, and θ1 is the first joint angle of the robot arm.

[0125] The random tree-based robotic arm path planning and calibration device provided in the above-mentioned embodiment of the present application has a corresponding relationship with the random tree-based robotic arm path planning and calibration method provided in the embodiment of the present application. Therefore, the specific content in the device has a corresponding relationship with the random tree-based robotic arm path planning and calibration method. The specific content can refer to the records in the random tree-based robotic arm path planning and calibration method, which will not be repeated in this application.

[0126] The random tree-based robotic arm path planning and calibration device provided in the above-mentioned embodiments of the present application and the random tree-based robotic arm path planning and calibration method provided in the embodiments of the present application are based on the same inventive concept and have the same beneficial effects as the methods adopted, run or implemented by the application programs stored therein.

[0127] The above describes the internal functions and structures of the random tree-based robot path planning and calibration device, such as Figure 5 As shown, in practice, the random tree-based robot path planning and calibration device can be implemented as an electronic device, including: a memory 301 and a processor 303.

[0128] The memory 301 may be configured to store programs.

[0129] In addition, the memory 301 may also be configured to store various other data to support operations on the electronic device. Examples of such data include instructions for any application or method operating on the electronic device, contact data, phone book data, messages, pictures, videos, etc.

[0130] The memory 301 can be implemented by any type of volatile or non-volatile storage device or a combination thereof, such as static random access memory (SRAM), electrically erasable programmable read-only memory (EEPROM), erasable programmable read-only memory (EPROM), programmable read-only memory (PROM), read-only memory (ROM), magnetic memory, flash memory, magnetic disk or optical disk.

[0131] The processor 303 is coupled to the memory 301 and is configured to execute a program in the memory 301 to:

[0132] Based on the kinematic parameters of the robot arm, the kinematic equation of the robot arm is constructed;

[0133] Based on the starting position and the target position of the robot arm, at least one intermediate node is selected; the distance between the intermediate node and any obstacle is greater than a threshold;

[0134] Parallel planning is performed on the paths between the intermediate node and any two adjacent ones of the starting position and the target position;

[0135] The parallel planned paths are summarized to obtain and execute the robot arm motion trajectory.

[0136] In one embodiment, the intermediate nodes are a first intermediate node, a second intermediate node, and a third intermediate node arranged in sequence; the second intermediate node is at the same distance from the starting position and the target position, the first intermediate node is at the same distance from the starting position and the second intermediate node, and the third intermediate node is at the same distance from the target position and the second intermediate node.

[0137] In one implementation, the processor 303 is further configured to:

[0138] Construct a first tree from the first intermediate node to the second intermediate node, and a second tree from the second intermediate node to the first intermediate node; call a random function to generate a random sampling point near the first tree, and expand the first tree; if no obstacles are encountered, take the second tree as the target, and continue to expand the first tree with this step size until an obstacle is encountered; take the latest node of the first tree as the target point, and control the second tree to expand with the corresponding step size until an obstacle is encountered or the target point is reached; summarize the location points of the expansion of the first tree and the second tree to obtain the planned path of the first intermediate node and the second intermediate node.

[0139] In one implementation, the processor 303 is further configured to:

[0140] When both the first tree and the second tree encounter obstacles, the random function is called to generate a random point that is not in the obstacle, and the random point is used as the target point to control the first tree to expand; if the first tree avoids the obstacle, the first tree is continuously expanded with the corresponding step size until the latest node of the second tree is reached; if the first tree does not avoid the obstacle, the second tree is controlled to expand; if the second tree avoids the obstacle, the second tree is continuously expanded with the corresponding step size until the latest node of the first tree is reached; if the second tree also does not avoid the obstacle, the random function is called again to generate a random point that is not in the obstacle.

[0141] In one implementation, the processor 303 is further configured to:

[0142] A calibration rod is generated in the working space of the robot arm; and the sixth axis of the robot arm is recalibrated based on the calibration rod.

[0143] In one implementation, the processor 303 is further configured to:

[0144] Taking the vertex of the calibration rod as the fixed point, move the robotic arm until the end point of the robotic arm coincides with the fixed point, and record the robotic arm posture; obtain the four robotic arm postures when the end point of the robotic arm coincides with the fixed point; solve the sixth axis length based on the four robotic arm postures and the inverse kinematics of the robotic arm.

[0145] In one embodiment, the calculation formula for the length of the sixth axis of the robotic arm is:

[0146]

[0147] Among them, d3 is the length of the third axis, d6 is the length of the sixth axis, and p x 、p y is the translation of the robot arm on the x and y axes, a x 、a y is the rotation angle of the robot arm around the x and y axes, s1 is the abbreviation of sinθ1, c1 is the abbreviation of cosθ1, and θ1 is the first joint angle of the robot arm.

[0148] In the present application, the processor is also specifically used to execute all processes and steps of the above-mentioned random tree-based robotic arm path planning and calibration method. The specific content can refer to the records in the random tree-based robotic arm path planning and calibration method, and will not be repeated in this application.

[0149] In this application, Figure 5 Only some components are shown schematically, which does not mean that the electronic device only includes Figure 5 Components shown.

[0150] The electronic device provided in this embodiment is based on the same inventive concept as the random tree-based robot path planning and calibration method provided in the embodiment of the present application, and has the same beneficial effects as the method adopted, run or implemented by the application stored therein.

[0151] Those skilled in the art will appreciate that the embodiments of the present application may be provided as methods, systems, or computer program products. Therefore, the present application may adopt the form of a complete hardware embodiment, a complete software embodiment, or an embodiment in combination with software and hardware. Moreover, the present application may adopt the form of a computer program product implemented in one or more computer-readable storage media (including but not limited to disk storage, CD-ROM, optical storage, etc.) that contain computer-usable program code.

[0152] The present application is described with reference to the flowcharts and / or block diagrams of the methods, devices (systems), and computer program products according to the embodiments of the present application. It should be understood that each process and / or box in the flowchart and / or block diagram, as well as the combination of the processes and / or boxes in the flowchart and / or block diagram, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, a special-purpose computer, an embedded processor, or other programmable data processing device to generate a machine, so that the instructions executed by the processor of the computer or other programmable data processing device generate instructions for implementing the processes in the flowchart and / or block diagram. Figure 1 A process or multiple processes and / or boxes Figure 1 A device that provides the functions specified in a block or multiple blocks.

[0153] These computer program instructions may also be stored in a computer readable memory capable of directing a computer or other programmable data processing device to operate in a specific manner, so that the instructions stored in the computer readable memory produce an article of manufacture including an instruction device, which implements the process Figure 1 A process or multiple processes and / or boxes Figure 1 A function specified in one or more boxes.

[0154] These computer program instructions can also be loaded onto a computer or other programmable data processing device so that a series of operating steps are executed on the computer or other programmable device to produce a computer-implemented process, thereby providing instructions for implementing the process in the computer or other programmable device. Figure 1 A process or multiple processes and / or boxes Figure 1 The steps for the functions specified in one or more boxes.

[0155] In a typical configuration, a computing device includes one or more processors (CPU), input / output interfaces, network interfaces, and memory.

[0156] The memory may include non-permanent storage in a computer-readable medium, random access memory (RAM) and / or non-volatile memory in the form of read-only memory (ROM) or flash RAM. The memory is an example of a computer-readable medium.

[0157] The present application also provides a computer-readable storage medium corresponding to the random tree-based robot arm path planning and calibration method provided in the aforementioned embodiment, on which a computer program (i.e., a program product) is stored. When the computer program is run by a processor, it will execute the random tree-based robot arm path planning and calibration method provided in any of the aforementioned embodiments.

[0158] Computer readable media include permanent and non-permanent, removable and non-removable media that can be implemented by any method or technology to store information. Information can be computer readable instructions, data structures, program modules or other data. Examples of computer storage media include, but are not limited to, phase change memory (PRAM), static random access memory (SRAM), dynamic random access memory (DRAM), other types of random access memory (RAM), read-only memory (ROM), electrically erasable programmable read-only memory (EEPROM), flash memory or other memory technology, compact disk read-only memory (CD-ROM), digital versatile disk (DVD) or other optical storage, magnetic cassettes, magnetic tape magnetic disk storage or other magnetic storage devices or any other non-transmission media that can be used to store information that can be accessed by a computing device. As defined herein, computer readable media does not include temporary computer readable media (transitory media), such as modulated data signals and carrier waves.

[0159] The computer-readable storage medium provided in the above-mentioned embodiments of the present application and the random tree-based robot path planning and calibration method provided in the embodiments of the present application are based on the same inventive concept and have the same beneficial effects as the methods adopted, run or implemented by the application programs stored therein.

[0160] It should be noted that in the description provided herein, a large number of specific details are described. However, it is understood that the embodiments of the present application can be practiced without these specific details. In some instances, well-known structures and technologies are not shown in detail so as not to obscure the understanding of this description.

[0161] It should also be noted that the terms "include", "comprises" or any other variations thereof are intended to cover non-exclusive inclusion, so that a process, method, commodity or device including a series of elements includes not only those elements, but also other elements not explicitly listed, or also includes elements inherent to such process, method, commodity or device. In the absence of more restrictions, the elements defined by the sentence "comprises a ..." do not exclude the existence of other identical elements in the process, method, commodity or device including the elements.

[0162] The above is only an embodiment of the present application and is not intended to limit the present application. For those skilled in the art, the present application may have various changes and variations. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present application should be included in the scope of the claims of the present application.

Claims

1. A robot path planning and calibration method based on random tree, characterized in that: include: Based on the kinematic parameters of the robot arm, the kinematic equation of the robot arm is constructed; Based on the starting position and the target position of the robot arm, at least one intermediate node is selected; The distance between the intermediate node and any obstacle is greater than a threshold; Parallel planning is performed on the paths between the intermediate node and any two adjacent ones of the starting position and the target position; The parallel planned paths are summarized to obtain and execute the robot arm motion trajectory.

2. The random tree-based robot path planning and calibration method according to claim 1, characterized in that: The intermediate nodes are the first intermediate node, the second intermediate node and the third intermediate node which are arranged in sequence; the second intermediate node is at the same distance from the starting position and the target position, the first intermediate node is at the same distance from the starting position and the second intermediate node, and the third intermediate node is at the same distance from the target position and the second intermediate node.

3. The random tree-based robot path planning and calibration method according to claim 2, characterized in that: The path planning of the first intermediate node and the second intermediate node includes: Construct a first tree from the first intermediate node to the second intermediate node, and a second tree from the second intermediate node to the first intermediate node; Call a random function to generate a random sampling point near the first tree and expand the first tree; If no obstacle is encountered, the second tree is taken as the target and the first tree is continuously expanded with the current step length until an obstacle is encountered; Taking the latest node of the first tree as the target point, control the second tree to expand with the corresponding step length until an obstacle is encountered or the target point is reached; The location points expanded from the first tree and the second tree are summarized to obtain the planned paths of the first intermediate node and the second intermediate node.

4. The random tree-based robot path planning and calibration method according to claim 3, characterized in that: The path planning of the first intermediate node and the second intermediate node further includes: When both the first tree and the second tree encounter obstacles, a random function is called to generate a random point that is not at the obstacle, and the random point is used as the target point to control the first tree to expand; If the first tree avoids the obstacle, continue to expand the first tree with the corresponding step size until the latest node of the second tree is reached; If the first tree fails to avoid the obstacle, the second tree is controlled to expand; If the second tree avoids the obstacle, the second tree is continuously expanded with the corresponding step length until the latest node of the first tree is reached; If the second tree also fails to avoid the obstacle, the random function is called again to generate a random point that is not at the obstacle.

5. The method for manipulator path planning and calibration based on random tree according to any one of claims 1 to 4, characterized in that: After constructing the kinematic equation of the robotic arm based on the kinematic parameters of the robotic arm, the method further includes: Generate a calibration rod in the working space of the robot arm; The sixth axis length of the robotic arm is recalibrated based on the calibration rod.

6. The method for robot path planning and calibration based on random tree according to claim 5, characterized in that: The recalibrating the length of the sixth axis of the robotic arm based on the calibration rod includes: Taking the vertex of the calibration rod as a fixed point, moving the robotic arm until the end point of the robotic arm coincides with the fixed point, and recording the position and posture of the robotic arm; Obtaining four robot arm positions when the end point of the robot arm coincides with the fixed point; According to the four robot arm postures and the inverse kinematics of the robot arm, the length of the sixth axis is solved.

7. The random tree-based robot path planning and calibration method according to claim 6, characterized in that: The calculation formula of the sixth axis length of the robotic arm is: Among them, d3 is the length of the third axis, d6 is the length of the sixth axis, and p x 、p y is the translation of the robot arm on the x and y axes, a x 、a y is the rotation angle of the robot arm around the x and y axes, s1 is the abbreviation of sinθ1, c1 is the abbreviation of cosθ1, and θ1 is the first joint angle of the robot arm.

8. A robot path planning and calibration device based on random tree, characterized in that: include: An equation building module, which is used to build the kinematic equations of the robot arm based on the kinematic parameters of the robot arm; A node selection module, which is used to select at least one intermediate node based on the starting position and the target position of the robot arm; the distance between the intermediate node and any obstacle is greater than a threshold; A path planning module, which is used to plan the paths between the intermediate node and any two adjacent ones of the starting position and the target position in parallel; The trajectory determination module is used to summarize the parallel planned paths, obtain and execute the robot arm motion trajectory.

9. An electronic device, characterized in that: include: Memory and processor; The memory is used to store programs; The processor, coupled to the memory, is configured to execute the program to: Based on the kinematic parameters of the robot arm, the kinematic equation of the robot arm is constructed; Based on the starting position and the target position of the robot arm, at least one intermediate node is selected; the distance between the intermediate node and any obstacle is greater than a threshold; Parallel planning is performed on the paths between the intermediate node and any two adjacent ones of the starting position and the target position; The parallel planned paths are summarized to obtain and execute the robot arm motion trajectory.

10. A computer-readable storage medium having a computer program stored thereon, characterized in that: The program is executed by the processor to implement the random tree-based robot arm path planning and calibration method described in any one of claims 1-7.

Images