Mechanical arm trajectory planning method and device, electronic equipment and storage medium

Abstract

The application relates to the technical field of robot motion planning, and provides a mechanical arm trajectory planning method and device, electronic equipment and a storage medium, which comprise the following steps: acquiring a starting joint configuration of a mechanical arm and a target pose of an end effector, and generating a plurality of candidate target joint configurations based on the target pose; taking each candidate target joint configuration as a root node and growing a plurality of target trees in parallel; growing a starting tree with the starting joint configuration as a root node, and controlling the starting tree to expand to the plurality of target trees; in the case that a node of the starting tree and a node of any target tree satisfy a connectivity condition, determining a target trajectory of the mechanical arm based on a path formed by the starting tree and the target tree satisfying the connectivity condition. According to the application, a plurality of candidate target configurations are generated based on a target end pose, and a plurality of target trees are grown in parallel, while the starting tree is controlled to expand to each target tree in a directional manner to be connected, so that the path planning success rate and path quality of the mechanical arm in a complex obstacle environment are improved.

Description

Technical Field

[0001] This invention relates to the field of robot motion planning technology, and in particular to a method, apparatus, electronic device, and storage medium for planning the trajectory of a robotic arm. Background Technology

[0002] High-degree-of-freedom serial robotic arms are widely used in various complex scenarios such as industrial manufacturing, medical surgery, and special operations. Currently, motion planning for robotic arms mainly employs Rapidly-exploring RandomTree (RRT) and its derivative algorithms (such as RRT). Algorithms such as RRT-Connect possess probabilistic completeness and asymptotic optimality, and can perform planning tasks well in normal environments. However, due to the irreversibility of the forward kinematics of a serial manipulator, a single end effector target pose often corresponds to multiple different joint space configurations. Existing trajectory planning methods typically select only a single inverse kinematic solution as the target configuration for path searching. This strategy has certain limitations in complex obstacle environments. If the selected single target configuration is located in an inaccessible area or its path is blocked by obstacles, even if other unselected configurations have feasible collision-free paths, it will directly lead to planning failure. Summary of the Invention

[0003] This invention provides a robotic arm trajectory planning method, apparatus, electronic device, and storage medium to address the shortcomings of existing trajectory planning methods in complex obstacle environments, which rely solely on a single target configuration for path search, leading to a tendency to get stuck in inaccessible areas and a low planning success rate.

[0004] This invention provides a robotic arm trajectory planning method, comprising: The starting joint configuration of the robotic arm and the target pose of the end effector are obtained, and multiple candidate target joint configurations are generated based on the target pose. Using each candidate target joint configuration as the root node, multiple corresponding target trees are grown in parallel in the free configuration space; The starting joint configuration is used as the root node to grow a starting tree, and the starting tree is controlled to expand to the multiple target trees; If the nodes of the starting tree and any node of any of the multiple target trees satisfy the connectivity condition, the target trajectory of the robotic arm is determined based on the path formed by the starting tree and the target trees that satisfy the connectivity condition.

[0005] According to a robotic arm trajectory planning method provided by the present invention, the step of generating multiple candidate target joint configurations based on the target pose includes: Based on the target pose, multiple seed joint configurations are obtained from a pre-built configuration sampling library; Each seed joint configuration is used as an initial value to solve a preset constraint optimization problem in parallel, resulting in multiple initial inverse kinematic solutions. The objective function of the constraint optimization problem is constructed based on the pose error between the forward kinematic pose and the target pose, as well as the deviation between the joint configuration to be solved and the corresponding seed joint configuration. The multiple initial inverse kinematics solutions are deduplicated, and the deduplicated initial inverse kinematics solutions are used as the multiple candidate target joint configurations.

[0006] According to the robotic arm trajectory planning method provided by the present invention, the construction steps of the configuration sampling library include: Sampling is performed within the joint space of the robotic arm according to a preset distribution to obtain multiple free joint configuration samples that meet the collision-free condition; A K-dimensional tree data structure is constructed using the pose coordinates corresponding to each of the free joint configuration samples as the index key, and the K-dimensional tree data structure is used as the configuration sampling library.

[0007] According to a robotic arm trajectory planning method provided by the present invention, the deduplication process for the plurality of initial inverse kinematic solutions includes: For any two initial inverse kinematics solutions among the plurality of initial inverse kinematics solutions, calculate the configuration difference distance between the two initial inverse kinematics solutions; If the configuration difference distance is less than or equal to a preset distance threshold, any one of the two calculated initial inverse kinematics solutions is removed.

[0008] According to a robotic arm trajectory planning method provided by the present invention, the step of controlling the expansion of the starting tree to the plurality of target trees includes: During each iteration of the growth of the initial tree, a random probability value is generated; If the random probability value is greater than the preset probability threshold, then uniform sampling is performed in the free configuration space to obtain the first extended node, and the starting tree is expanded and grown based on the first extended node; If the random probability value is not greater than the preset probability threshold, then obtain the set of new nodes of each of the multiple target trees compared to the previous iteration, merge the multiple candidate target joint configurations with the set of new nodes corresponding to all target trees to obtain a target guidance set, sample in the target guidance set to obtain a second extension node, and perform directional expansion growth of the starting tree based on the second extension node.

[0009] According to the present invention, a robotic arm trajectory planning method is provided, wherein the step of growing multiple corresponding target trees in parallel in a free configuration space, using each candidate target joint configuration as the root node, includes: Assign computation threads to each of the candidate target joint configurations; On each of the computing threads, the corresponding candidate target joint configuration is used as the root node of the target tree. An asynchronous expansion operation is performed on each of the target trees in the free configuration space based on a preset sampling bias rate, wherein the preset sampling bias rate is less than the sampling bias rate for performing the expansion operation on the starting tree.

[0010] According to a robotic arm trajectory planning method provided by the present invention, the step of determining the target trajectory of the robotic arm based on the path formed by the starting tree and the target trees satisfying the connectivity condition, when the nodes of the starting tree and any node of the plurality of target trees satisfy the connectivity condition, includes: If the nodes of the starting tree and any target tree satisfy the connectivity condition, the starting tree and any target tree are concatenated to form a connected path. Before reaching the preset stopping condition, a reconnection operation is performed on the starting tree after each tree iteration operation to update the path cost of the existing connected paths. In the case of multiple connected paths, the optimal connected path with the lowest cost is tracked in real time. When the preset stopping condition is met, the optimal connected path is output as the target trajectory.

[0011] The present invention also provides a robotic arm trajectory planning device, comprising: The configuration generation unit is used to obtain the initial joint configuration of the robotic arm and the target pose of the end effector, and generate multiple candidate target joint configurations based on the target pose. The target tree growth unit is used to grow multiple corresponding target trees in parallel in the free configuration space, with each candidate target joint configuration as the root node; A starting tree extension unit is used to grow a starting tree with the starting joint configuration as the root node, and to control the starting tree to extend to the multiple target trees; The trajectory output unit is used to determine the target trajectory of the robotic arm based on the path formed by the starting tree and the target tree that satisfies the connectivity condition, provided that the nodes of the starting tree and any node of the plurality of target trees satisfy the connectivity condition.

