Robot motion planning method based on mixed space

Abstract

The invention discloses a robot motion planning method based on a mixed space, which comprises the following steps: generating interpolation nodes according to a given tracking route, constructing a hierarchical search tree, then estimating a motion range of each joint based on the tracking route, screening out an effective joint configuration of a starting point on the basis of the motion range, and forming a root node set. And carrying out downward iterative search from the next planning point of the root node of the search tree, searching a sampling pose meeting a constraint condition, and selecting a node from the hierarchy of the hierarchical search tree corresponding to the previous planning point as a candidate father node pose. Then, detecting the validity of a local path between the candidate father node and the sample node; when the local path meets the task constraint, deriving the joint configuration of the sample node from the joint configuration of the candidate father node, and performing collision detection and joint limit judgment. And if all conditions are met, adding the joint configuration of the sample pose into the search tree. And when successfully expanding to the last planning point, the planning is successful.

Description

Technical Field

[0001] This invention relates to the field of artificial intelligence, and in particular to a robot motion planning method based on hybrid space. Background Technology

[0002] Controlling a robot's end effector to move along a given path is a common application scenario in production. In tasks such as welding, polishing, and cutting, the essential requirement is for the actuator (tool) to move along a predetermined path. These movements often place certain demands on the actuator's path tracking accuracy and its motion posture. Therefore, when using robots to replace human labor in these processing tasks, the relevant planning algorithms must prioritize satisfying various pose constraints within the operating space. However, due to the control characteristics of robots, which require a series of joint configurations to achieve motion control, the misalignment between the planning space and the control space poses a challenge to motion planning.

[0003] Based on the different search spaces planned, motion planning algorithms for moving along a given route can be divided into two categories:

[0004] Joint space planning: Directly plan the changes of the angle of each joint over time in the joint space, and then convert it to the end effector trajectory in Cartesian space through a forward kinematics model. However, the disadvantage is that due to the task constraints, the probability of sampling the effective pose in Cartesian space from the joint space is extremely low or even zero, which makes it difficult to guarantee that the motion of the end effector meets the constraints and accuracy.

[0005] Operation space planning: The trajectory is directly planned in the task space (i.e., the workspace of the end effector), and then converted into motion commands in the joint space through inverse kinematics. It is suitable for tasks that require precise control of the position and attitude of the end effector. However, since the inverse kinematics of a redundant degree-of-freedom robot is usually not unique and the relationship between the operation space and the joint space is difficult to express analytically, the planned joint motions often have abrupt changes, singularities, or exceed limits, making the motion practically infeasible. Summary of the Invention

[0006] In order to overcome the above-mentioned shortcomings and deficiencies of the prior art, the purpose of this invention is to provide a robot motion planning method based on hybrid space, wherein the hybrid space refers to the operation space search expansion and joint space derivation.

[0007] This invention first initializes the joint space of the root node of the search tree and performs an initial screening of the joint configuration of the root node based on the given tracking path. At subsequent planning points, the actuator motion pose is planned in the operation space, and the joint configuration of the current node is derived from the local path in the operation space and the joint configuration of the parent node. If a joint exceeds its limits or a collision occurs at a discrete point on the local path during the derivation process, that pose can be directly discarded, and other sample poses can be considered instead.

[0008] The objective of this invention is achieved through the following technical solution:

[0009] A robot motion planning method based on hybrid space, comprising:

[0010] S1 initializes the given tracking route and obtains the interpolation nodes, i.e., the planning points; constructs a hierarchical search tree based on the planning points, initializes the search tree and calculates the operation space corresponding to the root node, deletes poses that do not meet the constraints, and obtains the effective operation space;

[0011] S2 estimates the range of motion of each joint based on the given tracking route, and filters the joint configurations corresponding to each pose in the effective operating space based on the range of motion of each joint, to obtain a set of joint configurations, which serves as the root node of the search tree.

[0012] S3 starts iterating from the next planning point after the root node of the search tree. Each iteration determines the sampling pose that meets the constraints in the effective operation space.

[0013] S4. Select a joint configuration from the joint space of the search tree node corresponding to the previous planning point, and calculate the operation space pose corresponding to the selected joint configuration as the candidate parent node pose.

[0014] S5. Within the operating space, the local path between the candidate parent node pose and the sample pose is calculated using the operating space motion interpolation algorithm. The tracking accuracy and attitude constraints of each discrete point on the local path are judged. If they are satisfied, the sampled pose is legal and valid in the operating space. Otherwise, the pose is abandoned, and a valid local spatial path is finally obtained.

