Robot grabbing obstacle avoidance path planning method based on point cloud environment modeling, storage medium and computer equipment

Abstract

The invention discloses a robot grabbing obstacle avoidance path planning method based on point cloud environment modeling, a storage medium and computer equipment, and the method comprises the following steps: S1, obtaining a grabbing scene point cloud through employing three-dimensional vision equipment, and obtaining a current robot pose through employing a robot control system; s2, an environment modeling module is constructed based on a point cloud condition filtering method and an octree technology, and after the grabbed point cloud and the current robot pose are processed by the environment modeling module, space occupation information is output; s3, an obstacle avoidance path planning module PQ-RRT * is constructed, the obstacle avoidance path planning module PQ-RRT * is optimized, and a sampling planning algorithm module DP-PQ-RRT * is obtained; and S4, on the basis of the space occupation information, the optimized sampling planning algorithm module DP-PQ-RRT * is deployed to a mechanical arm grabbing platform so as to conduct grabbing obstacle avoidance work. According to the method, the optimization convergence speed and the accuracy and robustness of collision detection can be improved in path planning in the joint space, and a collision-free obstacle avoidance path is generated for robot grabbing.

Description

Technical Field

[0001] This invention relates to robotics technology, specifically to a method for planning a robot grasping and obstacle avoidance path based on point cloud environment modeling, a storage medium, and a computer device. Background Technology

[0002] With the development of intelligent vision algorithms, home service robots are improving users' quality of life by grasping everyday items. Modeling complex and ever-changing living scenarios to accurately estimate the space occupied by obstacles and planning a collision-free obstacle avoidance trajectory to guide the robot to the appropriate grasping posture to perform the grasping action will help improve the service quality of home service robots.

[0003] Robots need to embed object grasping algorithms in their control systems to perform grasping actions. These algorithms utilize scene information from visual sensors and employ specific neural networks to calculate the grasping pose for the target object. However, current grasping frameworks focus on accurately estimating the target grasping pose without considering obstacle avoidance issues during the robot's movement from its current pose to the grasped target pose.

[0004] In recent years, researchers have shown increasing interest in developing better obstacle avoidance path planning methods for robots. Path planning algorithms can be mainly divided into four categories: artificial potential field methods, graph-based search algorithms, intelligent algorithms, and sampling-based algorithms. Among them, artificial potential field methods are prone to getting trapped in local minima, leading to path planning failure; graph-based search algorithms incur high computational costs in high-dimensional spaces; and intelligent algorithms require a learning phase, which results in long computation times and significant storage costs.

[0005] Sampling-based algorithms can effectively solve high-dimensional planning problems, enabling planning within the robot's joint space without requiring a given path for the robot's end effector. Compared to the algorithms mentioned above, sampling-based algorithms require less computation and possess probabilistic completeness. Sampling-based algorithms are mainly divided into Probabilistic Path Graphs (PRM) and Random Tree Search (RRT). PRM's workflow consists of a learning phase and a query phase, and it is only suitable for path planning in static environments. RRT has higher efficiency and lower time cost than PRM, but its convergence speed is slow and it lacks asymptotic optimality. To address these issues, two variants of RRT have been developed: RRT-Connect and RRT*. Currently, researchers mainly improve RRT from five aspects: sampling method, node expansion method, cost function, post-planning processing, and step size optimization.

[0006] Home service robots perform grasping tasks using robotic arms. Since the robotic arm cannot be considered a point mass in the planning process, its path planning differs significantly in dimensionality from that of mobile robots, requiring planning within joint space. However, the aforementioned improvements to Path Retrieval and Optimization (RRT) focus on path quality and convergence rate in two-dimensional and three-dimensional spaces. When directly applied to joint space, the increased dimensionality of the required planning still results in low optimization efficiency and slow convergence speed. Furthermore, the aforementioned solutions treat the planned object as a point mass during the planning process; therefore, when applied to joint space, they can only consider collision detection at the end effector, not collision detection of the robotic arm itself.