[0012] The present invention also provides an electronic device, including a memory, a processor, and a computer program stored in the memory and running on the processor, wherein the processor executes the computer program to implement the robotic arm trajectory planning method as described above.

[0013] The present invention also provides a non-transitory computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the robotic arm trajectory planning method as described above.

[0014] The present invention also provides a computer program product, including a computer program that, when executed by a processor, implements the robotic arm trajectory planning method as described above.

[0015] The robotic arm trajectory planning method, apparatus, electronic device, and storage medium provided by this invention first generate multiple candidate target joint configurations based on the target pose of the end effector, effectively expanding the search and coverage range of feasible solutions and avoiding the risk of planning failure due to a single target configuration being located in an inaccessible region due to obstacle interference. Second, multiple target trees are grown in parallel in the free configuration space with each candidate target joint configuration as the root node, and the starting tree is actively expanded to these multiple target trees. This design of multi-target parallel exploration and multi-tree directional intersection enables the algorithm to simultaneously search for paths in multiple potential configuration space regions, thereby increasing the probability of bypassing complex obstacles. Finally, the target trajectory can be successfully determined when the starting tree and any target tree satisfy the connectivity condition, thus improving the planning success rate and robustness of the robotic arm in complex working environments while ensuring planning efficiency. Attached Figure Description

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

[0017] Figure 1 This is a flowchart illustrating the robotic arm trajectory planning method provided by the present invention; Figure 2 This is a flowchart illustrating the multi-objective parallel joint space trajectory planning method for serial robotic arms provided by the present invention. Figure 3 These are schematic diagrams of the four testing environments provided by this invention; Figure 4 This invention provides a comparison curve of the median path cost of each algorithm in each iteration on a 6-DOF and 7-DOF robotic arm under a random complex obstacle environment. Figure 5 This is a schematic diagram of the robotic arm trajectory planning device provided by the present invention; Figure 6 This is a schematic diagram of the structure of the electronic device provided by the present invention. Detailed Implementation

[0018] To make the objectives, technical solutions, and advantages of this invention clearer, the technical solutions of this invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some, not all, of the embodiments of this invention. All other embodiments obtained by those skilled in the art based on the embodiments of this invention without creative effort are within the scope of protection of this invention.

[0019] With the continuous development of robotics technology, high-degree-of-freedom serial robotic arms, thanks to their flexible movement capabilities, have been widely used in complex scenarios such as industrial manufacturing, medical surgery, and special operations. In these applications, motion planning (i.e., joint space trajectory planning) of the robotic arm is a key step in achieving automated and intelligent operations.

[0020] Currently, for motion planning of robotic arms in free-form configuration spaces, the industry mainly adopts sampling-based stochastic planning algorithms. Examples include the classic Rapid Randomized Tree Extension (RRT) algorithm, and RRT algorithms that incorporate reconnection mechanisms to continuously optimize the path. Algorithms, or the RRT-Connect algorithm which combines bidirectional tree growth strategies to improve search efficiency, are used. These algorithms, due to their probabilistic completeness and asymptotic optimality, can perform motion planning tasks well in typical working environments.

[0021] However, the aforementioned technologies still have certain shortcomings when facing practical planning tasks for high-degree-of-freedom serial manipulators. Specifically, due to the irreversibility of the forward kinematics of serial manipulators (i.e., the multivalued nature of inverse kinematics), a single end effector target pose often corresponds to an infinite number or many completely different joint space configurations. Existing trajectory planning methods typically select only a single inverse kinematic solution as the sole target joint configuration for path searching when setting the planning endpoint. This single-objective planning strategy faces significant limitations in complex obstacle environments. When the environment in which the manipulator is located is complex, if the algorithm randomly selects or calculates a single target joint configuration that happens to be located in an inaccessible area due to obstacle interference, or if the feasible path to that single configuration is completely blocked by obstacles, the planning will ultimately fail. In reality, for a given end effector target pose, other unselected inverse kinematic solutions may have completely feasible collision-free paths.

[0022] Therefore, existing robotic arm trajectory planning methods that rely on a single target configuration suffer from poor robustness and low planning success rate in complex obstacle environments. They also tend to miss better path selections and fail to fully utilize the obstacle avoidance and flexible movement potential of high-degree-of-freedom serial robotic arms in complex spaces. Overcoming the limitations caused by the multivalued nature of inverse kinematics and providing a robotic arm trajectory planning method that can effectively improve planning success rate and ensure path quality is a technical problem that urgently needs to be solved by those skilled in the art.

[0023] To address this, the present invention provides a robotic arm trajectory planning method. Based on the given target pose of the end effector, multiple candidate target configurations are generated, and multiple target trees are grown in parallel with each candidate target configuration as the root node. At the same time, a starting tree is grown with the starting configuration as the root node and adaptively extended to each target tree for connectivity. This achieves efficient and flexible search for globally feasible paths of the robotic arm in complex obstacle environments, improves the success rate and quality of path planning of the robotic arm, and thus overcomes the above-mentioned defects.

[0024] It should be noted that the method provided by this invention can run on electronic devices with trajectory planning and calculation capabilities. These electronic devices may include, but are not limited to, robotic arm controllers, personal computers, edge computing servers, or cloud servers. The electronic device can directly communicate with the robotic arm and issue planned trajectory control commands in real time, or it can simply act as a host computer to perform offline calculations and import the trajectory data into the actual robotic arm control system. For serial robotic arms, especially high-degree-of-freedom or redundant serial robotic arms with at least 6 degrees of freedom, which are widely used in industrial manufacturing, medical surgery, and special operations, this method is particularly suitable for these application scenarios with complex obstacle environments and high requirements for planning success rate and trajectory path quality.

[0025] Figure 1 This is a flowchart illustrating the robotic arm trajectory planning method provided by the present invention, as shown below. Figure 1 As shown, the method includes: Step S10: Obtain the initial joint configuration of the robotic arm and the target pose of the end effector, and generate multiple candidate target joint configurations based on the target pose.

[0026] Specifically, the initial joint configuration typically refers to the specific numerical combination of angles or displacement parameters of each joint of the robotic arm in its current initial state, representing the physical starting point of the robotic arm's planning. The target pose of the end effector, on the other hand, refers to the specific position coordinates and orientation that the center point of the robotic arm's end effector needs to ultimately reach in the three-dimensional task space. Since the forward kinematics of a serial robotic arm is usually irreversible, a given task space end effector pose may correspond to an infinite number of different joint configurations. Especially for redundant robotic arms with degrees of freedom greater than the task space dimension, their forward kinematic images often form a continuous manifold in joint space.

[0027] Traditional path planning methods often rely on a single inverse kinematics solution. If this unique solution happens to be located in an inaccessible region of an obstacle, the planning task will fail even if other feasible configuration paths actually exist in space. Therefore, this embodiment of the invention generates multiple candidate target joint configurations based on the acquired target pose. Here, the number N of candidate target joint configurations can be a flexible configurable parameter. When N=1, the method provided by this embodiment of the invention is a single-objective planning method, growing only one target tree and one starting tree; as N increases, the number of generated target trees increases accordingly, and the coverage of the multiple candidate target joint configurations on the forward kinematic image expands accordingly.

