A workspace-guided sampling motion planning method

Abstract

The application relates to a kind of sampling motion planning methods based on workspace guidance, which provides a workspace preprocessing technique, and active key areas and workspace reference paths are obtained by octree / quaternary tree decomposition, Watershed algorithm and multi-resolution A* algorithm;Secondly, based on the obtained workspace information, workspace guidance sampling and workspace guidance exploration are performed, so that the path in the configuration space converges quickly;Finally, through the balance of workspace and information space, the path converges to the optimal path.The application realizes a planning method combining workspace information and configuration space information, which can be combined with any sampling algorithm, has outstanding advantages in high-dimensional environment motion planning problems, and can improve the motion efficiency and action quality of robot in complex restricted environment.

Description

TECHNICAL FIELD

[0001] The present application relates to the field of robot motion planning technology, and particularly relates to a sampling motion planning method based on workspace guidance. BACKGROUND

[0002] The rapid development of technology and the improvement of automation and intelligence make the robot technology mature, which has been widely applied in industrial manufacturing, service industry, medical care and many other fields. However, with the increase of application scenarios, the environment faced by the robot is also more and more complex and diversified. In such an environment full of uncertainty, it is crucial to ensure the safe and efficient operation of the robot. Therefore, as the core technology of intelligent and autonomous operation of robots in various complex environments, motion planning has attracted the attention of many researchers. Whether in mobile robots, industrial robot arms, or autonomous driving, motion planning plays a key role. The main task of motion planning is to find a feasible path in the configuration space given the initial state and the target state, and to generate a sequence of robot configurations that can avoid obstacles.

[0003] In practical applications, the commonly used representative path planning methods include sampling-based planning algorithms such as Rapidly-exploring Random Tree (RRT) and Probabilistic Roadmap (PRMs), and various variants thereof. Although sampling-based planning algorithms have excellent performance in efficiency and versatility, in the limited configuration space, the narrow area will bring a computational bottleneck to the sampling and exploration process, and the planning efficiency will be seriously affected. In view of the fact that many robot applications, such as transportation, health care and manufacturing, involve limited configuration space, it is particularly important to reduce the impact of these limited environments on sampling-based planners.

[0004] The main constraint of path planning in a constrained environment usually depends on the distribution of obstacles in the configuration space. To improve the efficiency of sampling-based planning methods in such environments, a common strategy is to adjust the sampling and expansion strategy according to the information of C-space. The RRV method uses principal component analysis to explore the restricted areas in the C-space, and adjusts the exploration strategy according to this information to improve the expansion speed of the search tree in the restricted area. LGM-BRRT* identifies narrow areas by preprocessing the C-space and generates local trees in these areas to quickly explore the restricted areas. In addition, the search strategy based on the informed set is also a way to deal with the problem of constrained environment. This strategy compresses the planning space by constructing an informed set, so that the search process can focus on the key restricted areas, thereby increasing the sampling density and reducing invalid expansion. Informed-RRT* integrates the informed set into RRT* to speed up the search for high-quality solutions in a constrained environment. BIT* uses both informed sets and batch sampling strategies to further improve path planning efficiency while promoting the integration of various sampling methods. Although the above algorithms have made some progress in improving the ability to handle constrained environments, due to insufficient utilization of workspace information, their optimization effect is still restricted by redundant sampling and exploration, and the efficiency of the algorithm is very limited.