[0007] Existing path planning methods are also unsuitable for obstacle avoidance tasks in real-world scenarios. Most solutions neglect accurate environmental modeling to support the algorithm, often relying on simplified 3D models of the robot and obstacles for collision detection. Technically, accurate environmental modeling ensures that collision-free paths found within the environment are applicable to real-world conditions. However, obtaining high-precision environmental models is challenging due to a lack of measurement data. Some researchers use extended collision boxes to enclose obstacles in the environment, trading greater path safety tolerance for shorter modeling time. However, for coarsely modeled environments composed of simple bounding boxes, further path optimization is difficult to guarantee. Moreover, in real-world scenarios, the environment for grasping tasks is not fixed; it often changes with the movement of objects or shifts in the workspace, requiring frequent updates to the 3D model and leading to reduced service efficiency.

[0008] To address these issues, point clouds have emerged as a viable solution. Accurate point clouds over large areas can be obtained through scanning with cameras or radar mounted outside the eye-hand system. Modeling the environment using point clouds offers superior accuracy, accessibility, and timely acquisition. Some research has already utilized point clouds for environmental modeling, with widespread applications in the construction industry. However, in robotics, path planning methods integrating point clouds have primarily focused on unmanned ground vehicles (UGVs). These methods are limited to two-dimensional planar motion and are therefore unsuitable for path obstacle avoidance planning in articulated robotic arms. Summary of the Invention

[0009] The primary objective of this invention is to overcome the shortcomings of the existing technology and provide a robot grasping and obstacle avoidance path planning method based on point cloud environment modeling. This robot grasping and obstacle avoidance path planning method based on point cloud environment modeling improves the optimization convergence speed and the accuracy and robustness of collision detection.

[0010] A second objective of this invention is to provide a storage medium.

[0011] A third objective of this invention is to provide a computer device.

[0012] The first objective of this invention is achieved through the following technical solution: a robot grasping and obstacle avoidance path planning method based on point cloud environment modeling, comprising the following steps:

[0013] S1. Use a 3D vision device to acquire the point cloud of the grasping scene, and use the robot control system to obtain the current robot pose.

[0014] S2. An environment modeling module is constructed based on point cloud conditional filtering and octree technology. After the captured point cloud and the current robot pose are processed by the environment modeling module, the space occupancy information is output.

[0015] S3. Construct the obstacle avoidance path planning module PQ-RRT*, and optimize the obstacle avoidance path planning module PQ-RRT* to obtain the sampling planning algorithm module DP-PQ-RRT*.

[0016] S4. Based on the space occupancy information output by the environment modeling module, the optimized sampling planning algorithm module DP-PQ-RRT* is deployed to the robotic arm grasping platform to perform grasping and obstacle avoidance work.

[0017] Preferably, step S2 includes the following specific steps:

[0018] S21. After the original grasping scene point cloud and initial robot pose input environment modeling module are completed, the point cloud conditional filtering method uses the initial robot pose and camera extrinsic parameters to calculate the transformation matrix of each joint coordinate system of the robot relative to the camera coordinate system:

[0019]

[0020] in, This represents the transformation matrix between the world coordinate system and the camera coordinate system. Represents the transformation matrix of each joint relative to the world coordinate system, k∈{1,2,3,4,5,6};

[0021] S22. The joint coordinate system is translated along the connecting rod to obtain the clipping box coordinate system:

[0022]

[0023] Among them, Trans any ∈SE(3) represents a translation along a certain coordinate axis of joint k;

[0024] S23. Set the size of the cutting box according to the actual size of the robot, and finally filter out the robot body point cloud in the cutting box to obtain a clean environmental point cloud.

[0025] S24. The octree technique divides the 3D space into several voxels and updates the voxel states according to the probability update model based on the input clean environmental point cloud data:

[0026]

[0027] Wherein, P(n|z 1：t ) represents the probability that the smallest voxel is in the "occupied" state, n represents the "occupied" state, and z 1：t-1 ,z 1：t ,z t These represent the point clouds input from time 1 to time t-1, time 1 to time t, and time t, respectively.

[0028] S25. After the voxel state stabilizes, each discrete point in the environmental point cloud becomes a cubic region centered on the coordinates of that point and with the smallest voxel size as the side length. All the cubic regions together form complete spatial occupancy information.