[0028] There are various ways to generate these multiple candidate target joint configurations. For example, they can be obtained by multi-point sampling on the continuous manifold, or by other numerical optimization, analytical methods, or heuristic sampling based on big data to obtain a combination of multiple inverse kinematic solutions that meet the requirements.

[0029] Step S20: Using each candidate target joint configuration as the root node, grow multiple corresponding target trees in parallel in the free configuration space.

[0030] Specifically, after obtaining multiple candidate target joint configurations, each is used as a growth source (i.e., root node) for spatial exploration. Here, the free configuration space refers to the set of safe configuration subsets in the entire joint configuration space that neither collide with environmental obstacles nor compromise the robot's own physical limitations (such as joint angle limitations). The target tree refers to the tree obtained from randomized path planning (e.g., RRT fast random expansion tree or its variant RRT). In algorithms, a tree-like data structure is formed by taking the target state point as the starting point and expanding outwards by randomly sampling and connecting adjacent nodes.

[0031] In this step, each candidate target joint configuration is treated as the root node of a target tree, with corresponding computational resources allocated to each candidate target joint configuration. Since these target trees each explore the configuration space surrounding their root node, they can grow in parallel within the free configuration space. Here, parallel growth means that the node sampling, collision detection, and edge connection processes of each target tree are completely independent and do not interfere with each other. This design supports parallel acceleration using the hardware characteristics of modern computing architectures such as multi-core CPUs (Central Processing Units) or GPUs (Graphics Processing Units), thus ensuring that the algorithm maintains extremely high operating efficiency even after introducing multiple targets.

[0032] Step S30: Use the initial joint configuration as the root node to grow the starting tree, and control the starting tree to expand to the multiple target trees.

[0033] Specifically, while exploring the target side in parallel, this step establishes a starting tree on the starting side. The starting tree is a random tree that grows and expands from the starting joint configuration, the actual physical starting point of the robotic arm's motion, as the root node. To encourage the starting tree to find a feasible path to the target as quickly as possible, this embodiment of the invention uses a specific guidance mechanism to control the expansion of the starting tree into multiple target trees.

[0034] Here, controlled expansion is not a blind extension in a single direction, but an adaptive strategy that achieves a dynamic balance between global exploration and directional convergence. Specifically, the growth of the starting tree involves both uniform random exploration throughout the entire free configuration space to help it overcome or bypass complex local obstacles, and also grants it a certain probability to actively extend and sample in the direction of the multiple parallel-growing target trees (e.g., the root node of the target tree, or the edge of a newly extended node during the parallel growth of the target tree). This adaptive mechanism of extending towards multiple target trees accelerates the spatial convergence speed of the feasible configuration regions where the starting side and the target side are located.

[0035] Step S40: If the nodes of the starting tree and any node of the plurality of target trees satisfy the connectivity condition, the target trajectory of the robotic arm is determined based on the path formed by the starting tree and the target tree that satisfies the connectivity condition.

[0036] Specifically, as the tree expands and iterates, it determines whether the trees meet. The connectivity condition typically refers to the spatial distance (e.g., Euclidean distance, Manhattan distance, or configuration joint difference distance) between a newly expanded node in the starting tree and any node in any target tree falling within a preset neighborhood threshold range, and the line segment connecting these two points being verified to not collide in the free configuration space. When the above connectivity condition is met, it indicates that the starting tree has been successfully connected to at least one target tree.

[0037] At this point, the algorithm can seamlessly connect the path from the starting point to the connected node of the starting tree with the path from the connected node to the corresponding candidate target joint configuration of the target tree through inter-tree connectivity operations, thereby obtaining a complete joint space path. Since multiple target trees are growing simultaneously in space, the planning process may discover more than one connected path. The specific logic for determining the target trajectory can be to continuously track and compare all discovered connected paths before reaching the preset maximum number of iterations or computation timeout, and select the path with the shortest path length or the lowest smoothness cost as the final optimal joint space trajectory for output control.

[0038] The method provided in this invention first generates multiple candidate target joint configurations based on the target pose of the end effector, effectively expanding the search and coverage range of feasible solutions for the target, thereby avoiding the risk of planning failure due to a single target configuration being located in an inaccessible region due to obstacle interference. Second, multiple target trees are grown in parallel in the free configuration space with each candidate target joint configuration as the root node, and the starting tree is simultaneously controlled to actively expand to these multiple target trees. This design of multi-target parallel exploration and multi-tree directional intersection enables the algorithm to simultaneously search for paths in multiple potential configuration space regions, thereby increasing the probability of bypassing complex obstacles. Finally, when the starting tree and any target tree satisfy the connectivity condition, the target trajectory can be successfully determined, thereby improving the planning success rate and robustness of the robotic arm in complex working environments while ensuring planning efficiency.

[0039] Based on the above embodiments, since the solution process of inverse kinematics (IK) for a serial robotic arm is highly nonlinear, relying solely on random initial values ​​can easily lead to the numerical solution getting trapped in local minima or taking too long to solve. To efficiently and accurately generate high-quality solutions with spatial diversity, step S10, which generates multiple candidate target joint configurations based on the target pose, specifically includes: Step S11: Based on the target pose, obtain multiple seed joint configurations from a pre-built configuration sampling library.

[0040] Specifically, a configuration sampling library refers to a collection of large amounts of discretized configuration data and their corresponding end-effector pose information, sampled and stored in the safe configuration space of the robotic arm before executing a specific planning task. This sampling library can employ lists, hash tables, or various tree-like data structures with accelerated spatial retrieval capabilities as its underlying data storage structure.

[0041] Seed joint configurations refer to the joint configurations that serve as the initial values ​​for subsequent numerical optimization algorithm iterations. By comparing the input target pose with the pose data stored in the configuration sampling library (e.g., performing a nearest neighbor query), several configurations that are closest to the target pose in the task space are extracted as seed joint configurations. This strategy based on a pre-sampling library provides an excellent initial search starting point for inverse kinematics solving.

[0042] Step S12: Using each seed joint configuration as an initial value, solve the preset constraint optimization problem in parallel to obtain multiple initial inverse kinematic solutions. The objective function of the constraint optimization problem is constructed based on the pose error between the forward kinematic pose and the target pose, as well as the deviation between the joint configuration to be solved and the corresponding seed joint configuration.

[0043] Specifically, after obtaining multiple seed joint configurations, they are assigned to independent processing units and used in parallel as initial values ​​for iterative optimization. A constrained optimization problem framework is introduced here to solve the inverse kinematics, aiming to transform the geometric algebraic solution into a numerical optimization process. The core of the optimization problem lies in the design of the objective function. In this embodiment, the objective function is constructed from two key costs: the first is the pose error between the forward kinematic pose and the target pose, which is a hard requirement to ensure that the end of the solved configuration accurately fits the target pose; the second is the deviation between the joint configuration to be solved and the corresponding seed joint configuration (i.e., the regularization term), which aims to constrain the solution process from deviating excessively from the high-quality initial seed, thereby ensuring that it inherits the free, collision-free space properties of the seed configuration with a high probability.