[0005] Given the convenience of workspace information acquisition and its contribution to reducing invalid search attempts in high-dimensional configuration space, numerous studies have been devoted to improving sampling-based planning methods to enhance their performance in complex constrained environments. For example, the TS-RRT strategy plans a path in a lower-dimensional workspace for high-dimensional configuration space planning problems. However, this strategy may not be sufficient in practical applications because it fails to adequately consider the complex topological relationship between the workspace and the configuration space. Therefore, in some cases, TS-RRT may need to perform additional movement operations in the configuration space to achieve the same trajectory in the workspace, or even miss a feasible solution. To address this limitation, Mesesan et al. designed a hierarchical planning framework that decomposes the workspace and constructs a connection graph to guide a more accurate planning process. On the other hand, the EET strategy uses a spherical tree structure constructed in the workspace to capture local information and integrates this workspace preprocessing technique into the planning process. In this way, EET can adapt to narrow environments and quickly guide exploration behavior. However, EET relies heavily on a greedy search mechanism, and the guidance provided by the spherical tree may be biased when facing complex scenarios. Especially when the connectivity of the spherical tree does not match the real connectivity of the configuration space, EET may not effectively find a solution, resulting in a lack of probabilistic completeness. Overall, workspace-based sampling planning algorithms take full advantage of the information advantage of low-dimensional workspaces, to some extent, alleviating the planning efficiency problem in constrained areas. However, it is worth noting that current workspace-based sampling planning algorithms rarely have research specifically optimized for the characteristics of constrained areas. In addition, such algorithms that rely on workspace planning generally require the use of inverse kinematics solutions, which can cause significant computational resource pressure for robot systems with complex inverse kinematics characteristics. Most importantly, since the planning focus is placed in the workspace, the overall consideration of the configuration space is relatively scarce, and how to ensure the asymptotic optimality of such algorithms remains a major challenge to be addressed. SUMMARY

[0006] The present application aims to provide a workspace-guided sampling motion planning method that can integrate the complementary advantages of configuration space planning and workspace planning to quickly obtain optimal solutions in constrained environments.

[0007] To achieve the above-mentioned purpose, the technical scheme adopted by the present application is as follows: a workspace-guided sampling motion planning method, comprising the following steps:

[0008] Step S1, workspace decomposition: using quadtree / octree decomposition to divide the workspace into a set of workspace cells Free areas are decomposed layer by layer until a predetermined resolution is reached, obtaining a set of free workspace cells S;

[0009] Step S2, critical region search: search critical regions in narrow passages from the set of free workspace cells S using Watershed algorithm

[0010] Step S3, reference solution search: build a multi-resolution node-connected graph based on the set of workspace cells S For a reference path from the start point p start to the end point p goal is obtained using multi-resolution A* algorithm

[0011] Step S4, active critical region screening: select critical regions passed by the reference path and mark them as active critical regions

[0012] Step S5, initialize planner: initialize the set of sampling points V, the set of edges E, and the search tree of the planner ;

[0013] Step S6, workspace-guided sampling: first sample in the whole configuration space, if the end-effector position corresponding to the sampling point is located in an active critical region , then perform Gaussian sampling around the sampling point

[0014] Step S7, workspace-guided exploration: construct a workspace heuristic function based on the reference path, active critical regions, and iteration number, and expand the search tree based on the workspace heuristic function

[0015] Step S8, judge whether the workspace corresponding to the latest explored region is located in the coverage range of the reference path , if yes, execute Step S10, otherwise execute Step S9

[0016] Step S9, judge whether the current expansion number n exp is greater than the maximum expansion number N exp , if yes, switch the reference path and reselect active critical regions , then execute Step S10, otherwise directly execute Step S10

[0017] Step S10, update the expansion marker I con , reset the expansion number n exp to 0, and judge whether the planning end condition is met, if yes, end the planning, extract the motion trajectory from the search tree, otherwise return to Step S6 for continuous execution

[0018] ​Step S11, the robot receives the algorithm-planned motion trajectory, and controls the robot to move to the target position.

[0019] Further, the step S6 is implemented in the following way: in the state space informed set C inf Uniform sampling is performed, and if the uniform sampling point q uni corresponds to the end effector position f locate (q uni ) is located in the active key region , then the Gaussian sampling variance δ is set according to the f locate (q uni ) corresponding region grid size, taking q uni as the center, and n gau times of Gaussian sampling are performed with δ as the variance.

[0020] Further, the step S7 is implemented in the following way: on the basis of the standard heuristic function f(q)=g(q)+h(q), an attenuation factor e -αi and a workspace guiding term are added to form a workspace guiding heuristic function:

[0021] f w (q)=g(q)+h(q)+e -αi h w (q) (1)

[0022] In equation (1), α is a constant greater than 0, i is the iteration number of the algorithm, and is specifically expressed as:

[0023] h w (q)=F d w d (q)+F s w s (q)+F n w n (q) (2)

[0024] In equation (2), F d , F s , and F n are constant factors, ω d , ω s , and ω n are cost factors, and are specifically defined as follows:

[0025]