[0029] Preferably, step S3 includes the following specific steps:

[0030] S31. Construct the obstacle avoidance path planning module PQ-RRT* using PQ-RRT* as the main body;

[0031] S32. The pose isolation planning strategy is adopted to transform the high-dimensional planning process of the obstacle avoidance path planning module PQ-RRT*, which plans the position and pose simultaneously, into two low-dimensional planning processes: position first and pose second.

[0032] S33. Replace the original Euclidean distance-based collision detection method in the obstacle avoidance path planning module PQ-RRT* with a collision detection method based on the GJK algorithm to obtain the sampling planning algorithm module DP-PQ-RRT*.

[0033] Preferably, S32 includes the following steps:

[0034] S321. In the position planning phase, the fixed attitude part is the same as the target, and only the position part is planned. When the Euclidean distance between the position part and the target is less than one sampling step, the position planning stops and then switches to the attitude planning phase.

[0035] S322. In the attitude planning phase, the position part is fixed, and only the attitude part is planned. When the Euclidean distance between the attitude part and the target is less than one sampling step, the attitude planning stops, and then the position planning phase of the next planning cycle begins.

[0036] Preferably, the collision detection method based on the GJK algorithm in step S33 includes the following steps:

[0037] S331. First, define an initial direction vector unit_vector in three-dimensional space, and discretize the two geometric bodies into compact sets K1 and K2 in space;

[0038] S332. Using the Support function, calculate the two farthest points of K1 and K2 on this direction vector. Subtract the coordinates of the two points to obtain the Minkowski difference K of K1 and K2. The Support point P on the boundary is then calculated. p1 ;

[0039] S333, Determine P p1 Is it the origin?

[0040] If P p1 If this is the origin, then the capsule collides with the cube;

[0041] If P p1 If it is not the origin, proceed with the next steps;

[0042] S334, Using the origin and P p1 Vectors between As the new direction vector unit_vector, calculate another Support point P on the K boundary according to S32. p2 ;

[0043] S335. Judgment Is the angle with unit_vector obtuse?

[0044] If the angle is obtuse, it indicates that the capsule collided with the cube;

[0045] If it is not an obtuse angle, proceed to the next step;

[0046] S336, Determining Spatial Line Segments Did it pass through the origin?

[0047] If it passes through the origin, it means that the capsule collided with the cube;

[0048] If the origin is not crossed, proceed with the next steps;

[0049] S337, Using the origin and P p2 Vectors between As the new direction vector unit_vector, calculate another Support point P on the K boundary according to S32. p3 ;

[0050] S338. Determine if a spatial triangle P is in its correct position. p1 P p2 P p3 Does it include the origin?

[0051] If the origin is included, it indicates that the capsule collided with the cube;

[0052] If the origin is not included, proceed with the next steps;

[0053] S339, Using the origin and P p3 Vectors between As the new direction vector unit_vector, calculate another Support point P on the K boundary according to S32. p4 ;

[0054] S3310, Determine the spatial tetrahedron P p1 P p2 P p3 P p4 Does it include the origin?

[0055] If the origin is included, it indicates that the capsule collided with the cube;

[0056] If it does not include the origin, then clear P. p1 Then repeat steps S334-S339 to reconstruct a new spatial tetrahedron for judgment until a spatial tetrahedron containing the origin is found.

[0057] The storage medium is characterized in that it stores a program, which, when executed by a processor, achieves the robot grasping and obstacle avoidance path planning method based on point cloud environment modeling as described in the first objective.

[0058] A computer device, including a processor and a memory for storing processor-executable programs, wherein when the processor executes the program stored in the memory, it achieves the robot grasping and obstacle avoidance path planning method based on point cloud environment modeling as described in the first objective.

[0059] The present invention has the following advantages over the prior art:

[0060] 1. The method of the present invention uses point cloud conditional filtering and octree technology to construct an environment modeling module to process the data collected by the 3D vision device and the current robot pose data, thereby obtaining accurate spatial information. This solves the problems of insufficient modeling accuracy and low update efficiency caused by the environmental modeling method that relies heavily on measurement in the existing path planning method, and provides a reliable basis for subsequent obstacle avoidance path planning.

