Mechanical motion control method and apparatus

Abstract

The application relates to a mechanical motion control method, comprising the steps of: acquiring a starting point and an ending point to be planned, and constructing a first path tree and a second path tree by taking the starting point and the ending point as root nodes; for each random sampling point in a first sampling set and a second sampling set, determining the node closest to the random sampling point in the first path tree and the second path tree as a growth node; for each growth node, generating a new node at a growth step length in the direction of the growth node pointing to the corresponding random sampling point; for each new node, determining the growth node corresponding to the new node as the parent node of the new node; for each new node, if there is an obstacle within a certain range centered on the straight line path between the new node and the corresponding parent node, removing the new node; and if a node in the first path tree coincides with a node in the second path tree in position, determining the shortest path between the two nodes according to the first path tree and the second path tree as a motion control path.

Description

Technical Field

[0001] This invention relates to the field of mechanical motion control technology, and in particular to a mechanical motion control method and device. Background Technology

[0002] With the application and development of mechanical motion control technology in industrial production, the application scenarios of moving machinery are becoming increasingly diverse, such as robotic arms. Mechanical motion control includes motion path planning, controlling the moving machinery to move according to the planned motion path, and controlling the speed of the moving machinery. Currently, when using mechanical motion control technology to control the motion of moving machinery, the slow speed of motion path planning often fails to meet the real-time requirements of mechanical motion. Summary of the Invention

[0003] The purpose of this invention is to overcome the shortcomings and deficiencies of the prior art and provide a mechanical motion control method that can improve path planning speed, thereby meeting the real-time requirements of mechanical motion.

[0004] This invention is achieved through the following technical solution: a mechanical motion control method, comprising the following steps:

[0005] Obtain the starting point and ending point to be planned, construct a first path tree with the starting point as the root node, and construct a second path tree with the ending point as the root node;

[0006] Generate several random sampling points in the planned space;

[0007] The random sampling points that are farthest from the starting point are determined as a first sampling set, and the remaining random sampling points are determined as a second sampling set;

[0008] For each random sampling point in the first sampling set, the node in the first path tree closest to that random sampling point is determined as the growth node;

[0009] For each random sampling point in the second sampling set, the node in the second path tree closest to that random sampling point is determined as the growth node;

[0010] For each growth node, a new node is generated at the growth step length in the direction pointing from the growth node to the corresponding random sampling point.

[0011] For each new node, the growth node corresponding to the new node is determined as its parent node;

[0012] For each new node, if there is an obstacle within a certain range centered on the straight path between the new node and its corresponding parent node, then the new node is removed.

[0013] If a node in the first path tree coincides with a node in the second path tree, then the shortest path between the two root nodes is determined as the motion control path based on the first path tree and the second path tree.

[0014] Compared with existing technologies, the mechanical motion control method of the present invention constructs a path tree with the starting point and the ending point as root nodes simultaneously through bidirectional sampling, which quickly obtains multiple feasible paths between the starting point and the ending point. The motion control path is finally obtained based on the shortest path in the two path trees, thereby improving the speed of motion path planning and meeting the real-time requirements of the moving machinery.

[0015] Furthermore, for each new node, after determining that the growth node corresponding to the new node is its parent node, the method includes the following steps: for each new node, calculate the path length from the new node to the corresponding root node when each node in the same path tree is the parent node of the new node, and update the parent node of the new node to the node corresponding to the minimum path length.

[0016] Further, for each growth node, after generating a new node at the growth step size in the direction pointing to the corresponding random sampling point, the process includes the following steps:

[0017] For each new node, nodes within a preset range centered on the new node are identified as reconnection nodes;

[0018] For each reconnected node, if setting the corresponding new node as the parent node of the reconnected node reduces the path length from the reconnected node to the corresponding root node, then the new node is set as the parent node of the reconnected node.

[0019] Further, for each growth node, after generating a new node at the growth step size in the direction pointing to the corresponding random sampling point, the process includes the following steps:

[0020] For each new node, if there is an obstacle within a certain range centered on the straight path between the new node and its corresponding parent node, then the growth step size is reduced.

[0021] For each new node, if there are no obstacles within a certain range centered on the straight path between the new node and its corresponding parent node, then the growth step size is increased.