[0026] In equation (3), d is the distance between the start and end points of the workspace, and d is the distance between the start and end points in the configuration space. f is the maximum size of the workspace. FK (·) represents the positive kinematic function. and These represent the size and center position of the spatial unit, respectively; the symbol [·] represents Iverson brackets.

[0027] Furthermore, the specific implementation method of step S8 is as follows: when the search tree Workspace unit corresponding to the new expansion point With the nearest reference path unit The distance between them is less than the set coverage radius. When, the extended marker I will be... con To reference path unit In the reference path The sequence number in the sequence will expand the number of times n. exp Reset to 0.

[0028] Furthermore, the specific implementation method of step S9 is as follows: when the current expansion count n exp Greater than the maximum number of expansions N exp When, first set the current reference path Each element in the process is expanded until each edge (or face in the case of a 3D spatial element) contacts the workspace obstacle. The expanded region is then marked as the confined region S. block When performing a reference path search, the restricted region S is used. block This will increase the additional path cost; then, the multi-resolution A* planning reference path is reused to obtain a new reference path that is different from the original reference path, thus realizing the reference path update.

[0029] Compared with the prior art, the present invention has the following beneficial effects:

[0030] (1) This invention implements a sampling motion planning algorithm that processes workspace information and guides configuration space planning. As a meta-algorithm, it can be used as a sampling and extension method to combine with any sampling-based planning algorithm to improve its convergence performance. It has strong applicability and ensures the completeness and asymptotic optimality of the algorithm.

[0031] (2) For most configuration space sampling motion planning algorithms, obtaining sampling points in the constrained region of configuration space and realizing complete exploration requires multiple iterations, resulting in high computational overhead and low computational efficiency. This invention guides configuration space sampling and exploration by preprocessing the workspace, concentrating computational power on key regions, and can obtain high-quality planning results with a small number of iterations.

[0032] (3) For most workspace sampling motion planning algorithms, it is difficult to obtain planning results quickly while ensuring the optimality of the algorithm due to the high-dimensional configuration space and low-dimensional workspace complex topological relationship, and heavy inverse kinematics calculation. The present application can ensure the algorithm to converge to the optimal result while achieving efficient calculation by obtaining workspace information and converting it into configuration space planning information. BRIEF DESCRIPTION OF DRAWINGS

[0033] Figure 1 is a method implementation flowchart of an embodiment of the present application;

[0034] Figure 2 is a workspace decomposition principle diagram in an embodiment of the present application;

[0035] Figure 3 is a joint region screening principle diagram in an embodiment of the present application;

[0036] Figure 4 is a reference path planning principle diagram in an embodiment of the present application;

[0037] Figure 5 is a reference path switching principle diagram in an embodiment of the present application;

[0038] Figure 6 is a simulation result diagram of simulation scenario one in an embodiment of the present application;

[0039] Figure 7 is a simulation result diagram of simulation scenario two in an embodiment of the present application;

[0040] Figure 8 is a simulation result diagram of simulation scenario three in an embodiment of the present application;

[0041] Figure 9 is a static real experiment result diagram in an embodiment of the present application. DETAILED DESCRIPTION

[0042] The present application will be further described below in conjunction with the drawings and embodiments.

[0043] It should be noted that the following detailed description is exemplary and is intended to provide further explanation of the present application. Unless otherwise specified, all technical and scientific terms used herein have the same meaning as generally understood by those skilled in the art to which the present application belongs.

[0044] It is to be understood that the terminology used herein is for the purpose of describing particular embodiments only and is not intended to be limiting of example embodiments in accordance with the present application. As used herein, the singular forms "a", "an" and "the" are intended to include the plural forms as well, unless the context clearly indicates otherwise. It will be further understood that the terms "comprises" and / or "comprising," when used in this specification, specify the presence of stated features, steps, operations, elements, components, and / or groups thereof, but do not preclude the presence or addition of one or more other features, steps, operations, elements, components, and / or groups thereof.

[0045] As shown in Figure 1 , the embodiment provides a workspace-guided sampling motion planning method, comprising the following steps:

[0046] Step S1, workspace decomposition: using quadtree / octree decomposition to decompose the workspace into free regions layer by layer until a predetermined resolution is reached, to obtain a set of free workspace units S, as shown in Figure 2 .