[0015] S6. Obtain the joint configuration of the parent node based on the candidate parent node pose. Then, in the joint space, the joint configuration of the candidate parent node is derived sequentially along the local path to obtain the joint configuration of each discrete point, ensuring the continuity of joint motion. Collision detection and joint limit judgment are performed on the joint configuration. If all the above conditions are met, the joint configuration of the sample pose is obtained. The joint configuration of the sample pose is valid and is added to the search tree.

[0016] S7. When the termination condition is met, the planning ends and the motion planning path is obtained.

[0017] Furthermore, interpolation nodes are extracted from the given tracking route. Through interpolation operations, the interpolated curves are made to fit the points of the given route within the required range, which are then designated as planning points.

[0018] Furthermore, the effective operating space is calculated using the reachability sphere.

[0019] Furthermore, the specific steps are as follows:

[0020] Establish an accessibility sphere with the location of the planning point as its center;

[0021] Based on the constraints, the poses on the reachability sphere are filtered to obtain an effective operating space composed of discrete poses.

[0022] Furthermore, in S2, the range of motion of each joint is obtained by using differential inverse kinematics.

[0023] Furthermore, linear interpolation and unit quaternion interpolation are used to generate local paths in the operation space.

[0024] Furthermore, in S2, the screening is performed by inverse kinematics calculation.

[0025] Furthermore, the operational space pose corresponding to the joint configuration is obtained through forward kinematics calculations.

[0026] Furthermore, S6 specifically includes:

[0027] If the joint configuration of the discrete point is determined to be valid, then the configuration of the next discrete point is derived based on the joint configuration of the discrete point.

[0028] If a discrete point cannot meet the obstacle avoidance condition or joint limit judgment, it means that the parent node cannot move to the sample pose along the local path in the operation space. In this case, the candidate parent node is abandoned and other candidate parent nodes are considered. If all candidate parent nodes of the sample do not meet the requirements, other samples should be considered. If it is found that none of the n samples that have been verified can be connected to the search tree, it means that the planning has failed.

[0029] Furthermore, in S7, when the search tree expands to the last planning point, the search ends, and the node with the minimum cumulative cost is selected from the last layer and the search continues forward to the root node, which is the final motion planning path.

[0030] Compared with the prior art, the present invention has the following advantages and beneficial effects:

[0031] (1) This invention is applicable to most operations that require movement along a given path. While satisfying the requirement of moving along a given path, it also supports handling other multiple posture constraints and adapts to the processing requirements of different processing technologies.

[0032] (2) This invention solves the problems of traditional planning algorithms for motion along a given route by organically combining the planning of the operation space and the planning of the joint space, so that the algorithm can meet the motion requirements of each space in stages during the planning process. It avoids the problems of end effector tracking accuracy and work quality in the joint space planning algorithm, and solves the problems of multiple inverse kinematic solutions selection and joint mutation in the operation space planning algorithm.

[0033] (3) The present invention has strong scalability and retains a general-purpose interface in terms of planning space calculation and constraint model, which can be compatible with different planning space calculation methods and constraint description models. Taking planning space calculation methods as an example, it supports both continuous model calculation such as the incremental EM algorithm and discrete model calculation such as the reachability ball. Attached Figure Description

[0034] Figure 1 This is a flowchart of the process of this invention;

[0035] Figure 2 This is a schematic diagram illustrating the process of expanding nodes in the search tree according to the present invention;

[0036] Figure 3 This is a schematic diagram of the hierarchical search tree of the present invention. Detailed Implementation

[0037] The present invention will be further described in detail below with reference to the embodiments, but the implementation of the present invention is not limited thereto.

[0038] Example

[0039] like Figure 1 and Figure 2 As shown, a robot motion planning method based on hybrid space is presented in this embodiment. This method uses operation space expansion and joint space derivation to derive a hybrid robot motion planning method for a given weld seam path requiring full welding. The main steps include:

[0040] S1 initializes the given tracking route and obtains a series of interpolation nodes, i.e., planning points; constructs a hierarchical search tree based on the planning points, initializes the search tree and calculates the operation space corresponding to the root node, deletes poses that do not meet the constraints, and obtains the effective operation space;