[0044] Specifically, using each seed joint configuration as an initial value, the following constrained optimization problem can be solved in parallel: in, Describe the objective function. For positive kinematic functions, The target spatial pose (hereinafter referred to as the target pose) is set for the target task. This represents the pose error; The joint configuration to be solved. For the corresponding seed joint configuration, This represents configurational deviation; and These are all weight configuration parameters; Indicates constraints. and These are the physical limits of each joint of the robotic arm.

[0045] For the optimization problem described above, gradient descent solvers such as Sequential Quadratic Programming (SQP) can be used to solve it. The gradient is shown below: Through this solution process, a set of multiple initial inverse kinematic solutions can be obtained that not only meet the target pose requirements but are also subject to joint constraints.

[0046] Step S13: Deduplication is performed on the multiple initial inverse kinematics solutions, and the deduplicated initial inverse kinematics solutions are used as the multiple candidate target joint configurations.

[0047] Specifically, during the parallel solution process, even when different seed joint configurations are used as initial values, the optimization algorithm may still converge to the same local extremum point when searching in the nonlinear space, resulting in some initial inverse kinematics solutions being numerically identical or extremely similar. Retaining these identical solutions as independent parallel tree root nodes not only fails to increase the diversity of the exploration space but also wastes parallel computing resources. Therefore, this step introduces deduplication, i.e., by comparing the similarity between each initial inverse kinematics solution, redundant solutions are eliminated. The set of initial inverse kinematics solutions that remain after deduplication, exhibiting significant spatial differences, serves as the multiple candidate target joint configurations ultimately fed into step S20.

[0048] The method provided in this invention overcomes the problems of traditional inverse kinematics solutions, such as being prone to getting trapped in local optima and having a low success rate, by introducing a pre-built configuration sampling library as a solution seed and fusing it with a constrained optimization model with regularization terms. The parallel solution method using multiple seed joint configurations as initial values ​​not only improves the convergence speed and numerical stability of the solution, but also ensures that the solution configurations are less prone to abrupt collisions due to regularization terms. Furthermore, the deduplication process effectively eliminates redundant solutions while maintaining the diversity of the target state space, allowing computational resources to be highly concentrated on candidate target joint configurations with practical parallel exploration value.

[0049] Based on any of the above embodiments, the steps for constructing the configuration sampling library include: Sampling is performed within the joint space of the robotic arm according to a preset distribution to obtain multiple free joint configuration samples that meet the collision-free condition; A K-dimensional tree data structure is constructed using the pose coordinates corresponding to each of the free joint configuration samples as the index key, and the K-dimensional tree data structure is used as the configuration sampling library.

[0050] It should be noted that, in order to ensure that the configuration sampling library can support high real-time and high-precision seed configuration retrieval, the embodiments of the present invention provide a detailed description of the specific construction steps of the configuration sampling library. This construction process is usually performed as an offline preprocessing stage before the algorithm runs.

[0051] Specifically, extensive sampling is first required within the complete joint space of the robotic arm (i.e., the multi-dimensional space formed by the physical constraints of all joints). A uniform sampling distribution is typically used to ensure the breadth and unbiasedness of the samples throughout the entire configuration space. After sampling, collision detection is performed to filter out invalid samples that have experienced self-collisions or collisions with known environmental obstacles. The remaining samples are the free joint configuration samples that meet the collision-free condition.

[0052] As a specific embodiment, a rejection sampling method can be used to continuously sample in a uniform distribution across the joint space of the robotic arm until a sufficiently large number of samples are generated (e.g., no less than 10). 5 (Number) free collision joint configuration samples are used to ensure that the sampling library has sufficient spatial density.

[0053] After acquiring a massive number of free joint configuration samples, the end effector pose coordinates (i.e., task space coordinates) corresponding to each sample are calculated using the forward kinematics function of the robotic arm. These 3D or 6D pose coordinates are used as index keys to organize the massive number of sample points into a K-dimensional tree data structure. Here, the K-dimensional tree is a binary tree data structure used to partition data points in K-dimensional space, which is adept at handling keypoint search problems in multidimensional space (such as K-nearest neighbor queries). The constructed K-dimensional tree data structure serves as the final configuration sampling library. It should be understood that this K-dimensional tree sampling library is constructed for specific robotic arm morphological structures (such as specific link lengths and joint arrangements). Once calculated, it can be reused repeatedly in subsequent planning tasks without needing to be reconstructed before each planning session.

[0054] The method provided in this invention transforms the complex nonlinear inverse kinematics initial optimization problem into an efficient spatial tree nearest neighbor retrieval problem by constructing a collision-free configuration sampling library based on a K-dimensional tree offline. Since a massive number of uniformly distributed and collision-free free configuration samples are pre-generated and the K-dimensional tree is used for efficient spatial partitioning at the bottom layer, the query matching efficiency of seed configurations is improved. Furthermore, the library supports one-time construction and unlimited reuse across multiple tasks, eliminating the time bottleneck of seed retrieval while improving the real-time response capability of online trajectory planning.

[0055] Based on any of the above embodiments, step S13, the deduplication process for the plurality of initial inverse kinematic solutions, includes: Step S131: For any two initial inverse kinematics solutions among the plurality of initial inverse kinematics solutions, calculate the configuration difference distance between the two initial inverse kinematics solutions; Step S132: If the configuration difference distance is less than or equal to a preset distance threshold, remove any one of the two initial inverse kinematics solutions calculated.

[0056] Specifically, after obtaining multiple initial inverse kinematics solutions obtained in parallel, a pairwise comparison method can be used for traversal verification. Configuration difference distance refers to a mathematical measure of the similarity between two joint configuration vectors in the joint state space of the robotic arm. Specifically, this configuration difference distance can be the Euclidean distance (i.e., L2 norm), Manhattan distance (i.e., L1 norm), or the maximum absolute value of the difference between corresponding joint angle vectors between two joint angle vectors. For example, suppose two initial inverse kinematics solutions are arbitrarily selected as... and Then the Euclidean distance between the two can be calculated. As a distance of configurational difference.

[0057] This invention introduces a preset distance threshold as a criterion for determining whether two solutions are essentially the same. This threshold is a very small positive number (e.g., it can be set to...). When the calculated configurational difference distance is less than or equal to the preset distance threshold (i.e.) When the initial inverse kinematics solutions almost overlap in the multidimensional joint space, they are repeated solutions that converge to the same local minimum. In this case, during memory or set management operations, either solution is directly removed, leaving only one. By performing the above traversal comparison and removal on all solution pairs, the remaining solutions are spatially dispersed and non-overlapping independent solutions.

[0058] The method provided in this invention achieves accurate deduplication of parallel inverse kinematics solutions with minimal computational overhead by establishing a configuration difference distance metric and threshold comparison mechanism. This not only effectively eliminates redundant configurations caused by getting trapped in the same local minimum during numerical optimization, ensuring the diversity and representativeness of the final candidate target solution set in the joint space, but also minimizes the waste of subsequent parallel growth computation power by avoiding the repeated derivation of target trees at root nodes that are extremely close in space, thereby improving the exploration efficiency of the multi-target tree in the entire feasible space.

[0059] Based on any of the above embodiments, step S20 specifically includes: Step S21: Assign computation threads to each of the candidate target joint configurations; Step S22: On each of the computing threads, taking the corresponding candidate target joint configuration as the root node of the target tree, perform asynchronous expansion operations on each of the target trees in the free configuration space based on a preset sampling bias rate, wherein the preset sampling bias rate is less than the sampling bias rate for performing expansion operations on the starting tree.