[0061] 2. The method of this invention adopts the planning algorithm module DP-PQ-RRT*. This algorithm integrates the pose isolation planning strategy with the idea of ​​dimensionality reduction and a collision detection method based on the GJK algorithm, thereby improving the optimization convergence speed of path planning in joint space and the accuracy and robustness of collision detection. Therefore, it improves the accuracy of collision detection between the robot arm and the environment, and generates a collision-free obstacle avoidance path for robot grasping.

[0062] 3. The method of this invention deploys the improved sampling planning algorithm DP-PQ-RRT* into the robotic arm control system. While accurately modeling the robot's surrounding environment, it plans a collision-free robot grasping and obstacle avoidance trajectory, providing a solution to the problem of environmental modeling and obstacle avoidance for home service robots. Attached Figure Description

[0063] Figure 1 This is a flowchart of the robot grasping and obstacle avoidance path planning method based on point cloud environment modeling according to the present invention.

[0064] Figure 2 This is a block diagram of the robot grasping and obstacle avoidance path planning method based on point cloud environment modeling of the present invention.

[0065] Figure 3 This is a schematic diagram of the data structure of the octree technology of the present invention.

[0066] Figure 4 This is a schematic diagram of the voxel state update using the octree technique of the present invention;

[0067] Figure 5 This is a schematic diagram illustrating the principle of the pose isolation planning strategy of the present invention.

[0068] Figure 6 This is a schematic diagram of obstacle arrangement in three simulated life scenarios using the method of the present invention.

[0069] Figure 7 This is a schematic diagram of the method of the present invention deployed on a robotic arm gripping platform.

[0070] Figure 8 This is a schematic diagram of the environmental modeling process for three simulated life scenarios of the present invention.

[0071] Figure 9 These are schematic diagrams illustrating the robotic arm's grasping and obstacle avoidance processes in three simulated real-life scenarios according to the present invention. Detailed Implementation

[0072] The present invention will be further described below with reference to the accompanying drawings and embodiments.

[0073] like Figure 1 and Figure 2 As shown, the robot grasping and obstacle avoidance path planning method based on point cloud environment modeling includes the following steps:

[0074] S1. Use a 3D vision device to acquire the point cloud of the grasping scene, and use the robot control system to obtain the current robot pose.

[0075] S2. An environment modeling module is constructed based on point cloud conditional filtering and octree technology. After the captured point cloud and the current robot pose are processed by the environment modeling module, the spatial occupancy information is output.

[0076] Step S2 includes the following specific steps:

[0077] S21. After the original grasping scene point cloud and initial robot pose input environment modeling module are completed, the point cloud conditional filtering method uses the initial robot pose and camera extrinsic parameters to calculate the transformation matrix of each joint coordinate system of the robot relative to the camera coordinate system:

[0078]

[0079] in, The transformation matrix representing the world coordinate system relative to the camera coordinate system can be obtained from the camera extrinsic parameter calibration. Represents the transformation matrix of each joint relative to the world coordinate system, k∈{1,2,3,4,5,6};

[0080] S22. The joint coordinate system is translated along the connecting rod to obtain the clipping box coordinate system:

[0081]

[0082] Among them, Trans any ∈SE(3) represents a translation along a certain coordinate axis of joint k;

[0083] S23. Set the size of the cutting box according to the actual size of the robot, and finally filter out the robot body point cloud in the cutting box to obtain a clean environmental point cloud.

[0084] S24, such as Figure 3 and Figure 4 As shown, the octree technique divides the three-dimensional space into several voxels, and updates the voxel states according to the probability update model based on the input clean environmental point cloud data:

[0085]

[0086] Wherein, P(n|z 1：t ) represents the probability that the smallest voxel is in the "occupied" state, n represents the "occupied" state, and z 1：t-1 , z 1：t , z t These represent the point clouds input from time 1 to time t-1, time 1 to time t, and time t, respectively.