[0047] Step S2, key region search: using Watershed algorithm to search for key regions in narrow channels from the set of free workspace units S , as shown in Figure 3 .

[0048] Step S3, reference solution search: based on the set of workspace units S, a multi-resolution node connectivity graph G is established . A multi-resolution A* algorithm is used to obtain a reference path from the start point p start to the end point p goal . , as shown in Figure 4 .

[0049] Step S4, active key region screening: selecting the key regions passed by the reference path and marking them as active key regions .

[0050] Step S5, initialization of the planner: initializing the set of sampling points V, the set of edges E, and the search tree of the planner. .

[0051] Step S6, workspace-guided sampling: first sampling in the overall configuration space, if the end effector position corresponding to the sampling point is located in the active key region , then performing Gaussian sampling near the sampling point.

[0052] Step S7, workspace-guided exploration: constructing a workspace heuristic function according to the reference path, the active key region, and the number of iterations, and expanding the search tree based on the workspace heuristic function.

[0053] Step S8, judge whether the latest exploration area corresponding workspace is located in the reference path If yes, execute step S10, otherwise execute step S9.

[0054] Step S9, judge whether the current expansion times n exp is greater than the maximum expansion times N exp If yes, switch the reference path And reselect the active key area Then execute step S10, otherwise directly execute step S10.

[0055] Step S10, update the expansion mark I con , reset the expansion times n exp to 0, and judge whether the planning end condition is met. If yes, end the planning, extract the motion trajectory from the search tree, otherwise return to step S6 for continuous execution.

[0056] Step S11, the robot receives the motion trajectory planned by the algorithm, and controls the robot to move to the target position.

[0057] The specific implementation method of the step S6 is: uniformly sampling in the state space informed set C inf , if the uniformly sampled point q uni corresponds to the end effector position f locate (q uni ) is located in the active key area , then set the Gaussian sampling variance δ according to the area grid size of f locate (q uni ), take q uni as the center, and perform n gau times Gaussian sampling with δ as the variance.

[0058] The specific implementation method of the step S7 is: on the basis of the standard heuristic function f(q)=g(q)+h(q), add the decay factor e -αi and the workspace guide term to form the workspace guide heuristic function:

[0059] f w (q)=g(q)+h(q)+e -αi h w (q) (1)

[0060] In formula (1), α is a constant greater than 0, i is the algorithm iteration times, Specifically represented as:

[0061] h w (q)=F d wd (q)+F s w s (q)+F n w n (q) (2)

[0062] In equation (2), F d F s F n ω is a constant factor. d ω s ω n The cost factor is defined as follows:

[0063]

[0064] In equation (3), The distance between the start and end points of the workspace. This is the distance between the start and end points in the configuration space; f is the maximum size of the workspace. FK (·) represents the positive kinematic function. and These represent the size and center position of the spatial unit, respectively; the symbol [·] represents Iverson brackets.

[0065] The specific implementation method of step S8 is as follows: when the search tree Workspace unit corresponding to the new expansion point With the nearest reference path unit The distance between them is less than the set coverage radius. When, the extended marker I will be... con To reference path unit In the reference path The sequence number in the sequence will expand the number of times n. exp Reset to 0.

[0066] The specific implementation method of step S9 is as follows: when the current expansion count n exp Greater than the maximum number of expansions N exp When, first set the current reference path Each element in the process is expanded until each edge (or face in the case of a 3D spatial element) contacts the workspace obstacle. The expanded region is then marked as the confined region S. block When performing a reference path search, the restricted region S is used. block This will increase additional path costs; then, multi-resolution A* planning is used again to obtain a new reference path different from the original reference path, thus updating the reference path, such as... Figure 5 As shown.

[0067] The pseudo code of the method is as follows:

[0068]

[0069] The method is further described below through specific simulation experiments. The embodiment mainly verifies the effectiveness through simulation experiments and real experiments. The specific experimental settings are as follows:

[0070] Simulation experiment:

[0071] The simulation experiment is carried out in MATLAB R2021b software.

[0072] (1) Simulation scenario one