[0060] Specifically, after deduplication, multiple (e.g., N=10) independent and spatially distributed candidate target joint configurations are obtained. At the underlying operating system or computing framework level, the system allocates independent computing threads to each of these N candidate configurations. This means that each target tree will have its own dedicated computing resources, and operations such as collision detection, node sampling, and spatial nearest neighbor search for each tree will all run within their own independent thread lifecycles, without the need for data synchronization or inter-thread communication. This design aligns with the parallel processing characteristics of multi-core CPU or GPU hardware-accelerated architectures.

[0061] On separate computation threads, each target tree begins to execute asynchronous extension operations (i.e., EXTEND operations). Here, "asynchronous" means that the growth progress of each tree is not constrained by other trees and does not require waiting for synchronization locks, thereby improving operational efficiency. This embodiment of the invention also introduces a sampling bias rate, which is used in classic tree search algorithms (such as the standard RRT). In this context, the sampling bias rate determines the extent to which the algorithm favors directly connecting targets versus randomly scattering points in space.

[0062] In this embodiment of the invention, the preset sampling bias rate set for the target tree is lowered (e.g., much lower than the standard RRT). The algorithm typically sets the sampling bias to 0.5, which is significantly smaller than the sampling bias probability of the initial tree expansion. The purpose of this setting is that, since the root node of the target tree is already the inverse kinematics solution of the robotic arm, the task of the target tree is to focus on exploration rather than exploitation. That is, to explore the local free configuration space around each target configuration as fully and uniformly as possible, expanding the original single point-like target solution into a large-volume target reachable connected component, thus waiting for the arrival of the initial tree.

[0063] The method provided in this invention expands a single target into a vast target reachable space by combining multi-threaded asynchronous concurrency and a low-bias exploration strategy. Independent thread allocation eliminates the communication overhead and resource contention of multi-tree growth, achieving a high parallel speedup ratio. The low sampling bias design allows each target tree to deeply explore the safe configuration area around its root node, thereby increasing the success rate of the starting tree and the target tree intersecting in the complex obstacle avoidance space.

[0064] Based on any of the above embodiments, in step S30, controlling the expansion of the starting tree to the plurality of target trees includes: Step S31: During each iteration of the growth of the starting tree, a random probability value is generated.

[0065] Specifically, the growth of a random tree is a continuous iterative process of exploration. In this step, at the beginning of each expansion iteration of the initial tree, the algorithm generates a random probability value (e.g., a continuous random number uniformly distributed in the interval between 0 and 1) through the underlying random number generator. This random probability value will serve as the core basis for determining which growth strategy the starting tree should adopt in the current iteration step.

[0066] Step S32: If the random probability value is greater than the preset probability threshold, then uniform sampling is performed in the free configuration space to obtain the first extended node, and the starting tree is extended and grown based on the first extended node.

[0067] Specifically, the system is pre-set with a preset probability threshold (e.g., design parameters) to control the exploration tendency. When the generated random probability value is greater than this threshold (i.e. When ), it means that the current iteration step's starting tree will adopt a bias strategy of global uniform exploration.

[0068] Specifically, the algorithm temporarily ignores the exact location of the target tree and performs completely random uniform sampling across the entire free-form configuration space of the robotic arm, generating a first expansion node (i.e., a globally random exploration node). Subsequently, the starting tree finds the node closest to this first expansion node and attempts to extend along the line connecting them. This globally uniform exploration mechanism, which retains a certain probability, essentially gives the starting tree the ability to diverge and find new paths in unknown space, ensuring the robotic arm can overcome complex obstacles and escape local spatial traps.

[0069] Step S33: If the random probability value is not greater than the preset probability threshold, then obtain the set of newly added nodes of each of the multiple target trees compared to the previous iteration, merge the multiple candidate target joint configurations with the set of newly added nodes corresponding to all target trees to obtain a target guidance set, sample in the target guidance set to obtain a second extension node, and perform directional expansion growth of the starting tree based on the second extension node.