[0087] S25. After the voxel state stabilizes, each discrete point in the environmental point cloud becomes a cubic region centered on the coordinates of that point and with the smallest voxel size as the side length. All the cubic regions together form complete spatial occupancy information.

[0088] S3. Construct the obstacle avoidance path planning module PQ-RRT*, and optimize the obstacle avoidance path planning module PQ-RRT* to obtain the sampling planning algorithm module DP-PQ-RRT*.

[0089] Step S3 includes the following specific steps:

[0090] S31. Construct the obstacle avoidance path planning module PQ-RRT* using PQ-RRT* as the main body;

[0091] S32. The pose isolation planning strategy is adopted to transform the high-dimensional planning process of the obstacle avoidance path planning module PQ-RRT*, which plans the position and pose simultaneously, into two low-dimensional planning processes: position first and pose second.

[0092] like Figure 5 As shown, S32 includes the following steps:

[0093] S321. In the position planning phase, the fixed attitude part is the same as the target, and only the position part is planned. When the Euclidean distance between the position part and the target is less than one sampling step, the position planning stops and then switches to the attitude planning phase.

[0094] S322. In the attitude planning phase, the position part is fixed, and only the attitude part is planned. When the Euclidean distance between the attitude part and the target is less than one sampling step, the attitude planning stops, and then the position planning phase of the next planning cycle begins.

[0095] S33. Replace the original Euclidean distance-based collision detection method in the obstacle avoidance path planning module PQ-RRT* with a collision detection method based on the GJK algorithm to obtain the sampling planning algorithm module DP-PQ-RRT*.

[0096] Specifically, the collision detection method based on the GJK algorithm in this embodiment uses a capsule-shaped approximation of the robotic arm's collision box. The geometric coordinate system is located at its geometric center, and its transformation matrix relative to the world coordinate system is obtained by performing matrix multiplication operations sequentially on the joint coordinate system transformation matrix, a translation transformation matrix, and a rotation transformation matrix.

[0097]

[0098] in, Represents the capsule volume coordinate system, Rot any ∈SE(3) rotate along a certain coordinate axis of joint k.

[0099] The collision detection method based on the GJK algorithm in this embodiment essentially considers the collision relationship between a single capsule and a single three-dimensional object. That is, the working process of the collision detection method based on the GJK algorithm includes the following steps:

[0100] S331. First, define an initial direction vector unit_vector in three-dimensional space, and discretize the two geometric bodies into compact sets K1 and K2 in space;

[0101] S332. Based on the Support function, calculate the two farthest points of K1 and K2 on this direction vector. Subtract the coordinates of the two points to obtain the Minkowski difference K of K1 and K2. The Support point P on the boundary is then calculated. p1 ;

[0102] S333, Determine P p1 Is it the origin?

[0103] If P p1 If this is the origin, then the capsule collides with the cube;

[0104] If P p1 If it is not the origin, proceed with the next steps;

[0105] S334, Using the origin and P p1 Vectors between As the new direction vector unit_vector, calculate another Support point P on the K boundary according to S32. p2 ;

[0106] S335. Judgment Is the angle with unit_vector obtuse?

[0107] If the angle is obtuse, it indicates that the capsule collided with the cube;

[0108] If it is not an obtuse angle, proceed to the next step;

[0109] S336, Determining Spatial Line Segments Did it pass through the origin?

[0110] If it passes through the origin, it means that the capsule collided with the cube;

[0111] If the origin is not crossed, proceed with the next steps;

[0112] S337, Using the origin and P p2 Vectors between As the new direction vector unit_vector, calculate another Support point P on the K boundary according to S32. p3 ;

[0113] S338. Determine if a spatial triangle P is in its correct position. p1 P p2 P p3 Does it include the origin?

[0114] If the origin is included, it indicates that the capsule collided with the cube;

[0115] If the origin is not included, proceed with the next steps;

[0116] S339, Using the origin and P p3 Vectors between As the new direction vector unit_vector, calculate another Support point P on the K boundary according to S32. p4 ;

[0117] S3310, Determine the spatial tetrahedron P p1 P p2 P p3 P p4 Does it include the origin?

[0118] If the origin is included, it indicates that the capsule collided with the cube;