[0073] The simulation static scene map size is X=Y=300 cm, all obstacles are static obstacles, and simulation tests are carried out on the planar three-joint and five-joint robot arms respectively, as shown in Figure 6 (a) and Figure 6 (b). The lengths of the robot arms are (50, 50, 50) and (25, 25, 50, 25, 25) respectively, in units of cm. The starting points are (180, 0, 0) and (180, 0, 0, 0, 0) respectively, and the target points are (180, 0, 0) and (180, 0, 90, 90, 0) respectively, in units of degrees (°). The simulation results are shown in Figure 6 (c) and Figure 6 (d), where S represents the starting point, G represents the target point, the light-colored line represents the explored trajectory, and the dark-colored line represents the trajectory planned by the algorithm proposed in the application when the planning time is 1.01 seconds and 1.60 seconds respectively.

[0074] (2) Simulation scenario two

[0075] The simulation static scene map size is X=Y=300 cm, all obstacles are static obstacles, and simulation tests are carried out on the planar three-joint and five-joint robot arms respectively, as shown in Figure 7 (a) and Figure 7 (b). The lengths of the robot arms are (55, 55, 30) and (30, 30, 30, 30, 30) respectively, in units of cm. The starting points are (45, 0, 45) and (45, 0, 0, 0, 45) respectively, and the target points are (-35, -70, 75) and (-30, -15, -70, 15, 75) respectively, in units of degrees (°). The simulation results are shown in Figure 7 (c) and Figure 7 (d), where S represents the starting point, G represents the target point, the light-colored line represents the explored trajectory, and the dark-colored line represents the trajectory planned by the algorithm proposed in the application when the planning time is 1.20 seconds and 3.96 seconds respectively.

[0076] (3) Simulation scenario three

[0077] The simulation static scene map size is X=Y=Z=300cm, all obstacles are static obstacles, and the simulation test is based on a UR5 robot, as shown in Figure 8 (a). The starting point is (-90, 0, 90, 0, 90) and the target point is (0, 0, 90, 0, 90), and the unit is degree (°). The simulation result is as shown in Figure 8 (b), which is the simulation map of static scene three, S point represents the starting point, G represents the target point, light color represents the explored trajectory, and dark color line represents the trajectory planned by the algorithm proposed in the application when the planning time is 1.29 seconds.

[0078] Real experiment:

[0079] The application can be directly used in path planning of mobile robots, motion planning of mechanical arms and trajectory planning of unmanned aerial vehicles. When executing robot motion planning, the algorithm can be called for planning.

[0080] The static real experiment environment is a Lenovo notebook computer running Microsoft Windows 10 and MATLAB 2021b, with an Intel Core i5-7300HQ processor and 16GB of memory, and a Fetch robot running Ubuntu 14.04 system with a 7-degree-of-freedom mechanical arm. The experimental task is target grasping and placing in a complex restricted environment.

[0081] (1) Experimental scene one

[0082] The scene map size is X=Y=Z=300cm, and all obstacles are known static obstacles, as shown in Figure 9 (a). The robot starting configuration is (-50, 15, 90, 40, 0, 40, 75) and the target configuration is (50, 15, 90, -40, 0, -40, 105), and the unit is degree (°). As shown in Figure 9 (c), which is the static real experiment process, and the dark color line represents the optimal path planned.

[0083] (2) Experimental scene two

[0084] The scene map size is X=Y=Z=300cm, and all obstacles are known static obstacles, as shown in Figure 9 (b). The robot starting configuration is (-85, 0, 90, 60, 0, 20, 90) and the target configuration is (15, 5, 90, 15, 0, 65, 85), and the unit is degree (°). As shown in Figure 9 (d), which is the static real experiment process, the left side of the figure is a real shot of the experiment process, and the dark color line represents the optimal path planned.

[0085] Those skilled in the art will appreciate that embodiments of the application can be readily used as software, hardware, or a combination of software and hardware. In one

[0086] The processes and logic flows described in this specification can be performed by one or more programmable computers executing one or more computer programs to Figure 1 The processes and logic flows can also be performed by, and apparatus can also be implemented as, special purpose logic Figure 1 The processes and logic flows can also be performed by, and apparatus can also be implemented as, special purpose logic