[0022] Furthermore, several random sampling points are generated in the planned space, including the following steps:

[0023] Obtain the environmental complexity, determine the closed radius based on the environmental complexity, and determine the closed radius region centered on each node in the first path tree and the second path tree as the closed region;

[0024] Several random sampling points are generated in the planned space, and the random sampling points generated in the closed area are removed.

[0025] Further, for each growth node, a new node is generated at the growth step size in the direction pointing from the growth node to the corresponding random sampling point, including the following steps:

[0026] For each growth node, if the distance between the growth node and the corresponding random sampling point is greater than the growth step size, then the new node is generated at the growth step size in the direction from the growth node to the corresponding random sampling point.

[0027] For each growth node, if the distance between the growth node and the corresponding random sampling point is less than the growth step size, then the random sampling point is determined to be the new node.

[0028] Further, after determining the shortest path between the two root nodes as the motion control path based on the first path tree and the second path tree, the process includes the following steps:

[0029] Cubic spline interpolation is performed between adjacent nodes of the motion control path to divide the path between each adjacent node into three variable-speed sub-paths. The path equation expression for the first sub-path is as follows:

[0030] h1(t j1 ) = a 10 +a 11 t j1 +a 12 t j1 2 +a 13 t j1 3

[0031] Among them, t j1 Let a be the time point of motion on the first segment of the path. 10 ,a 11 ,a 12 ,a 13 These are the polynomial coefficients;

[0032] The path equation for the second sub-path is:

[0033] h2(t j2 ) = a 20 +a 21 t j2 +a 22 t j2 2 +a 23 t j2 3 +a 24 tj2 4 +a 25 t j2 5

[0034] Among them, t j2 For the time point of movement on the second sub-path, a 20 ,a 21 ,a 22 ,a 23 ,a 24 ,a 25 These are the polynomial coefficients;

[0035] The path equation for the third sub-path is expressed as follows:

[0036] h3(t j3 ) = a 30 +a 31 t j3 +a 32 t j3 2 +a 33 t j3 3

[0037] Among them, t j3 Let a be the time point of movement on the third sub-path. 30 ,a 31 ,a 32 ,a 33 These are the polynomial coefficients;

[0038] Based on the fact that the acceleration a1(0) at the beginning of the first sub-path and the acceleration a3(f) at the end of the third sub-path are both 0, and the moving speed v1(0) at the beginning of the first sub-path and the moving speed v3(f) at the end of the third sub-path are both 0, determine the values ​​of the coefficients of each polynomial.

[0039] The movement speed at each time point is determined based on the path equation of the sub-path.

[0040] Further, after performing cubic spline interpolation between adjacent nodes of the motion control path to divide the path between each adjacent node into three variable-speed sub-paths, the following steps are included:

[0041] The target machine is controlled to move according to the motion control path and the speed corresponding to each time point. When the target machine stops moving, a movement end signal is generated.

[0042] The actual position of the target machine is obtained based on the movement end signal. If the distance between the actual position and the endpoint is greater than a preset threshold, the straight path between the actual position and the endpoint is set as the motion control path.

[0043] Based on the same inventive concept, the present invention also provides a mechanical motion control device, comprising:

[0044] The root node determination module is used to obtain the starting point and the ending point to be planned, construct a first path tree with the starting point as the root node, and construct a second path tree with the ending point as the root node.

[0045] The sampling point generation module is used to generate several random sampling points in the planned space;

[0046] The sampling set determination module is used to determine a number of random sampling points that are farthest from the starting point as the first sampling set, and the remaining random sampling points as the second sampling set;

[0047] The first growth node determination module is used to determine, for each random sampling point in the first sampling set, the node in the first path tree closest to the random sampling point as the growth node;

[0048] The second growth node determination module is used to determine, for each random sampling point in the second sampling set, the node in the second path tree that is closest to the random sampling point as the growth node;

[0049] The new node generation module is used to generate a new node for each growth node at the growth step length in the direction of the growth node pointing to the corresponding random sampling point.

[0050] The parent node determination module is used to determine the growth node corresponding to each new node as its parent node.