[0070] Specifically, when the random probability value is not greater than a preset probability threshold (i.e. When the initial tree switches to a growth strategy that converges towards the target tree, the system continuously tracks the latest state of multiple target trees growing in parallel on other independent threads, extracting the set of newly added nodes that have just grown in the most recent iteration for each target tree. These newly added nodes represent the latest and most active exploration frontier boundaries of each target tree in space. Next, the algorithm takes the union of multiple candidate target joint configurations (which serve as the tree root) with the set of newly added frontier nodes from all the target trees, summarizing them into a dynamically updated target-oriented set. Subsequently, the algorithm randomly selects a node from this target-oriented set as a second extension node and forcibly pulls the initial tree towards the location of this second extension node (i.e., directional extension growth). This mechanism ensures that each advancement of the target tree's frontier translates into an attraction to the initial tree.

[0071] The method provided in this invention proposes an adaptive tree expansion strategy based on dynamic switching of random probability thresholds. This strategy achieves a balance between global exploration and obstacle avoidance and directional convergence towards multiple target trees in the starting tree. By introducing a dynamic target-oriented set composed of newly added nodes at the forefront of the target trees and the target root nodes, the starting tree can explore the growth trends of multiple target trees and actively move towards the most active feasible region. This greatly accelerates the spatial convergence speed of the tree structures at both ends of the starting and target sides. At the same time, the retained global uniform sampling mechanism ensures the probabilistic completeness of the algorithm, effectively prevents local extreme value stagnation, and improves the connectivity success rate and overall planning efficiency in complex environments.

[0072] Based on any of the above embodiments, step S40 specifically includes: Step S41: If the nodes of the starting tree and any target tree satisfy the connectivity condition, the starting tree and any target tree are spliced ​​together to form a connected path. Step S42: Before reaching the preset stopping condition, perform a reconnection operation on the starting tree after each tree iteration operation to update the path cost of the existing connected paths. Step S43: In the case of multiple connected paths, track the optimal connected path with the lowest cost in real time. Step S44: When the preset stopping condition is met, the optimal connected path is output as the target trajectory.

[0073] Specifically, during the iterative expansion of the starting tree, the system constantly checks whether the newly generated node in the starting tree falls within the neighborhood of any existing node in the target tree, and whether there are no collision obstacles between these two points. When this connectivity condition is met, an inter-tree connectivity operation (i.e., the CONN_TREE operation) is performed, concatenating the path segment from the starting configuration to the connected node in the starting tree with the path segment from the connected node back to the corresponding candidate target configuration in the target tree, thereby forming a complete connected path (i.e., a joint space path) in the multi-dimensional joint space.

[0074] Unlike traditional algorithms that exit immediately upon finding the first solution, this invention endows the algorithm with the ability to continuously evolve. The system sets a preset stopping condition, which can be reaching the maximum number of iterations (e.g., 3000 iterations in this embodiment) or reaching the maximum planning timeout set by the system (e.g., 3000ms). Before this stopping condition is triggered, the algorithm does not terminate computation but instead executes the RRT after each new node is generated in the initial tree. The algorithm includes a reconnection operation (REWIRE operation). This operation re-evaluates the parent-child connections of nodes within the local neighborhood. If it finds that the path cost (e.g., path geometric length) connecting an old node in the neighborhood through a new node is lower, the old branch is pruned and a new branch is built. Through this continuous pruning and recombination process, the path cost of existing connected paths decreases as the iteration progresses.

[0075] Because there are multiple candidate target configurations in space, and multiple target trees are scattered throughout, the starting tree may connect with different target trees or different branches of the same target tree during global exploration, resulting in multiple connected paths. The system maintains a path cost monitor in the background to track all successfully connected paths in real time. When a reconnection operation reduces the cost of an existing path, or a new target connection path with a lower cost is discovered, the system immediately updates the pointer, always locking onto the optimal connected path with the lowest cost.

[0076] After numerous iterative reconnections and multi-objective optimization, the planning process officially ends when the maximum number of iterations or the timeout limit (i.e., the preset stopping condition) is finally triggered. The system extracts the optimal connected path with the lowest global cost that is currently locked, performs necessary post-processing, and outputs it as the target trajectory for the final control of the robotic arm's actual movement.

[0077] The method provided in this invention integrates real-time tracking and comparison of multi-target connected paths with an asymptotically optimal reconnection mechanism. Within the same computational runtime, because it simultaneously captures multiple feasible paths leading to different inverse kinematic solutions and continuously performs reconnection optimization in the background, the algorithm can select a globally asymptotically optimal solution from multiple locally suboptimal paths. This results in a significantly lower final target trajectory cost compared to traditional methods, thereby improving the planning success rate in complex environments.

[0078] Based on any of the above embodiments Figure 2 This is a flowchart illustrating the multi-objective parallel joint space trajectory planning method for serial robotic arms provided by the present invention, as shown below. Figure 2 As shown, the overall algorithm architecture of this invention can be divided into three stages: (a) inverse kinematics (IK) sampling stage, (b) multi-objective tree parallel growth stage, and (c) initial tree expansion and connectivity stage.

[0079] In the (a) IK sampling phase, the system first receives the target pose in the task space. Then, the algorithm quickly retrieves K seed configurations from a pre-built configuration sampling library using a K-dimensional tree nearest neighbor query at the lower level. Using these as initial values ​​for optimization, the system calls the SQP optimization solver to solve the IK (inverse kinematics) constraint optimization problem in parallel to obtain initial solutions that satisfy the target pose and physical constraints. Next, after deduplication and downsampling, redundant solutions that are excessively clustered in the space are eliminated, ultimately locking in N spatially dispersed high-quality candidate target configurations (e.g., N=10).

[0080] In the parallel growth stage of the multi-objective tree in (b), the N target configurations generated above are respectively used as N target trees. The root node. These target trees are assigned to their respective independent threads, using standard RRT. The algorithm employs asynchronous parallel growth. Since the trees do not interfere with each other, it can maximize the computing power of multi-core processors. Simultaneously, newly added nodes generated during the growth of each target tree are extracted in real time and used to guide the growth of the starting tree.

[0081] In the initial tree expansion and connectivity phase (c), the system uses the initial configuration of the robotic arm. Using the root node, construct the (N+1)th tree, which is the initial tree. The initial tree employs an adaptive sampling strategy, which, in each iteration, uses a probability... Explore in the global free configuration space, using probability. Forced directional expansion is performed on the newly added front-end nodes of each target tree in stage (b). After each expansion, the system determines whether the starting tree is connected to any target tree. If not, the growth continues in a loop (e.g., ...). Figure 2The system detects whether a connection is established (e.g., a feedback path is established). If a connection is established, a reconnection operation is immediately performed (REWIRE optimization), continuously updating and tracking the optimal path with the lowest cost. When preset termination conditions are met (e.g., reaching the maximum number of iterations of 3000 or the planning time reaches 3000ms), the system outputs the optimal trajectory.

[0082] To verify the actual effectiveness of the above-mentioned multi-objective parallel planning method, the embodiments of the present invention were rigorously simulated and verified under various typical obstacle environments. Figure 3 These are schematic diagrams of the four testing environments provided by this invention, such as... Figure 3 As shown, the experiment set up four test environments with increasing difficulty, from left to right: baseline free space (Table), simple obstacle (Wall), configuration space split (Passage), and random complex obstacle (Random). The baseline free space contains only a simple environment with a tabletop support, used to test the algorithm's baseline convergence performance. The simple obstacle adds a vertical wall between the starting point S and the ending point G, requiring the robotic arm to have basic detour planning capabilities. The configuration space split sets up a narrow passage; this environment is prone to local minima in the joint configuration space, posing a significant challenge to the algorithm's space exploration capabilities. The random complex obstacle randomly scatters numerous spherical and cylindrical obstacles in the space, creating a complex environment where conventional single-objective inverse kinematics solutions are prone to falling into unreachable collision regions.

[0083] Figure 4 This invention provides a comparison curve of the median path cost of various algorithms in each iteration on a 6-DOF and 7-DOF robotic arm under random complex obstacle environments, as shown in the figure. Figure 4 As shown, under the most demanding random and complex obstacle environment, this invention compares with existing mainstream benchmark algorithms (RRT). Comparative experiments were conducted on 6-DOF and 7-DOF (redundant) serial robotic arms using RRT-Connect. The median path cost comparison curves for each algorithm in each iteration are shown below. Figure 4 As shown. From Figure 4 As can be seen, when facing complex obstacle spaces with limited height, the traditional RRT is prone to falling into the collision zone due to the single target's forward image. Both the RRT-Connect algorithm and the RRT-Connect algorithm failed to find a feasible path in the early stages of iteration (initially, the path cost was infinite, i.e.) Figure 4 The straight dashed line at the top of the middle section only occasionally happens to connect when the number of iterations is very high, and the path cost after convergence is still very high.

[0084] In contrast, the present invention (Many-RRT) By exploring multiple target foreimages in parallel, a connected path is rapidly found in the very early stages of iteration (around 1000 iterations), manifested as a significant decrease in the solid line value, resulting in a geometrical improvement in planning success rate. Furthermore, since multiple target trees collectively delineate a large reachable manifold, coupled with reconnection optimization of the starting tree, the median path cost after convergence in 6-DOF and 7-DOF robotic arms approaches the theoretical optimum (stabilizing at around 10), providing path quality and robustness far exceeding benchmark algorithms under the same iteration resources. This fully verifies the effectiveness of the method in handling challenging industrial environments.

[0085] The method provided in this invention improves the planning success rate of a high-degree-of-freedom robotic arm in complex obstacle avoidance environments by generating multiple candidate target configurations and growing multiple target trees in parallel in a free configuration space, while supplementing it with an adaptive multi-target expansion mechanism of the starting tree. Furthermore, the framework of multi-target parallel exploration enables the algorithm to quickly capture multiple potential solutions covering a wider space, so that the final output target trajectory gradually converges from the local asymptotic optimum to the global asymptotic optimum, achieving better path quality in the same running time. In addition, the characteristic of each target tree growing completely independently makes the method not only robust but also easy to parallelize and accelerate, possessing extremely high engineering practical value.

[0086] The robotic arm trajectory planning device provided by the present invention is described below. The robotic arm trajectory planning device described below can be referred to in correspondence with the robotic arm trajectory planning method described above.

[0087] Based on any of the above embodiments Figure 5 This is a schematic diagram of the robotic arm trajectory planning device provided by the present invention, as shown below. Figure 5 As shown, the device includes: The configuration generation unit 510 is used to obtain the initial joint configuration of the robotic arm and the target pose of the end effector, and generate multiple candidate target joint configurations based on the target pose. The target tree growth unit 520 is used to grow multiple corresponding target trees in parallel in the free configuration space, with each candidate target joint configuration as the root node. The starting tree expansion unit 530 is used to grow a starting tree with the starting joint configuration as the root node, and to control the starting tree to expand to the multiple target trees; The trajectory output unit 540 is used to determine the target trajectory of the robotic arm based on the path formed by the starting tree and the target tree that satisfies the connectivity condition, provided that the nodes of the starting tree and any node of the plurality of target trees satisfy the connectivity condition.

[0088] The device provided in this invention first generates multiple candidate target joint configurations based on the target pose of the end effector, effectively expanding the search and coverage range of feasible solutions for the target, thereby avoiding the risk of planning failure due to a single target configuration being located in an inaccessible area due to obstacle interference. Second, multiple target trees are grown in parallel in the free configuration space with each candidate target joint configuration as the root node, and the starting tree is simultaneously controlled to actively expand to these multiple target trees. This design of multi-target parallel exploration and multi-tree directional intersection enables the algorithm to simultaneously search for paths in multiple potential configuration space regions, thereby increasing the probability of bypassing complex obstacles. Finally, when the starting tree and any target tree satisfy the connectivity condition, the target trajectory can be successfully determined, thereby improving the planning success rate and robustness of the robotic arm in complex working environments while ensuring planning efficiency.

[0089] Based on any of the above embodiments, the configuration generation unit includes: The subunit is used to obtain multiple seed joint configurations from a pre-built configuration sampling library based on the target pose. The solution sub-unit is used to solve the preset constraint optimization problem in parallel with each seed joint configuration as the initial value, and obtain multiple initial inverse kinematic solutions. The objective function of the constraint optimization problem is constructed based on the pose error between the positive kinematic pose and the target pose, as well as the deviation between the joint configuration to be solved and the corresponding seed joint configuration. The deduplication subunit is used to deduplicate the multiple initial inverse kinematics solutions and use the deduplicated initial inverse kinematics solutions as the multiple candidate target joint configurations.

[0090] Based on any of the above embodiments, the device further includes a sampling library construction unit, the sampling library construction unit being used for: Sampling is performed within the joint space of the robotic arm according to a preset distribution to obtain multiple free joint configuration samples that meet the collision-free condition; A K-dimensional tree data structure is constructed using the pose coordinates corresponding to each of the free joint configuration samples as the index key, and the K-dimensional tree data structure is used as the configuration sampling library.

[0091] Based on any of the above embodiments, the deduplication subunit is specifically used for: For any two initial inverse kinematics solutions among the plurality of initial inverse kinematics solutions, calculate the configuration difference distance between the two initial inverse kinematics solutions; If the configuration difference distance is less than or equal to a preset distance threshold, any one of the two calculated initial inverse kinematics solutions is removed.

[0092] Based on any of the above embodiments, the starting tree expansion unit is specifically used for: During each iteration of the growth of the initial tree, a random probability value is generated; If the random probability value is greater than the preset probability threshold, then uniform sampling is performed in the free configuration space to obtain the first extended node, and the starting tree is expanded and grown based on the first extended node; If the random probability value is not greater than the preset probability threshold, then obtain the set of new nodes of each of the multiple target trees compared to the previous iteration, merge the multiple candidate target joint configurations with the set of new nodes corresponding to all target trees to obtain a target guidance set, sample in the target guidance set to obtain a second extension node, and perform directional expansion growth of the starting tree based on the second extension node.

[0093] Based on any of the above embodiments, the target tree growth unit is specifically used for: Assign computation threads to each of the candidate target joint configurations; On each of the computing threads, the corresponding candidate target joint configuration is used as the root node of the target tree. An asynchronous expansion operation is performed on each of the target trees in the free configuration space based on a preset sampling bias rate, wherein the preset sampling bias rate is less than the sampling bias rate for performing the expansion operation on the starting tree.

[0094] Based on any of the above embodiments, the trajectory output unit is specifically used for: If the nodes of the starting tree and any target tree satisfy the connectivity condition, the starting tree and any target tree are concatenated to form a connected path. Before reaching the preset stopping condition, a reconnection operation is performed on the starting tree after each tree iteration operation to update the path cost of the existing connected paths. In the case of multiple connected paths, the optimal connected path with the lowest cost is tracked in real time. When the preset stopping condition is met, the optimal connected path is output as the target trajectory.

[0095] Figure 6 This is a schematic diagram of the structure of the electronic device provided by the present invention, such as... Figure 6As shown, the electronic device may include a processor 610, a communication interface 620, a memory 630, and a communication bus 640, wherein the processor 610, the communication interface 620, and the memory 630 communicate with each other through the communication bus 640. The processor 610 can call logical instructions in the memory 630 to execute a robotic arm trajectory planning method. The method includes: acquiring the initial joint configuration of the robotic arm and the target pose of the end effector, and generating multiple candidate target joint configurations based on the target pose; using each candidate target joint configuration as a root node, growing multiple corresponding target trees in parallel in a free configuration space; growing an initial tree using the initial joint configuration as a root node, and controlling the initial tree to expand to the multiple target trees; and determining the target trajectory of the robotic arm based on the path formed by the initial tree and the target trees that satisfy the connectivity condition, provided that the nodes of the initial tree and any node of any of the multiple target trees satisfy the connectivity condition.

[0096] Furthermore, the logical instructions in the aforementioned memory 630 can be implemented as software functional units and, when sold or used as independent products, can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of the present invention, or the part that contributes to related technologies, or a portion of the technical solution, can be embodied in the form of a software product. This computer software product is stored in a storage medium and includes several instructions to cause a computer device (which may be a personal computer, server, or network device, etc.) to execute all or part of the steps of the methods described in the various embodiments of the present invention. The aforementioned storage medium includes various media capable of storing program code, such as USB flash drives, portable hard drives, read-only memory (ROM), random access memory (RAM), magnetic disks, or optical disks.

[0097] On the other hand, the present invention also provides a computer program product, which includes a computer program that can be stored on a non-transitory computer-readable storage medium. When the computer program is executed by a processor, the computer can execute the robotic arm trajectory planning method provided by the above methods. The method includes: obtaining the initial joint configuration of the robotic arm and the target pose of the end effector, and generating multiple candidate target joint configurations based on the target pose; using each candidate target joint configuration as a root node, growing multiple corresponding target trees in parallel in a free configuration space; growing an initial tree using the initial joint configuration as a root node, and controlling the initial tree to expand to the multiple target trees; and determining the target trajectory of the robotic arm based on the path formed by the initial tree and the target trees that satisfy the connection condition, provided that the nodes of the initial tree and any node of the multiple target trees satisfy the connection condition.

[0098] In another aspect, the present invention also provides a non-transitory computer-readable storage medium storing a computer program thereon. When executed by a processor, the computer program implements a robotic arm trajectory planning method provided by the above methods. The method includes: acquiring the initial joint configuration of the robotic arm and the target pose of the end effector, and generating multiple candidate target joint configurations based on the target pose; using each candidate target joint configuration as a root node, growing multiple corresponding target trees in parallel in a free configuration space; growing an initial tree using the initial joint configuration as a root node, and controlling the initial tree to expand to the multiple target trees; and determining the target trajectory of the robotic arm based on the path formed by the initial tree and the target trees that satisfy the connectivity condition, provided that the nodes of the initial tree and any node of any of the multiple target trees satisfy the connectivity condition.

[0099] The device embodiments described above are merely illustrative. The units described as separate components may or may not be physically separate. The components shown as units may or may not be physical units; that is, they may be located in one place or distributed across multiple network units. Some or all of the modules can be selected to achieve the purpose of this embodiment according to actual needs. Those skilled in the art can understand and implement this without any creative effort.

[0100] Through the above description of the embodiments, those skilled in the art can clearly understand that each embodiment can be implemented by means of software plus necessary general-purpose hardware platforms, and of course, it can also be implemented by hardware. Based on this understanding, the above technical solutions, in essence or the parts that contribute to the related technology, can be embodied in the form of software products. This computer software product can be stored in a computer-readable storage medium, such as ROM / RAM, magnetic disk, optical disk, etc., and includes several instructions to cause a computer device (which may be a personal computer, server, or network device, etc.) to execute the methods described in the various embodiments or some parts of the embodiments.

[0101] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, and not to limit them; although the present invention has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that modifications can still be made to the technical solutions described in the foregoing embodiments, or equivalent substitutions can be made to some of the technical features; and these modifications or substitutions do not cause the essence of the corresponding technical solutions to deviate from the spirit and scope of the technical solutions of the embodiments of the present invention.

Claims

1. A method for planning the trajectory of a robotic arm, characterized in that, include: The starting joint configuration of the robotic arm and the target pose of the end effector are obtained, and multiple candidate target joint configurations are generated based on the target pose. Using each candidate target joint configuration as the root node, multiple corresponding target trees are grown in parallel in the free configuration space; The starting joint configuration is used as the root node to grow a starting tree, and the starting tree is controlled to expand to the multiple target trees; If the nodes of the starting tree and any node of any of the multiple target trees satisfy the connectivity condition, the target trajectory of the robotic arm is determined based on the path formed by the starting tree and the target tree that satisfies the connectivity condition.

2. The robotic arm trajectory planning method according to claim 1, characterized in that, The generation of multiple candidate target joint configurations based on the target pose includes: Based on the target pose, multiple seed joint configurations are obtained from a pre-built configuration sampling library; Each seed joint configuration is used as an initial value to solve a preset constraint optimization problem in parallel, resulting in multiple initial inverse kinematic solutions. The objective function of the constraint optimization problem is constructed based on the pose error between the forward kinematic pose and the target pose, as well as the deviation between the joint configuration to be solved and the corresponding seed joint configuration. The multiple initial inverse kinematics solutions are deduplicated, and the deduplicated initial inverse kinematics solutions are used as the multiple candidate target joint configurations.

3. The robotic arm trajectory planning method according to claim 2, characterized in that, The construction steps of the configuration sampling library include: Sampling is performed within the joint space of the robotic arm according to a preset distribution to obtain multiple free joint configuration samples that meet the collision-free condition; A K-dimensional tree data structure is constructed using the pose coordinates corresponding to each of the free joint configuration samples as the index key, and the K-dimensional tree data structure is used as the configuration sampling library.

4. The robotic arm trajectory planning method according to claim 2, characterized in that, The deduplication process for the plurality of initial inverse kinematic solutions includes: For any two initial inverse kinematics solutions among the plurality of initial inverse kinematics solutions, calculate the configuration difference distance between the two initial inverse kinematics solutions; If the configuration difference distance is less than or equal to a preset distance threshold, any one of the two calculated initial inverse kinematics solutions is removed.

5. The robotic arm trajectory planning method according to claim 1, characterized in that, The process of controlling the expansion of the starting tree to the plurality of target trees includes: During each iteration of the growth of the initial tree, a random probability value is generated; If the random probability value is greater than the preset probability threshold, then uniform sampling is performed in the free configuration space to obtain the first extended node, and the starting tree is expanded and grown based on the first extended node; If the random probability value is not greater than the preset probability threshold, then obtain the set of newly added nodes of each of the multiple target trees compared to the previous iteration, merge the multiple candidate target joint configurations with the set of newly added nodes corresponding to all target trees to obtain a target guidance set, sample in the target guidance set to obtain a second extension node, and perform directional expansion growth of the starting tree based on the second extension node.

6. The robotic arm trajectory planning method according to claim 1, characterized in that, The process of growing multiple target trees in parallel in the free configuration space, using each candidate target joint configuration as the root node, includes: Assign computation threads to each of the candidate target joint configurations; On each of the computing threads, the corresponding candidate target joint configuration is used as the root node of the target tree. An asynchronous expansion operation is performed on each of the target trees in the free configuration space based on a preset sampling bias rate, wherein the preset sampling bias rate is less than the sampling bias rate for performing the expansion operation on the starting tree.

7. The robotic arm trajectory planning method according to any one of claims 1 to 6, characterized in that, When the nodes of the starting tree and any node of any of the plurality of target trees satisfy the connectivity condition, the target trajectory of the robotic arm is determined based on the path formed by the starting tree and the target trees that satisfy the connectivity condition, including: If the nodes of the starting tree and any target tree satisfy the connectivity condition, the starting tree and any target tree are concatenated to form a connected path. Before reaching the preset stopping condition, a reconnection operation is performed on the starting tree after each tree iteration operation to update the path cost of the existing connected paths. In the case of multiple connected paths, the optimal connected path with the lowest cost is tracked in real time. When the preset stopping condition is met, the optimal connected path is output as the target trajectory.

8. A robotic arm trajectory planning device, characterized in that, include: The configuration generation unit is used to obtain the initial joint configuration of the robotic arm and the target pose of the end effector, and generate multiple candidate target joint configurations based on the target pose. The target tree growth unit is used to grow multiple corresponding target trees in parallel in the free configuration space, with each candidate target joint configuration as the root node; A starting tree extension unit is used to grow a starting tree with the starting joint configuration as the root node, and to control the starting tree to extend to the multiple target trees; The trajectory output unit is used to determine the target trajectory of the robotic arm based on the path formed by the starting tree and the target tree that satisfies the connectivity condition, provided that the nodes of the starting tree and any node of the plurality of target trees satisfy the connectivity condition.

9. An electronic device comprising a memory, a processor, and a computer program stored in the memory and running on the processor, characterized in that, When the processor executes the computer program, it implements the robotic arm trajectory planning method as described in any one of claims 1 to 7.

10. A non-transitory computer-readable storage medium having a computer program stored thereon, characterized in that, When the computer program is executed by the processor, it implements the robotic arm trajectory planning method as described in any one of claims 1 to 7.

Images