[0041] To further explain, such as Figure 3As shown, this embodiment obtains five planning points P0, P1, P2, P3, and P4 based on a given tracking route. A hierarchical search tree is constructed based on these planning points. During initialization, this embodiment sets the root node and maximum number of levels in the search tree. The maximum number of levels, the discrete points corresponding to each level, the coordinate system of the discrete points, and the constraint information are all obtained through preprocessing of the given tracking route. The root node (starting point) of the search tree is not limited to a fixed configuration but is a set of joint configurations that satisfy all constraints at the starting planning point, which can improve the planning success rate of this type of multi-constraint planning problem.

[0042] In this embodiment, an reachability sphere is used to calculate the effective operating space because welding has high tracking accuracy for a given weld seam, and the position information of the end effector is relatively fixed. Specifically:

[0043] S11. Construct an accessibility sphere with the location of the planned point as its center, and exhaustively discretize all poses at that location using a certain resolution. Here, the accessibility sphere is only used to determine whether each pose is reachable by a joint configuration, and does not directly determine the joint configurations of each pose on the sphere.

[0044] S12. The reachability sphere can directly obtain poses that satisfy reachability and obstacle avoidance constraints and tracking constraints. Other constraints are adapted according to process requirements, such as welding working angle and travel angle requirements. These attitude constraints further filter the poses on the reachability sphere. Finally, an effective operating space composed of multiple discrete poses is obtained.

[0045] In this embodiment, the constraints are determined by the specific processing technology, as well as the basic obstacle avoidance risks and joint limit constraints.

[0046] S2 estimates the range of motion of each joint based on the given tracking route, and filters the joint configurations corresponding to each pose in the effective operating space based on the estimated range of motion of each joint, so as to obtain a set of joint configurations with high reliability, which serves as the root node of the search tree.

[0047] To further explain, when the path search process is set to a hybrid spatial planning mode based on operation space search of motion continuity and the derivation of parent node joint configuration to determine child node joint configuration, the root node joint configuration becomes crucial throughout the planning process. When the root node joint configuration is in a poor configuration with insufficient motion margin, it often leads to certain joints reaching their limit positions during the joint derivation process, resulting in planning failure. Therefore, it is necessary to appropriately screen the root node configuration during tree initialization.

[0048] Before planning, we discretized the given route and established a discrete point coordinate system for each discrete point. The discrete point coordinate system, as the ideal working pose of TCP, can serve as the basic reference for the operation motion path. Similarly, the discrete point coordinate system of the root node can be used to represent the basic pose at the root node. By evaluating whether the motion margin of different (inverse kinematics) joint configurations corresponding to the root node coordinate system is sufficient, we can determine which configuration among the robot's several inverse kinematic solutions has insufficient motion margin.

[0049] In this embodiment, during the coarse evaluation of the actuator's motion based on the discrete point coordinate system, differential inverse kinematics is used to estimate the range of joint changes of several sets of inverse solutions corresponding to the discrete point coordinate system of the root node during the motion along a given path. Then, using this range of motion as a benchmark, it is determined whether the joint margins of the 16 sets of inverse solutions (taking a six-axis robot as an example) corresponding to the poses satisfying the task constraints in the effective pose space of the root node satisfy the range of motion. This is used to select the relatively matching inverse solution (joint configuration) among the 16 sets of inverse solutions for that pose.

[0050] In this embodiment, step S2, which estimates the range of motion of each joint based on the given tracking route, is mainly achieved using differential inverse kinematics.

[0051]

[0052] In the formula, the pose change of adjacent discrete point coordinate systems is Δt = [x,y,z,rx,ry,rz]. It is the pseudo-inverse of the corresponding Jacobian matrix. Δθ is calculated along the discrete point coordinate system of the given route. Finally, it can roughly determine the amount of joint motion required by each joint of the joint configuration when completing the given route. If the possibility of exceeding the limit is found, the inverse solution is excluded.

[0053] S3. Starting from the next planning point of the root node of the search tree, each iteration calculates the effective planning space at the position of the planning point in the operation space, and samples the valid pose, i.e. the sample pose, from the effective planning space.

[0054] S4. Select a suitable joint configuration from the joint space of the search tree node corresponding to the previous planning point, and obtain the corresponding operation space pose through forward kinematics calculation, as the candidate parent node pose.