[0051] An obstacle detection module is used to remove a new node if an obstacle exists within a certain range centered on the straight path between the new node and its corresponding parent node for each new node.

[0052] The path determination module is used to determine the shortest path between the two root nodes as the motion control path if a node in the first path tree coincides with a node in the second path tree.

[0053] Furthermore, the parent node determination module is used to calculate, for each new node, the path length from the new node to the corresponding root node when each node in the same path tree is the parent node of the new node, and update the parent node of the new node to the node corresponding to the minimum path length.

[0054] To better understand and implement this invention, the following detailed description is provided in conjunction with the accompanying drawings. Attached Figure Description

[0055] Figure 1 This is a schematic diagram of the structure of a mechanical motion control system according to an embodiment;

[0056] Figure 2 This is a schematic diagram of the structure of motion control device B in the embodiment;

[0057] Figure 3 This is a flowchart illustrating the mechanical motion control method in an embodiment. Detailed Implementation

[0058] To make the objectives, technical solutions, and advantages of this application clearer, the embodiments of this application will be described in further detail below with reference to the accompanying drawings.

[0059] Please see Figure 1 This is a schematic diagram of a mechanical motion control system according to this embodiment. The system includes a camera device A and a motion control device B. Camera device A is used to acquire a mechanical motion space map and a mechanical end-effector image. The mechanical motion space map includes an image of the entire space accessible by the mechanical motion; the mechanical end-effector image includes an image of the end of the movable part of the mechanical device. During system operation, camera device A is positioned in a fixed location and does not move with the mechanical end-effector. In this embodiment, camera device A is preferably a binocular vision camera, which acquires depth information of the object being photographed while acquiring images. Motion control device B is used to plan a motion control path based on the mechanical motion space map and mechanical end-effector image acquired by camera device A, and control the mechanical movement according to the motion control path, while simultaneously controlling the machine's speed.

[0060] For details, please refer to the following: Figure 2 and Figure 3 ,in Figure 2 This is a schematic diagram of the motion control device B in this embodiment; Figure 3 This is a flowchart illustrating the mechanical motion control method executed by the motion control device B. The motion control device B includes a path planning unit 10, a speed planning unit 20, a movement control unit 30, and a path adjustment unit 40. The path planning unit 10 is used to execute step S1: obtaining the starting point and ending point to be planned, and determining the motion control path based on the starting point and ending point.

[0061] In step S1, the movable space of the target machine to be controlled is used as the planning space. The starting point is the initial position of the target machine's movement within the planning space. The initial position of the target machine's movement can be its current location or a specified location. The ending point is the final position of the target machine's movement within the planning space. The final position of the target machine's movement is usually a specified location.

[0062] Furthermore, the path planning unit 10 includes a root node determination module 11, a sampling point generation module 12, a sampling set determination module 13, a first growth node determination module 14a, a second growth node determination module 14b, a new node generation module 15, a parent node determination module 16, a node reconnection module 17, an obstacle detection module 18, and a path determination module 19.

[0063] The root node determination module 11 is used to execute step S11: obtain the starting point and ending point to be planned, construct the first path tree with the starting point as the root node, and construct the second path tree with the ending point as the root node.

[0064] In step S11, the starting point is determined as the root node of the first path tree, and the ending point is determined as the root node of the second path tree, thus beginning the construction of the first and second path trees. Both the first and second path trees will include a root node and multiple child nodes. Adjacent nodes with a parent-child relationship are connected by a straight line, which represents the path between adjacent nodes. The length of the connecting line is the straight-line distance between adjacent nodes in the planning space, i.e., the path length between adjacent nodes.

[0065] The sampling point generation module 12 is used to perform step S12: generate several random sampling points in the planning space.

[0066] The random sampling points are points at random locations in the planning space, and can be generated randomly using a greedy growth strategy. The number of random sampling points generated each time can be set appropriately according to the specific implementation. For example, in one specific implementation, the number of random sampling points generated each time is set to 14, which is more beneficial for path planning speed.

[0067] Random sampling points will serve as a reference for the generation direction of new nodes in the subsequent first path tree and second path tree. In order to reduce the sampling area and improve the planning speed, the sampling point generation module 12 further includes a closed area determination submodule 121 and a sampling submodule 122. The closed area determination submodule 121 is used to perform step S121: obtain the environmental complexity, determine the closed radius according to the environmental complexity, and determine the closed radius area centered on each node in the first path tree and the second path tree as the closed area.