[0119] If it does not include the origin, then clear P. p1 Then repeat steps S334-S339 to reconstruct a new spatial tetrahedron for judgment until a spatial tetrahedron containing the origin is found.

[0120] S4. Based on the space occupancy information output by the environment modeling module, the optimized sampling planning algorithm module DP-PQ-RRT* is deployed to the robotic arm grasping platform to perform grasping and obstacle avoidance work.

[0121] The following is a comparative experiment using the robot grasping and obstacle avoidance path planning method based on point cloud environment modeling described above:

[0122] A. Six simulation environments were designed for the experiment. The obstacles in the simulation environments included both cubes and spheres. Based on the number of obstacles, the simulation environments were divided into three levels of complexity: easy, medium, and difficult.

[0123] B. In all simulation environments, the robotic arm is programmed with the same initial joint angle and target joint angle. The initial end effector position and the target end effector position are (-0.109, -0.400, 0.057) and (0.442, 0.081, 0.500), respectively, in meters.

[0124] C. The experiment used RRT, RRT*, Informed-RRT*, Q-RRT*, and PQ-RRT* as comparison objects. All algorithms were repeatedly planned 50 times in each simulation environment. The simulations were performed on an Intel Core processor. TM The simulation was performed on an industrial control computer with an i7-12700F CPU and 32GB of RAM, using Python as the simulation platform.

[0125] D. The following evaluation indicators were set for the experiment: (1) Initial solution output time T init : The iteration cycle taken by the algorithm to output a feasible solution for the first time reflects the efficiency of the algorithm in finding a feasible path for the first time. (2) Initial solution quality c init The cost of the feasible solution output by the algorithm for the first time reflects the quality of the feasible path found by the algorithm in the first search. (3) Optimal path quality c best The cost of the optimal solution output by the algorithm reflects the algorithm's optimization ability. (4) Number of nodes in the optimal path. best The number of nodes in the optimal path output by the algorithm reflects the control cost incurred by the robot in tracking the optimal path. (5) Approximate optimal solution output time T 5％ The algorithm initially converged to 1.05*c. best The number of iterations taken reflects the convergence ability of the algorithm. If the initial solution is the optimal solution, then T... 5％ =T init (6) Planning success rate SR: The percentage of successful planning attempts for each algorithm in 50 planning experiments.

[0126] The data obtained from the above experiment are as follows:

[0127] Table 1. Statistical analysis of experimental results for simulation scenario 1

[0128]

[0129]

[0130] Table 2. Statistical analysis of experimental results for simulation scenario 2

[0131]

[0132] Table 3. Statistical analysis of experimental results for simulation scenario 3

[0133]

[0134] Table 4. Statistical analysis of experimental results for simulation scenario 4

[0135]

[0136] Table 5. Statistical analysis of experimental results for simulation scenario 5.

[0137]

[0138]

[0139] Table 6. Statistical analysis of experimental results for simulation scenario 6

[0140]

[0141] As shown in the table above, in all experimental scenarios, the method of this embodiment outperforms the basic methods using RRT, RRT*, Informed-RRT*, Q-RRT*, and PQ-RRT*. The time cost reduction rate of the method of this embodiment in the six environments is 67.6%, 31.1%, 56.2%, 37.9%, 52.2%, and 42.0%, respectively. Therefore, this method can effectively solve the problem of low efficiency in high-dimensional planning in joint space and improve planning efficiency. Although in c init , cbest While DP-PQ-RRT* is slightly inferior to PQ-RRT* and Q-RRT*, in grasping and obstacle avoidance tasks, the robotic arm needs to respond to commands quickly and focuses on real-time performance and path safety. Therefore, at the overall level, this method is valuable in trading the optimal path for efficient planning and the suboptimal but safer path.

[0142] Then, the method was applied to different life scenarios to conduct crawling experiments. The results are as follows:

[0143] like Figure 6 and Figure 7 As shown, this method is applied to simulate obstacle avoidance scenarios using common objects from everyday life. In this example, common everyday objects (obstacle 1) and desktop ornaments (obstacle 2) are selected to build an unstructured obstacle avoidance scenario. A rope is used to fix obstacle 1, simulating an object suspended in the air in a living environment, and a box is used to fix obstacle 2, simulating an object placed on a flat surface in a living environment. Therefore, this embodiment sets up three obstacle avoidance scenarios in the grasping experiment.