[0087] These computer programs (also known as programs, software, software applications programs, applications, components, program instructions, routines, subroutines, programs, functions, modules, code segments, etc.) include any sequence or Figure 1 The processes and logic flows can also be performed by, and apparatus can also be implemented as, special purpose logic Figure 1 The processes and logic flows can also be performed by, and apparatus can also be implemented as, special purpose logic

[0088] These computer programs (also known as programs, software, software applications programs, applications, components, program instructions, routines, subroutines, programs, functions, modules, code segments, etc.) include any sequence or Figure 1 The processes and logic flows can also be performed by, and apparatus can also be implemented as, special purpose logic Figure 1 The processes and logic flows can also be performed by, and apparatus can also be implemented as, special purpose logic

[0089] The above description is the preferred embodiment of the application. Various modifications and changes can be made without departing from the scope of the application as set forth in the claims.

Claims

1. A workspace-based guidance-based sampling motion planning method, characterized in that, Comprising the following steps: Step S1, workspace decomposition: using quadtree or octree decomposition to divide the workspace into a plurality of workspace units The free area is decomposed layer by layer until a predetermined resolution is reached, obtaining a set of free workspace units ; Step S2, critical region search: search for critical regions in narrow passages from the free working space cell set using the Watershed algorithm ;​ Step S3, reference solution search: based on the workspace cell set Establishing a multi-resolution node connectivity graph , the reference path from the start point to the end point is obtained by using a multi-resolution A* algorithm ; Step S4, Selecting active key area: Selecting reference path Key area passed , Marked as active key area ; Step S5, initialize planner: initialize the set of sample points for the planner , the set of edges , the search tree ; Step S6, workspace-guided sampling: first sample in the whole configuration space, if the end-effector position corresponding to the sampling point is in the active critical region , then perform Gaussian sampling near the sampling point; Step S7, workspace guidance exploration: constructing a workspace heuristic function according to the reference path, the active key region and the iteration number, and expanding the search tree based on the workspace heuristic function; Step S8, judging whether the latest exploration area corresponding workspace is located on the reference path If yes, step S10 is executed, otherwise, step S9 is executed. Step S9, judging the current expansion times whether greater than the maximum expansion times Yes, switch the reference path And reselect the active key area Then execute step S10, otherwise directly execute step S10; Step S10, updating the expansion mark , the expansion times are reset to 0, and it is determined whether the planning end condition is met. If yes, the planning is ended, the motion trajectory is extracted from the search tree, otherwise, the step S6 is returned to continue the execution. Step S11, the robot receives the motion trajectory planned by the algorithm and controls the robot to move to the target position; The specific implementation method of the step S7 is: on the basis of a standard heuristic function , adding an attenuation factor and a workspace guiding item to form a workspace guiding heuristic function: In the formula (1), is a constant greater than 0, is the number of iterations of the algorithm, is specifically expressed as: In the formula (2), , , is a constant factor, , , is a cost factor, defined as follows: In (3) formula, is the distance between the start and end points of the workspace, is the distance between the start and end points in the configuration space; is the maximum dimension of the workspace; is the forward kinematics function, ) and ) represent the size and center position of the spatial unit respectively; the symbol is the Iverson bracket.

2. The workspace-guided sampling motion planning method of claim 1, wherein, The specific implementation method of the step S6 is: in the state space informed set Uniform sampling is performed, and if the uniform sampling point The corresponding end effector position Is located in the active key area , then according to The corresponding area grid size sets the Gaussian sampling variance , with As the center, The variance is Sub-gaussian sampling is performed.

3. The workspace-guided sampling motion planning method of claim 1, wherein, The specific implementation method of the step S8 is: when the distance between the search tree The newly added extension point corresponds to a workspace unit And the nearest reference path unit The distance between them is less than the set coverage radius Then mark the extension Update to the reference path unit In the reference path The sequence number, the number of extensions Reset to 0.

4. The workspace-guided sampling motion planning method of claim 1, wherein, The specific implementation method of the step S9 is: when the current expansion times is greater than the maximum expansion times , first, each unit in the current reference path is inflated until each edge or each face contacts the working space obstacle, and the inflated area is marked as a limited area . When performing the reference path search, the restricted area An additional path cost is added when the restricted area is encountered; then the multi-resolution A* planning reference path is reused to obtain a new reference path different from the original reference path, thereby achieving reference path updating.

Images