[0068] Among them, environmental complexity refers to the proportion of obstacles in the planned space. The closed radius is determined based on the environmental complexity; the greater the environmental complexity, the smaller the closed radius.

[0069] The sampling submodule 122 is used to perform step S122: generate several random sampling points in the planning space and remove the random sampling points generated in the closed area.

[0070] The sampling set determination module 13 is used to execute step S13: determine the first sampling set of several random sampling points that are farthest from the starting point, and the remaining random sampling points as the second sampling set.

[0071] In another implementation, step S13 can be replaced by determining a number of random sampling points that are farthest from the endpoint as the second sampling set, and the remaining random sampling points as the first sampling set.

[0072] The first growth node determination module 14a is used to perform step S14a: for each random sampling point in the first sampling set, determine the node in the first path tree that is closest to the random sampling point as the growth node.

[0073] The second growth node determination module 14b is used to perform step S14b: for each random sampling point in the second sampling set, determine the node in the second path tree that is closest to the random sampling point as the growth node.

[0074] The new node generation module 15 is used to perform step S15: for each growth node, a new node is generated at the growth step length in the direction of the growth node pointing to the corresponding random sampling point.

[0075] The growth step size is the distance between the new node to be generated and the growing node. The growth step size can be set reasonably according to the complexity of the environment. It can be a fixed value or dynamically adjusted according to certain conditions.

[0076] Each growth node is determined based on a random sampling point, which is the random sampling point corresponding to the growth node. The growth direction is the direction from the growth node to the random sampling point. A new node is generated in this growth direction at a growth step size away from the growth node.

[0077] Furthermore, the new node generation module 15 includes a first generation submodule 15a and a second generation submodule 15b. The first generation submodule 15a is used to perform step S15a: for each growth node, if the distance between the growth node and the corresponding random sampling point is greater than the growth step size, a new node is generated at the growth step size in the direction from the growth node to the corresponding random sampling point.

[0078] The second generation submodule 15b is used to execute step S15a: for each growth node, if the distance between the growth node and the corresponding random sampling point is less than the growth step size, then the random sampling point is determined to be a new node.

[0079] The parent node determination module 16 is used to perform step S16: for each new node, determine the parent node of the new node.

[0080] In order to ultimately obtain a motion control path with a smaller distance, in a preferred embodiment, the parent node determination module 16 includes a pre-determination sub-module 161 and a parent node update sub-module 162. The pre-determination sub-module 161 is used to perform step S161: for each new node, determine the growth node corresponding to the new node as its parent node.

[0081] The parent node update submodule 162 is used to execute step S162: For each new node, calculate the path length from the new node to the corresponding root node when each node in the same path tree is the parent node of the new node, and determine the node corresponding to the minimum path length as the parent node of the new node.

[0082] The path length from the new node to the corresponding root node is the shortest path length from the root node along the path in the path tree to the new node. For each new node, if a node in the same path tree is the parent node of the new node, and the path length from the new node to the root node of the same path tree is the shortest, then that node is determined to be the parent node of the new node, which can ultimately obtain a motion control path with a smaller distance.

[0083] The node reconnection module 17 is used to perform step S17: reconnect the nodes in the first path tree and the second path tree so that a motion control path with a smaller distance can be obtained in the end.

[0084] Specifically, the node reconnection module 17 includes a reconnection node determination submodule 171 and a reconnection submodule 172. The reconnection node determination submodule 171 is used to perform step S171: for each new node, determine the nodes within a preset range centered on the new node as reconnection nodes.

[0085] The reconnection submodule 172 is used to execute step S172: For each reconnection node, if setting the corresponding new node as the parent node of the reconnection node reduces the path length from the reconnection node to the corresponding root node, then the new node is set as the parent node of the reconnection node. Simultaneously setting the new node as the parent node of the reconnection node allows the reconnection node to be disconnected from its original parent node.

[0086] The obstacle detection module 18 is used to perform step S18: obstacle avoidance processing based on whether there are obstacles within a certain range centered on the straight path between the new node and the corresponding parent node.