[0144] like Figure 8 and Figure 9 As shown, the experimental process of the grasping experiment is as follows: The robotic arm starts from the initial joint angle, runs the corresponding trajectory according to the obstacle avoidance requirements of the scene, and then reaches the predetermined grasping pose. The robotic arm then performs the grasping, returns to the starting point according to the obstacle avoidance trajectory, and places the target object in the storage box. In the experiment, the initial pose of the robotic arm and the target grasping pose are the same for all three scenarios. The target grasping pose is pre-calculated by the end-to-end grasping network AnyGrasp.

[0145] The results of the capture experiments in different simulated life scenarios are as follows:

[0146] Despite the presence of irregular objects in the environment, this method uses environmental point clouds as input, accurately describing the space occupied by obstacles without direct measurement. When the environment changes, only the point cloud needs to be updated, rather than tedious re-measurement, to reconstruct the environmental model, thus improving the efficiency of the grasping task. Notably, the robotic arm does not choose paths where the end effector can safely connect but the robotic arm itself would collide during the grasping obstacle avoidance process. This is thanks to the proposed robotic arm collision box modeling and collision detection method. This method ensures that there are no obstacles obstructing the nodes of feasible paths between the robotic arm end effectors, and further enhances the safety of the grasping trajectory by adding a collision detection mechanism.

[0147] The storage medium is characterized in that it stores a program, which, when executed by a processor, achieves the robot grasping and obstacle avoidance path planning method based on point cloud environment modeling as described in the first objective.

[0148] A computer device, including a processor and a memory for storing processor-executable programs, wherein when the processor executes the program stored in the memory, it achieves the robot grasping and obstacle avoidance path planning method based on point cloud environment modeling as described in the first objective.

[0149] The above-described specific embodiments are preferred embodiments of the present invention and are not intended to limit the present invention. Any other changes or equivalent substitutions made without departing from the technical solution of the present invention are included within the protection scope of the present invention.

Claims

1. A robot grasping and obstacle avoidance path planning method based on point cloud environment modeling, characterized in that, Includes the following steps: S1. Use a 3D vision device to acquire the point cloud of the grasping scene, and use the robot control system to obtain the current robot pose. S2. An environment modeling module is constructed based on point cloud conditional filtering and octree technology. After the captured point cloud and the current robot pose are processed by the environment modeling module, the space occupancy information is output. S3. Construct the obstacle avoidance path planning module PQ-RRT*, and optimize the obstacle avoidance path planning module PQ-RRT* to obtain the sampling planning algorithm module DP-PQ-RRT*. S4. Based on the space occupancy information output by the environment modeling module, the optimized sampling planning algorithm module DP-PQ-RRT* is deployed to the robotic arm grasping platform to perform grasping and obstacle avoidance work.

2. The robot grasping and obstacle avoidance path planning method based on point cloud environment modeling according to claim 1, characterized in that, Step S2 includes the following specific steps: S21. After the original grasping scene point cloud and initial robot pose input environment modeling module are completed, the point cloud conditional filtering method uses the initial robot pose and camera extrinsic parameters to calculate the transformation matrix of each joint coordinate system of the robot relative to the camera coordinate system: in, This represents the transformation matrix between the world coordinate system and the camera coordinate system. Represents the transformation matrix of each joint relative to the world coordinate system, k∈{1,2,3,4,5,6}; S22. The joint coordinate system is translated along the connecting rod to obtain the clipping box coordinate system: Among them, Trans any ∈SE(3) represents a translation along a certain coordinate axis of joint k; S23. Set the size of the cutting box according to the actual size of the robot, and finally filter out the robot body point cloud in the cutting box to obtain a clean environmental point cloud. S24. The octree technique divides the 3D space into several voxels and updates the voxel states according to the probability update model based on the input clean environmental point cloud data: Wherein, P(n|z 1：t ) represents the probability that the smallest voxel is in the "occupied" state, n represents the "occupied" state, and z 1:t-1 , z 1:t , z t These represent the point clouds input from time 1 to time t-1, time 1 to time t, and time t, respectively. S25. After the voxel state stabilizes, each discrete point in the environmental point cloud becomes a cubic region centered on the coordinates of that point and with the smallest voxel size as the side length. All the cubic regions together form complete spatial occupancy information.