[0055] S5. Temporarily ignore the limitations of the joint space and only consider the motion in the operation space. Generate a local path in the operation space between the candidate parent node pose obtained in S4 and the sample pose obtained in S3. The sample pose is the sampled pose in S3. Calculate the local path according to the operation space interpolation function. Perform tracking accuracy and attitude constraint judgment on each discrete point of the local path. If both accuracy and constraint requirements are met, the sample is considered valid in the operation space; otherwise, discard the sample and consider other samples.

[0056] S6. After obtaining a valid operation space sample, attempt to derive the joint configuration of each discrete point sequentially along the local path, starting from the joint configuration of the candidate parent node, to ensure the continuity of joint motion. Collision detection and joint limit judgment are then performed on the joint configuration. If all the above conditions are met, and the joint configuration of the sample pose can be successfully derived, then the joint configuration of this sample pose is valid and added to the search tree. Specifically:

[0057] S61. For a sample pose that is determined to be valid, try to derive the joint configuration of the discrete point by starting from the joint configuration of the candidate parent node in the joint space and using the numerical solution of inverse kinematics. Iterate along the local path to derive the joint configuration of the discrete point.

[0058] S62. Perform collision detection and joint limit judgment on the joint configuration. If the joint configuration at a discrete point is determined to be valid, then use the joint configuration at that discrete point as the starting value, and iteratively derive the joint configuration at the next discrete point using the inverse kinematics numerical solution method, repeating the above judgment. If the obstacle avoidance conditions cannot be met or the joint movement exceeds the joint limit at some intermediate discrete point, it means that the parent node cannot move to the sample pose along the local path in the operation space, so the candidate parent node is directly abandoned, and other candidate parent nodes are considered instead; if all candidate parent nodes of the sample do not meet the requirements, other samples should be considered; if it is found that none of the n samples that have been verified can be connected to the search tree, it means that the planning has failed.

[0059] S63. If the joint configuration of the sample pose can be obtained normally through derivation, it means that the joint configuration of the sample pose is valid and it is added to the search tree.

[0060] Specific hierarchical search tree node expansions include: Figure 2As shown, new nodes are sampled from the sampling space, and several nodes with the lowest cost are selected from the previous layer of the hierarchical search tree as candidate parent nodes. Then, interpolation is performed between each candidate parent node and its child node to generate a local path in the operation space. If the route feature between the current child node sample and the parent node is a straight line, then a straight line interpolation is directly performed between the child node and the parent node to generate a local path. If the route feature between the current child node and the parent node is an arc, after knowing the pose of the sample and the parent node, the position information of one interpolation node is still missing before the arc interpolation can be realized. First, it is determined whether the current layer is the last point of the planned point in the current arc. If it is the last point, the parent node of the candidate parent node is taken as the first interpolation node of the arc interpolation to obtain the local path of the second segment of the arc interpolation. If it is not the last point, the discrete coordinate system of the planned point of the next layer is taken, and the pose of the coordinate system is used as the position information of the third point of the arc interpolation. After obtaining the position information of the three points, the local arc path between the candidate parent node and the child node can be calculated and generated, combined with the local path in the operation space between the candidate parent node and the child node. If the route feature between the current child node sample and the parent node is a free curve, since spline interpolation is a global operation, it is necessary to extract the poses of all planned points belonging to the current feature edge. Besides the sample and parent node poses, the poses of planned points that have already been planned are retrieved by backtracking from the candidate parent nodes using a hierarchical search tree. For unplanned points, their discrete coordinate systems are used to replace the poses of these planned points. After obtaining enough points, spline interpolation is performed between each candidate parent node to generate a local path in the operation space.

[0061] Then, it is determined whether the pose of each interpolation node in the local path satisfies the defined operation space constraints, including position, orientation, and process. If each interpolation point in the local path satisfies all operation space constraints, the local path is considered valid.

[0062] Then, based on the joint configuration of the parent node, the configuration of each joint is recursively derived according to the principle of motion continuity. During this process, it is checked whether the joint configuration corresponding to each interpolation node satisfies all joint space constraints. When the joint configuration of the child node is successfully derived along the local path, it indicates that the parent node meets the requirements.

[0063] S7. Determine the termination condition: Determine whether the sampling of the current planned point in the search tree has ended. The termination condition is that the node expansion of the last layer of the hierarchical search tree has been completed.