[0087] Furthermore, the obstacle detection module 18 includes a node filtering submodule 18a, a step size decreasing submodule 18b, and a step size increasing submodule 18c. The node filtering submodule 18a is used to perform step S18a: for each new node, if there is an obstacle within a certain range centered on the straight path between the new node and its corresponding parent node, then the new node is removed.

[0088] The step size reduction submodule 18b is used to execute step S18b: for each new node, if there are obstacles within a certain range centered on the straight path between the new node and the corresponding parent node, then reduce the growth step size.

[0089] The step size increase submodule 18c is used to execute step S18c: for each new node, if there are no obstacles within a certain range centered on the straight path between the new node and the corresponding parent node, then increase the growth step size.

[0090] The coordinates of the obstacles in the planning space are obtained based on the mechanical motion control diagram. The acquisition steps include: acquiring two mechanical motion control diagrams, left and right, captured by a binocular vision camera, and performing grayscale processing on the mechanical motion control diagrams; extracting edges from the grayscale mechanical motion control diagrams using the Canny operator to obtain multiple connected components; determining the largest connected component as the obstacle region; obtaining the minimum bounding rectangle of the obstacle region, and obtaining the vertex coordinates of the minimum bounding rectangle in the planning space using a binocular vision positioning algorithm.

[0091] The path determination module 19 is used to execute step S19: If a node in the first path tree coincides with a node in the second path tree, then the shortest path between the two nodes is determined as the motion control path based on the first and second path trees. If no node in the first path tree coincides with a node in the second path tree, then steps S12-S18 are repeated to generate nodes for the first and second path trees.

[0092] Alternatively, step S19 can be as follows: If a node in the first path tree is less than a set threshold in distance from a node in the second path tree, then the shortest path between the two nodes is determined as the motion control path based on the first and second path trees. If no node in the first path tree is less than a set threshold in distance from a node in the second path tree, then steps S12-S18 are repeated to generate nodes for the first and second path trees.

[0093] The shortest path between two nodes is the path corresponding to the shortest path length from the starting root node along the first path tree and the second path tree to the ending root node.

[0094] In specific implementation, Dijkstra's algorithm can be used to generate an undirected path graph based on the first and second path trees. The shortest path from the starting point to the ending point is determined based on the undirected path graph: declare an array dis to store the shortest path length from the root node to each node, and use a set T to store the nodes for which the shortest path has been found; initially, the path weight of the root node s is assigned to 0 (dis[s] = 0). If for any node m, there exists an edge (s, m) that the root node s can directly reach, then let dis[m] = w(s, m), and at the same time set the path length of all other nodes that the root node s cannot directly reach to infinity; check whether the connection between two nodes encounters an obstacle, and whether the path length from the node to other nodes is shorter than that from the root node. If so, replace the values ​​of these nodes in the array dis; select the minimum value in the array dis and add it to the set T. This is considered as completing the addition of a node; repeat the above steps until the set T contains all the nodes of the undirected path graph.

[0095] The speed planning unit 20 is used to execute step S2: perform cubic spline interpolation between adjacent nodes of the motion control path to divide the path between each adjacent node into three sub-path segments.

[0096] In the process of performing cubic spline interpolation between adjacent nodes of the motion control path, the path between adjacent nodes is divided into three variable-speed sub-paths, and the path equations of the three sub-paths are as follows:

[0097] h1t j1 ) = a 10 +a 11 t j1 +a 12 t j1 2 +a 13 t j1 3

[0098] h2(t j2 ) = a 20 +a 21 t j2 +a 22 t j2 2 +a 23 t j2 3 +a 24 t j2 4 +a 25 t j2 5

[0099] h3(t j3 ) = a30 +a 31 t j3 +a 32 t j3 2 +a 33 t j3 3

[0100] Among them, h1(t) j1 ) represents the path equation for the first sub-path segment, h2(t j2 h3(t) is the path equation for the second sub-path. j3 ) represents the path equation for the third sub-path; t j1 Let t be the time point of movement on the first sub-path. j2 Let t be the time point of movement on the second sub-path. j3 Let a be the time point of movement on the third sub-path; ij =[a 10 ,a 11 ,a 12 ,a 13 ,a 20 ,a 21 ,a 22 ,a 23 ,a 24 ,a 25 ,a 30 ,a 31 ,a 32 ,a 33 ] represents the polynomial coefficients.