3. The robot grasping and obstacle avoidance path planning method based on point cloud environment modeling according to claim 1, characterized in that, Step S3 includes the following specific steps: S31. Construct the obstacle avoidance path planning module PQ-RRT* using PQ-RRT* as the main body; S32. The pose isolation planning strategy is adopted to transform the high-dimensional planning process of the obstacle avoidance path planning module PQ-RRT*, which plans the position and pose simultaneously, into two low-dimensional planning processes: position first and pose second. S33. Replace the original Euclidean distance-based collision detection method in the obstacle avoidance path planning module PQ-RRT* with a collision detection method based on the GJK algorithm to obtain the sampling planning algorithm module DP-PQ-RRT*.

4. The robot grasping and obstacle avoidance path planning method based on point cloud environment modeling according to claim 3, characterized in that, S32 includes the following steps: S321. In the position planning phase, the fixed attitude part is the same as the target, and only the position part is planned. When the Euclidean distance between the position part and the target is less than one sampling step, the position planning stops and then switches to the attitude planning phase. S322. In the attitude planning phase, the position part is fixed, and only the attitude part is planned. When the Euclidean distance between the attitude part and the target is less than one sampling step, the attitude planning stops, and then the position planning phase of the next planning cycle begins.

5. The robot grasping and obstacle avoidance path planning method based on point cloud environment modeling according to claim 3, characterized in that, The collision detection method based on the GJK algorithm in step S33 includes the following steps: S331. First, define an initial direction vector uni_vector in three-dimensional space, and discretize the two geometric bodies into compact sets K1 and K2 in space; S332. Based on the Support function, calculate the two farthest points of K1 and K2 on this direction vector. Subtract the coordinates of the two points to obtain the Minkowski difference K of K1 and K2. The Support point P on the boundary is then calculated. p1 ; S333, Determine P p1 Is it the origin? If P p1 If this is the origin, then the capsule collides with the cube; If P p1 If it is not the origin, proceed with the next steps; S334, Using the origin and P p1 Vectors between As the new direction vector unit_vector, calculate another Support point P on the K boundary according to S32. p2 ; S335. Judgment Is the angle with unit_vector obtuse? If the angle is obtuse, it indicates that the capsule collided with the cube; If it is not an obtuse angle, proceed to the next step; S336, Determining Spatial Line Segments Did it pass through the origin? If it passes through the origin, it means that the capsule collided with the cube; If the origin is not crossed, proceed with the next steps; S337, Using the origin and P p2 Vectors between As the new direction vector unit_vector, calculate another Support point P on the K boundary according to S32. p3 ; S338. Determine if a spatial triangle P is in its correct position. p1 P p2 P p3 Does it include the origin? If the origin is included, it indicates that the capsule collided with the cube; If the origin is not included, proceed with the next steps; S339, Using the origin and P p3 Vectors between As the new direction vector unit_vector, calculate another Support point P on the K boundary according to S32. p4 ; S3310, Determine the spatial tetrahedron P p1 P p2 P p3 P p4 Does it include the origin? If the origin is included, it indicates that the capsule collided with the cube; If it does not include the origin, then clear P. p1 Then repeat steps S334-S339 to reconstruct a new spatial tetrahedron for judgment until a spatial tetrahedron containing the origin is found.

6. A storage medium, characterized in that, The system stores a program that, when executed by a processor, implements the robot grasping and obstacle avoidance path planning method based on point cloud environment modeling as described in any one of claims 1-5.

7. A computer device, characterized in that, It includes a processor and a memory for storing processor-executable programs. When the processor executes the program stored in the memory, it implements the robot grasping and obstacle avoidance path planning method based on point cloud environment modeling as described in any one of claims 1-5.

Images