[0064] Specifically, the search ends when the search tree extends to the last planning point. The node with the minimum cumulative cost is selected from the last layer, and the search continues forward to the root, yielding the final optimal path. The cumulative cost is a function that primarily considers joint motion and the end effector's tracking accuracy for a given route.

[0065] This invention proposes a sampling planning algorithm based on hybrid space. This method combines the advantages of joint space and Cartesian space planning, and can cleverly plan joint configurations that can ensure trajectory tracking accuracy, meet various pose constraints in the operating space, and obtain continuous joint configurations.

[0066] The above embodiments are preferred embodiments of the present invention, but the embodiments of the present invention are not limited to the embodiments described above. Any changes, modifications, substitutions, combinations, or simplifications made without departing from the spirit and principle of the present invention shall be considered equivalent substitutions and shall be included within the protection scope of the present invention.

Claims

1. A robot motion planning method based on hybrid space, characterized in that, include: S1 initializes the given tracking route and obtains the interpolation nodes, i.e., the planning points; Construct a hierarchical search tree based on the planning points, initialize the search tree and calculate the operation space corresponding to the root node, delete poses that do not meet the constraints, and obtain the effective operation space. S2 estimates the range of motion of each joint based on the given tracking route, and filters the joint configurations corresponding to each pose in the effective operating space based on the range of motion of each joint, to obtain a set of joint configurations, which serves as the root node of the search tree. S3 starts iterating from the next planning point after the root node of the search tree. Each iteration determines the sampling pose that meets the constraints in the effective operation space. S4. Select a joint configuration from the joint space of the search tree node corresponding to the previous planning point, and calculate the operation space pose corresponding to the selected joint configuration as the candidate parent node pose. S5. Within the operating space, the local path between the candidate parent node pose and the sample pose is calculated using the operating space motion interpolation algorithm. The tracking accuracy and attitude constraints of each discrete point on the local path are judged. If they are satisfied, the sampled pose is legal and valid in the operating space. Otherwise, the pose is abandoned, and a valid local spatial path is finally obtained. S6. Obtain the joint configuration of the parent node based on the candidate parent node pose. Then, in the joint space, the joint configuration of the candidate parent node is derived sequentially along the local path to obtain the joint configuration of each discrete point, ensuring the continuity of joint motion. Collision detection and joint limit judgment are performed on the joint configuration. If all the above conditions are met, the joint configuration of the sample pose is obtained. The joint configuration of the sample pose is valid and is added to the search tree. S7. When the termination condition is met, the planning ends and the motion planning path is obtained.

2. The robot motion planning method according to claim 1, characterized in that, Interpolation nodes are extracted from the given tracking route. Through interpolation operations, the interpolated curve is made to fit the points of the given route within the required range, which are then designated as planning points.

3. The robot motion planning method according to claim 1, characterized in that, The effective operating space is calculated using the reachability sphere.

4. The robot motion planning method according to claim 3, characterized in that, The specific steps are as follows: Establish an accessibility sphere with the location of the planning point as its center; Based on the constraints, the poses on the reachability sphere are filtered to obtain an effective operating space composed of discrete poses.

5. The robot motion planning method according to claim 1, characterized in that, In S2, the range of motion of each joint is obtained by using differential inverse kinematics.

6. The robot motion planning method according to claim 1, characterized in that, Local paths in the operation space are generated using linear interpolation and unit quaternion interpolation.

7. The robot motion planning method according to claim 1, characterized in that, In S2, the screening is performed by inverse kinematics calculation.

8. The robot motion planning method according to claim 1, characterized in that, The operational space pose corresponding to the joint configuration is obtained through forward kinematics calculations.

9. The robot motion planning method according to claim 1, characterized in that, Specifically, S6 is: If the joint configuration of the discrete point is determined to be valid, then the configuration of the next discrete point is derived based on the joint configuration of the discrete point. If a discrete point cannot meet the obstacle avoidance condition or joint limit judgment, it means that the parent node cannot move to the sample pose along the local path in the operation space. In this case, the candidate parent node is abandoned and other candidate parent nodes are considered. If all candidate parent nodes of the sample do not meet the requirements, other samples should be considered. If it is found that none of the n samples that have been verified can be connected to the search tree, it means that the planning has failed.

10. The robot motion planning method according to any one of claims 1-9, characterized in that, In step S7, when the search tree expands to the last planning point, the search ends. The node with the minimum cumulative cost is selected from the last layer and the search continues forward to the root node, which is the final motion planning path.

Images