[0101] The movement speed of the target machine on the sub-path is obtained based on the path equation, where the movement speed v1(t) on the first sub-path segment is... j1 The movement speed v2(t) on the second sub-path j2 ) and the movement speed v3(t) on the third sub-path j3 The expressions for ) are as follows:

[0102]

[0103]

[0104]

[0105] The acceleration of the target machine on the sub-path is obtained from the path equation, where the acceleration a1(t) on the first segment of the sub-path is... j1 ), the acceleration a2(t) on the second sub-path j2 ) and the acceleration a3(t) on the third sub-path j3 The expressions for ) are as follows:

[0106]

[0107]

[0108]

[0109] Let the acceleration a1(0) at the start of the first sub-path and the acceleration a3() at the end of the third sub-path be 0, and let the moving speed v1(0) at the start of the first sub-path and the moving speed v3() at the end of the third sub-path be 0, so as to obtain the values ​​of the polynomial coefficients. By obtaining the values ​​of the polynomial coefficients, the moving speed corresponding to different time points on each sub-path can be determined.

[0110] The motion control unit 30 is used to execute step S3: control the target machine to move according to the motion control path and the speed corresponding to each time point, and generate a movement end signal when the target machine stops moving.

[0111] The path adjustment unit 40 is used to execute step S4: obtain the actual position of the target machine according to the movement end signal. If the distance between the actual position and the endpoint is greater than the preset threshold, set the straight path between the actual position and the endpoint as the motion control path, and execute steps S2-S3 again.

[0112] The process of obtaining the actual position of the target machine based on the end-effector images includes the following steps: acquiring two end-effector images of the machine captured by a binocular vision camera; performing stereo matching on the two end-effector images to obtain the centroid position of the target machine in the two end-effector images; performing three-dimensional reconstruction based on the two end-effector images using the least squares method to obtain the three-dimensional coordinate system of the planned space; and obtaining the pose of the target machine in the three-dimensional coordinate system of the planned space using the SIFT algorithm based on the centroid position of the target machine in the two end-effector images.

[0113] To obtain the centroid position of the target machine by stereo matching of two images of the mechanical end effector, the steps include: obtaining the rotation matrix R and translation matrix T of the right camera relative to the left camera in the binocular vision camera; taking the root of the rotation matrix R to obtain a composite rotation matrix r that makes the optical axes of the left and right cameras parallel; and constructing a transformation matrix R based on the translation matrix T that makes the baseline parallel to the imaging plane. rect Its expression is:

[0114]

[0115]

[0116]

[0117] e3 = 1 × e2

[0118] Based on the synthesis rotation matrix r and transformation matrix R rect Obtain the global rotation matrix R of the left camera l And the overall rotation matrix R with the camera r R r = l = rect r, through the global rotation matrix R of the left camera l And the overall rotation matrix R with the camera r Achieve coplanar and parallel alignment of images captured by the left and right cameras;

[0119] Obtain the centroid of the target machine in the target machine template drawing. in, (x i ,y i (i) represents the coordinates of the i-th pixel in the template image; the centroid of the target machine in the template image is determined by the homography matrix. The centroid of the target machine is obtained by conversion in the left and right images of the end effector of the mechanical motion.

[0120] The centroid of the target machine in the left and right end-effector images is obtained using a template matching algorithm based on gray-level cross-correlation coefficients.

[0121] Compared with existing technologies, the mechanical motion control method of the present invention constructs a path tree with the starting point and the ending point as root nodes simultaneously through bidirectional sampling, which quickly obtains multiple feasible paths between the starting point and the ending point. The motion control path is finally obtained based on the shortest path in the two path trees, thereby improving the speed of motion path planning and meeting the real-time requirements of the moving machinery.

[0122] Furthermore, this invention also replaces the parent node of the new node and compares the path length from the new node to the root node after the parent node replacement. Based on the comparison result, the parent node of the new node is updated to minimize all feasible paths. Simultaneously, by comparing the path length from the new node to the root node when it acts as the parent node of a reconnected node, the parent node of the reconnected node is updated based on the comparison result, which also minimizes all feasible paths. When all feasible paths are shorter, a more optimized motion control path can ultimately be obtained.

[0123] Furthermore, this invention also selects random sampling points for generating new nodes based on environmental complexity and dynamically adjusts the growth step size for generating new nodes based on obstacle detection results, which can further accelerate path planning speed.

[0124] Based on the same inventive concept, this application also provides an electronic device, which can be a server, desktop computing device, or mobile computing device (e.g., laptop computing device, handheld computing device, tablet computer, netbook, etc.) or other terminal device. The device includes one or more processors and a memory, wherein the processor is used to execute a mechanical motion control method according to an embodiment of the program implementation method; the memory is used to store a computer program executable by the processor.

[0125] Based on the same inventive concept, this application also provides a computer-readable storage medium corresponding to the aforementioned embodiments of the mechanical motion control method. The computer-readable storage medium stores a computer program thereon, which, when executed by a processor, implements the steps of the mechanical motion control method described in any of the above embodiments.

[0126] This application may take the form of a computer program product implemented on one or more storage media (including but not limited to disk storage, CD-ROM, optical storage, etc.) containing program code. Computer storage media include permanent and non-permanent, removable and non-removable media, and information storage can be implemented by any method or technology. Information may 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 technologies, CD-ROM, digital versatile optical disc (DVD) or other optical storage, magnetic tape, magnetic disk storage or other magnetic storage devices, or any other non-transfer medium that can be used to store information accessible by a computing device.

[0127] The embodiments described above are merely examples of several implementations of the present invention, and while the descriptions are relatively specific and detailed, they should not be construed as limiting the scope of the invention. It should be noted that those skilled in the art can make various modifications and improvements without departing from the concept of the present invention, and the present invention also intends to include these modifications and variations.

Claims

1. A mechanical motion control method, characterized in that, Including the following steps: Obtain the starting point and ending point to be planned, construct a first path tree with the starting point as the root node, and construct a second path tree with the ending point as the root node; Generate several random sampling points in the planned space; The random sampling points that are farthest from the starting point are determined as a first sampling set, and the remaining random sampling points are determined as a second sampling set; For each random sampling point in the first sampling set, the node in the first path tree closest to that random sampling point is determined as the growth node; For each random sampling point in the second sampling set, the node in the second path tree closest to that random sampling point is determined as the growth node; For each growth node, a new node is generated at the growth step length in the direction pointing from the growth node to the corresponding random sampling point. For each new node, the growth node corresponding to the new node is determined as its parent node; For each new node, if there is an obstacle within a certain range centered on the straight path between the new node and its corresponding parent node, then the new node is removed. If a node in the first path tree coincides with a node in the second path tree, then the shortest path between the two root nodes is determined as the motion control path based on the first path tree and the second path tree.

2. The method according to claim 1, characterized in that, For each new node, after determining that the growth node corresponding to the new node is its parent node, the steps include: for each new node, calculating the path length from the new node to the corresponding root node when each node in the same path tree is the parent node of the new node, and updating the parent node of the new node to the node corresponding to the minimum path length.

3. The method according to any one of claims 1-2, characterized in that, For each growth node, after generating a new node at the growth step size in the direction pointing to the corresponding random sampling point, the process includes the following steps: For each new node, nodes within a preset range centered on the new node are identified as reconnection nodes; For each reconnected node, if setting the corresponding new node as the parent node of the reconnected node reduces the path length from the reconnected node to the corresponding root node, then the new node is set as the parent node of the reconnected node.

4. The method according to claim 3, characterized in that, For each growth node, after generating a new node at the growth step size in the direction pointing to the corresponding random sampling point, the process includes the following steps: For each new node, if there is an obstacle within a certain range centered on the straight path between the new node and its corresponding parent node, then the growth step size is reduced. For each new node, if there are no obstacles within a certain range centered on the straight path between the new node and its corresponding parent node, then the growth step size is increased.

5. The method according to claim 4, characterized in that, Generate several random sampling points in the planned space, including the following steps: Obtain the environmental complexity, determine the closed radius based on the environmental complexity, and determine the closed radius region centered on each node in the first path tree and the second path tree as the closed region; Several random sampling points are generated in the planned space, and the random sampling points generated in the closed area are removed.

6. The method according to claim 5, characterized in that, For each growth node, a new node is generated at the growth step size in the direction pointing from the growth node to the corresponding random sampling point, including the following steps: For each growth node, if the distance between the growth node and the corresponding random sampling point is greater than the growth step size, then the new node is generated at the growth step size in the direction from the growth node to the corresponding random sampling point. For each growth node, if the distance between the growth node and the corresponding random sampling point is less than the growth step size, then the random sampling point is determined to be the new node.

7. The method according to claim 6, characterized in that, After determining the shortest path between the two root nodes as the motion control path based on the first path tree and the second path tree, the following steps are included: Cubic spline interpolation is performed between adjacent nodes of the motion control path to divide the path between each adjacent node into three variable-speed sub-paths. The path equation expression for the first sub-path is as follows: h1(t j1 )=a 10 +a 11 t j1 +a 12 t j1 2 +a 13 t j1 3 Among them, t j1 Let a be the time point of motion on the first segment of the path. 10 ,a 11 ,a 12 ,a 13 These are the polynomial coefficients; The path equation for the second sub-path is: h2(t j2 )=a 20 +a 21 t j2 +a 22 t j2 2 +a 23 t j2 3 +a 24 t j2 4 +a 25 t j2 5 Among them, t j2 For the time point of movement on the second sub-path, a 20 ,a 21 ,a 22 ,a 23 ,a 24 ,a 25 These are the polynomial coefficients; The path equation for the third sub-path is expressed as follows: h3(t j3 )=a 30 +a 31 t j3 +a 32 t j3 2 +a 33 t j3 3 Among them, t j3 Let a be the time point of movement on the third sub-path. 30 ,a 31 ,a 32 ,a 33 These are the polynomial coefficients; Based on the fact that the acceleration a1(0) at the beginning of the first sub-path and the acceleration a3(f) at the end of the third sub-path are both 0, and the moving speed v1(0) at the beginning of the first sub-path and the moving speed v3(f) at the end of the third sub-path are both 0, determine the values ​​of the coefficients of each polynomial. The movement speed at each time point is determined based on the path equation of the sub-path.

8. The method according to claim 7, characterized in that: After performing cubic spline interpolation between adjacent nodes of the motion control path to divide the path between each adjacent node into three variable-speed sub-paths, the following steps are included: The target machine is controlled to move according to the motion control path and the speed corresponding to each time point. When the target machine stops moving, a movement end signal is generated. The actual position of the target machine is obtained based on the movement end signal. If the distance between the actual position and the endpoint is greater than a preset threshold, the straight path between the actual position and the endpoint is set as the motion control path.

9. A mechanical motion control device, characterized in that, include: The root node determination module is used to obtain the starting point and the ending point to be planned, construct a first path tree with the starting point as the root node, and construct a second path tree with the ending point as the root node. The sampling point generation module is used to generate several random sampling points in the planned space; The sampling set determination module is used to determine a number of random sampling points that are farthest from the starting point as the first sampling set, and the remaining random sampling points as the second sampling set; The first growth node determination module is used to determine, for each random sampling point in the first sampling set, the node in the first path tree closest to the random sampling point as the growth node; The second growth node determination module is used to determine, for each random sampling point in the second sampling set, the node in the second path tree that is closest to the random sampling point as the growth node; The new node generation module is used to generate a new node for each growth node at the growth step length in the direction of the growth node pointing to the corresponding random sampling point. The parent node determination module is used to determine the growth node corresponding to each new node as its parent node. An obstacle detection module is used to remove a new node if an obstacle exists within a certain range centered on the straight path between the new node and its corresponding parent node for each new node. The path determination module is used to determine the shortest path between the two root nodes as the motion control path if a node in the first path tree coincides with a node in the second path tree.

10. The apparatus according to claim 9, characterized in that, The parent node determination module is used to calculate, for each new node, the path length from the new node to the corresponding root node when each node in the same path tree is the parent node of the new node, and update the parent node of the new node to the node with the smallest path length.

Images