A robot arm obstacle avoidance path planning method, system and medium

Abstract

The embodiment of the present application provides a method, system and medium for planning a robot arm obstacle avoidance path, the method comprising: constructing a static scene model and a dynamic model based on a grid point cloud, and reconstructing an obstacle avoidance scene with an additional safety distance; obtaining a reachable obstacle avoidance path explored by the robot arm in collaboration, finding obstacle avoidance path points, and controlling the robot arm to move to the nearest collision-free point according to the minimum obstacle avoidance direction; judging whether the current posture of the robot arm is collision-free and whether there is collision-free with the linear interpolation motion between adjacent points, if there is no collision, keeping the current posture unchanged, if there is a collision, controlling the robot arm to move a step unit length according to the minimum obstacle avoidance direction; optimizing the reachable obstacle avoidance path based on the path point state by using a wire pulling strategy to obtain the optimized target displacement point, reconstructing the scene based on the grid point cloud, supporting collision detection with adjustable accuracy, obtaining the reachable obstacle avoidance path explored by the robot arm in collaboration, reducing the number of invalid explorations, and optimizing the reachable path by using a wire pulling strategy to improve the convergence speed of the path optimization.

Description

Technical Field

[0001] The present application relates to the field of path planning technology, and more specifically, to a method, system and medium for robot arm obstacle avoidance path planning. Background Art

[0002] Recently, artificial intelligence technology has shown good application capabilities and great potential in many fields, and more and more artificial intelligence researchers have joined various fields of social production. In recent years, with the vigorous promotion of industrial automation, more and more artificial intelligence researchers have gradually participated in the design and application of artificial intelligence in the industrial field. Recently, there are many participants in the field of obstacle avoidance path planning of robotic arms, especially in the industrial field. There are two important modules in the obstacle avoidance path planning of robotic arms: one is the path planning algorithm; the other is the collision detection algorithm for obstacle avoidance. In the field of obstacle avoidance path planning of industrial robotic arms, a lot of scene data is point cloud data. In the scene represented by point cloud, the computing power requirements of the collision detection algorithm increase quadratically with the improvement of collision detection accuracy and the increase of space. Therefore, application scenarios with rate requirements bring great challenges to researchers.

[0003] Currently, the more common AI algorithms for robot arm obstacle avoidance path planning are mainly based on random sampling RRT and its improved algorithms. However, these algorithms all have algorithm pressure problems caused by abnormally low planning efficiency in narrow spaces. Effective technical solutions are urgently needed to address the above problems. Summary of the invention

[0004] The purpose of the embodiments of the present application is to provide a method, system and medium for robot arm obstacle avoidance path planning, which optimizes the achievable obstacle avoidance path by adopting a wire-pulling strategy based on the state of the path points, obtains the optimized target displacement points, and generates the robot arm optimized path; reconstructs the scene based on the grid point cloud, supports collision detection with adjustable accuracy, and supports safe distance obstacle avoidance planning.

[0005] The present application also provides a method for robot arm obstacle avoidance path planning, including:

[0006] Build static scene models and dynamic models based on raster point clouds, and reconstruct obstacle avoidance scenarios with additional safety distances;

[0007] Obtain the obstacle avoidance path that can be reached by the robot arm's collaborative exploration, find the obstacle avoidance path points, calculate the obstacle avoidance exploration direction, and control the robot arm to move directly to the nearest collision-free point recorded according to the minimum obstacle avoidance direction;

[0008] Get the current position of the robot arm, and determine whether the current position of the robot arm is collision-free and whether there is no collision with the linear interpolation motion between adjacent points. If there is no collision, keep the current position unchanged and record the path point status;

[0009] If there is a collision, the robot arm is controlled to move one step unit length in the minimum obstacle avoidance direction;

[0010] Based on the state of the path points, the wire pulling strategy is used to optimize the accessible obstacle avoidance path, obtain the optimized target displacement point, and generate the optimized path of the robotic arm.

[0011] Optionally, in the robot arm obstacle avoidance path planning method described in the embodiment of the present application, a static scene model and a dynamic model are constructed based on a grid point cloud to reconstruct an obstacle avoidance scene with an additional safety distance, specifically including:

[0012] Obtain model file data with posture transformation; rasterize the model physical data using voxelization method and convert the model into a point cloud model;

[0013] Combine the corresponding kinematic data to build a point cloud algorithm-driven dynamic model;

[0014] Obtain static model file data, rasterize the model physical data using voxelization method, convert the model into a point cloud model, expand the model point cloud according to the safety distance and model point cloud accuracy, and obtain a point cloud that meets the safety distance requirements;

[0015] Arrange the dynamic models according to their respective initial positions; arrange the static models according to their initial positions and integrate them into a point cloud model;

[0016] Remove duplicate points based on the point cloud model, generate an integrated model, and build an obstacle avoidance scenario based on the integrated model.

[0017] Optionally, in the robot arm obstacle avoidance path planning method described in the embodiment of the present application, the expanding the model point cloud according to the safety distance and the model point cloud accuracy to obtain a point cloud that meets the safety distance requirement specifically includes:

[0018] Based on the point cloud accuracy requirement of the model, the point cloud model represented by non-integer numbers is converted into a point cloud model represented by integer numbers;

[0019] Based on the safety distance and model point cloud accuracy requirements, calculate the number of layers that the point cloud model needs to expand;

[0020] Taking the current outermost point cloud as the core, generate adjacent points in six directions (front, back, left, right, up, and down) for each single point in the point cloud to generate a new point cloud;

[0021] Remove duplicate points in the new point cloud and the reference point cloud points as the new outermost point cloud, and iteratively generate multiple expanded point cloud layers according to the specified number of layers;

[0022] Integrate the expanded point cloud, take the outermost point cloud as the core, remove the points that are repeated with the inner point cloud, and generate a point cloud that meets the safety distance requirements.

[0023] Optionally, in the robot arm obstacle avoidance path planning method described in the embodiment of the present application, obtaining the robot arm collaborative exploration reachable obstacle avoidance path, finding the obstacle avoidance path point, and calculating the obstacle avoidance exploration direction specifically include:

[0024] Connect the line from the starting point to the end point of the task, perform straight line pose interpolation, and use the interpolation point as the path point; calculate the motion collision detection status between all path points and adjacent path points;

[0025] Eliminate completely collision-free path points based on the status of each path point;

[0026] Connect the task starting point to the end point to construct the task unit vector; take the task starting point as the center point and translate upward by one unit to construct the upward unit vector;

[0027] Based on the upward unit vector, calculate the vertical vector perpendicular to the task vector; the vertical vector is rotated around the axis at a specified angle to construct an obstacle avoidance exploration direction group.

[0028] Optionally, in the robot arm obstacle avoidance path planning method described in the embodiment of the present application, the robot arm is controlled to move directly to the nearest recorded collision-free point according to the minimum obstacle avoidance direction, specifically including:

[0029] Calculate the maximum obstacle avoidance distance of all path points in each obstacle avoidance direction;

[0030] Select the smallest obstacle avoidance direction among the maximum obstacle avoidance distances of the path points in all directions as the minimum obstacle avoidance direction, and record the minimum obstacle avoidance distance in the minimum obstacle avoidance direction;

[0031] Move directly to the nearest collision-free point recorded in the direction of minimum obstacle avoidance.

[0032] Optionally, in the robot arm obstacle avoidance path planning method described in the embodiment of the present application, a wire pulling strategy is adopted based on the path point state to optimize the reachable obstacle avoidance path, obtain the optimized target displacement point, and generate the robot arm optimized path, which specifically includes:

[0033] Calculating the optimized target displacement point of the path point, the optimizing target displacement point of the path point includes: designing a line-pulling optimization strategy based on the idea that the sum of two sides of a triangle is greater than the third side, pulling the middle point to the straight line between two adjacent points, and obtaining the first target point - the midpoint between adjacent path points; setting a half midpoint, a front adjacent midpoint, and a rear adjacent midpoint;

[0034] Use odd-numbered and even-numbered path points for alternate optimization;

[0035] The alternating optimization includes: detecting the current target point;

[0036] Determine whether the current target point has no collision and whether the linear interpolation motion with adjacent points has no collision, and record the status;

[0037] If the target point is in a collision-free state, the path point moves to the target point;

[0038] If the current target point is the midpoint between adjacent path points, remove the current path point and connect the two adjacent path points;

[0039] If it is not the midpoint between adjacent path points, the optimization of the current path point ends;

[0040] If the target point is not in a collision-free state, the path point will not move and will switch to the next target point until all target points are trained in rotation and the current path point optimization ends;

[0041] The optimization operation is performed once for all path points, which is called a round. If all path points are not displaced and the number of path points is unchanged for three consecutive rounds, the path optimization stage is ended, and the current path point data is recorded to obtain the optimized obstacle avoidance path.

[0042] In a second aspect, an embodiment of the present application provides a robot arm obstacle avoidance path planning system, the system comprising: a memory and a processor, the memory comprising a program of a robot arm obstacle avoidance path planning method, and the program of the robot arm obstacle avoidance path planning method is executed by the processor to implement the following steps:

[0043] Build static scene models and dynamic models based on raster point clouds, and reconstruct obstacle avoidance scenarios with additional safety distances;

[0044] Obtain the obstacle avoidance path that can be reached by the robot arm's collaborative exploration, find the obstacle avoidance path points, calculate the obstacle avoidance exploration direction, and control the robot arm to move directly to the nearest collision-free point recorded according to the minimum obstacle avoidance direction;

[0045] Get the current position of the robot arm, and determine whether the current position of the robot arm is collision-free and whether there is no collision with the linear interpolation motion between adjacent points. If there is no collision, keep the current position unchanged and record the path point status;

[0046] If there is a collision, the robot arm is controlled to move one step unit length in the minimum obstacle avoidance direction;

[0047] Based on the state of the path points, the wire pulling strategy is used to optimize the accessible obstacle avoidance path, obtain the optimized target displacement point, and generate the optimized path of the robotic arm.

[0048] Optionally, in the robot arm obstacle avoidance path planning system described in the embodiment of the present application, a static scene model and a dynamic model are constructed based on the grid point cloud to reconstruct an obstacle avoidance scene with an additional safety distance, specifically including:

[0049] Obtain model file data with posture transformation; rasterize the model physical data using voxelization method and convert the model into a point cloud model;

[0050] Combine the corresponding kinematic data to build a point cloud algorithm-driven dynamic model;

[0051] Obtain static model file data, rasterize the model physical data using voxelization method, convert the model into a point cloud model, expand the model point cloud according to the safety distance and model point cloud accuracy, and obtain a point cloud that meets the safety distance requirements;

[0052] Arrange the dynamic models according to their respective initial positions; arrange the static models according to their initial positions and integrate them into a point cloud model;

[0053] Remove duplicate points based on the point cloud model, generate an integrated model, and build an obstacle avoidance scenario based on the integrated model.

[0054] Optionally, in the robot arm obstacle avoidance path planning system described in the embodiment of the present application, the expanding the model point cloud according to the safety distance and the model point cloud accuracy to obtain a point cloud that meets the safety distance requirement specifically includes:

[0055] Based on the point cloud accuracy requirement of the model, the point cloud model represented by non-integer numbers is converted into a point cloud model represented by integer numbers;

[0056] Based on the safety distance and model point cloud accuracy requirements, calculate the number of layers that the point cloud model needs to expand;

[0057] Taking the current outermost point cloud as the core, generate adjacent points in six directions (front, back, left, right, up, and down) for each single point in the point cloud to generate a new point cloud;

[0058] Remove duplicate points in the new point cloud and the reference point cloud points as the new outermost point cloud, and iteratively generate multiple expanded point cloud layers according to the specified number of layers;

[0059] Integrate the expanded point cloud, take the outermost point cloud as the core, remove the points that are repeated with the inner point cloud, and generate a point cloud that meets the safety distance requirements.

[0060] In a third aspect, an embodiment of the present application further provides a computer-readable storage medium, which includes a robotic arm obstacle avoidance path planning method program. When the robotic arm obstacle avoidance path planning method program is executed by a processor, the steps of the robotic arm obstacle avoidance path planning method as described in any one of the above items are implemented.

[0061] As can be seen from the above, a method, system and medium for obstacle avoidance path planning of a robotic arm provided in an embodiment of the present application reconstruct an obstacle avoidance scene with an additional safety distance by constructing a static scene model and a dynamic model based on a grid point cloud; obtaining a reachable obstacle avoidance path for collaborative exploration of the robotic arm, finding obstacle avoidance path points, calculating the obstacle avoidance exploration direction, and controlling the robotic arm to move directly to the nearest recorded collision-free point according to the minimum obstacle avoidance direction; obtaining the current posture of the robotic arm, judging whether the current posture of the robotic arm is collision-free and whether there is collision-free with the linear interpolation motion between adjacent points, if there is no collision, keeping the current posture unchanged, and recording the path point status; if there is a collision, controlling the robotic arm to move a step unit length according to the minimum obstacle avoidance direction; optimizing the reachable obstacle avoidance path based on the path point status by adopting a wire pulling strategy, obtaining the optimized target displacement point, and generating an optimized path for the robotic arm; reconstructing the scene based on the grid point cloud, supporting collision detection with adjustable accuracy, and supporting obstacle avoidance planning with a safe distance; obtaining a reachable obstacle avoidance path for collaborative exploration of the robotic arm, reducing the number of invalid explorations, and reducing computing power requirements; optimizing the reachable obstacle avoidance path by adopting a wire pulling strategy, and improving the convergence speed of path optimization. BRIEF DESCRIPTION OF THE DRAWINGS

[0062] In order to more clearly illustrate the technical solutions of the embodiments of the present application, the drawings required for use in the embodiments of the present application will be briefly introduced below. It should be understood that the following drawings only show certain embodiments of the present application and therefore should not be regarded as limiting the scope. For ordinary technicians in this field, other related drawings can be obtained based on these drawings without paying creative work.

[0063] Figure 1 A flowchart of a robot arm obstacle avoidance path planning method provided in an embodiment of the present application;

[0064] Figure 2 A flow chart of an obstacle avoidance scenario construction method for a robot arm obstacle avoidance path planning method provided in an embodiment of the present application;

[0065] Figure 3 A flow chart of a method for acquiring a point cloud that meets a safe distance for a robot arm obstacle avoidance path planning method provided in an embodiment of the present application. DETAILED DESCRIPTION

[0066] The technical solutions in the embodiments of the present application will be clearly and completely described below in conjunction with the drawings in the embodiments of the present application. Obviously, the described embodiments are only a part of the embodiments of the present application, rather than all of the embodiments. The components of the embodiments of the present application described and shown in the drawings here can be arranged and designed in various different configurations. Therefore, the following detailed description of the embodiments of the present application provided in the drawings is not intended to limit the scope of the application claimed for protection, but merely represents the selected embodiments of the present application. Based on the embodiments of the present application, all other embodiments obtained by those skilled in the art without making creative work belong to the scope of protection of the present application.

[0067] It should be noted that similar reference numerals and letters represent similar items in the following drawings, so once an item is defined in one drawing, it does not need to be further defined and explained in subsequent drawings. At the same time, in the description of this application, the terms "first", "second", etc. are only used to distinguish the description and cannot be understood as indicating or implying relative importance.

[0068] Please refer to Figure 1 , Figure 1 1 is a flow chart of a robot arm obstacle avoidance path planning method in some embodiments of the present application. The robot arm obstacle avoidance path planning method is used in a terminal device, and the robot arm obstacle avoidance path planning method includes the following steps:

[0069] S101, builds static scene model and dynamic model based on grid point cloud, and reconstructs obstacle avoidance scene with additional safety distance;

[0070] S102, obtaining the obstacle avoidance path that can be reached by the robot arm collaborative exploration, finding the obstacle avoidance path point, calculating the obstacle avoidance exploration direction, and controlling the robot arm to move directly to the nearest collision-free point recorded according to the minimum obstacle avoidance direction;

[0071] S103, obtaining the current position and posture of the robot arm, determining whether the current position and posture of the robot arm is collision-free and collision-free with the linear interpolation motion between adjacent points, if there is no collision, keeping the current position and posture unchanged, and recording the state of the waypoint;

[0072] S104, if there is a collision, control the robot arm to move a step unit length in the minimum obstacle avoidance direction;

[0073] S105, based on the state of the path points, a wire-pulling strategy is adopted to optimize the reachable obstacle avoidance path, an optimized target displacement point is obtained, and an optimized path of the robot arm is generated.

[0074] It should be noted that the static scene model and the dynamic model are constructed with raster point cloud, the obstacle avoidance scene with an additional safety distance is reconstructed, the obstacle avoidance path is explored by multi-agents, and the accessible path is optimized by the wire-pulling method. This method is suitable for obstacle avoidance planning scenarios with different collision detection accuracies, and at the same time reduces the computing power requirements for obstacle avoidance planning of the robot arm.

[0075] Please refer to Figure 2 , Figure 2 This is a flow chart of an obstacle avoidance scene construction method of a robot arm obstacle avoidance path planning method in some embodiments of the present application. According to an embodiment of the present invention, a static scene model and a dynamic model are constructed based on a grid point cloud to reconstruct an obstacle avoidance scene with an additional safety distance, specifically including:

[0076] S201, obtaining model file data with posture transformation; rasterizing the model physical data using a voxelization method, and converting the model into a point cloud model;

[0077] S202, constructing a point cloud algorithm-driven dynamic model in combination with corresponding kinematic data;

[0078] S203, obtaining static model file data, rasterizing the model physical data using a voxelization method, converting the model into a point cloud model, and expanding the model point cloud according to the safety distance and the model point cloud accuracy to obtain a point cloud that meets the safety distance requirements;

[0079] S204, placing the dynamic models according to their respective initial positions; placing the static models according to their initial positions, and integrating them into a point cloud model;

[0080] S205, removing duplicate points based on the point cloud model, generating an integrated model, and constructing an obstacle avoidance scene based on the integrated model.

[0081] It should be noted that the model file data of posture transformation includes the robot arm, tools and mobile materials. The static models are mostly static obstacles, and the obstacle avoidance range, that is, the safety distance, needs to be increased. The static model file data includes the robot arm bracket, table and feeder.

[0082] Please refer to Figure 3 , Figure 3 This is a flow chart of a method for obtaining a point cloud that meets the safety distance in a robot arm obstacle avoidance path planning method in some embodiments of the present application. According to an embodiment of the present invention, the model point cloud is expanded according to the safety distance and the model point cloud accuracy to obtain a point cloud that meets the safety distance requirement, specifically including:

[0083] S301, converting a point cloud model represented by a non-integer number into a point cloud model represented by an integer number based on the point cloud accuracy requirement of the model;

[0084] S302, based on the safety distance and the point cloud accuracy requirement of the model, calculating the number of layers that the point cloud model needs to expand;

[0085] S303, taking the current outermost point cloud as the core, generating adjacent points in six directions of front, back, left, right, up and down for each single point in the point cloud, and generating a new point cloud;

[0086] S304, removing duplicate points in the new point cloud and reference point cloud points as a new outermost point cloud, and iteratively generating multiple expanded point cloud layers according to a specified number of layers;

[0087] S305, integrating the expanded point cloud, taking the outermost point cloud as the core, removing the points repeated with the inner point cloud, and generating a point cloud that meets the safety distance requirements.

[0088] It should be noted that the formula for calculating the number of layers required to expand the point cloud model is as follows:

[0089] ,

[0090] Wherein, RoundToInt represents the approximate integer function; Indicates safe distance; Indicates the point cloud accuracy.

[0091] According to an embodiment of the present invention, obtaining a reachable obstacle avoidance path for collaborative exploration by a robotic arm, finding obstacle avoidance path points, and calculating an obstacle avoidance exploration direction specifically include:

[0092] Connect the line from the starting point to the end point of the task, perform straight line pose interpolation, and use the interpolation point as the path point; calculate the motion collision detection status between all path points and adjacent path points;

[0093] Eliminate completely collision-free path points based on the status of each path point;

[0094] Connect the task starting point to the end point to construct the task unit vector; take the task starting point as the center point and translate upward by one unit to construct the upward unit vector;

[0095] Based on the upward unit vector, calculate the vertical vector perpendicular to the task vector; the vertical vector is rotated around the axis at a specified angle to construct an obstacle avoidance exploration direction group.

[0096] It should be noted that the task unit vector is represented by Vectask; the upward unit vector is Vecup; based on the upward unit vector, the vertical vector perpendicular to the task vector is calculated and recorded as VecVertical; the vertical vector is rotated around the axis Vectask at a specified angle θ to construct an obstacle avoidance exploration direction group, and the linear pose interpolation (that is, linear interpolation motion, the position uses the Cartesian linear interpolation method, and the attitude uses the quaternion spherical interpolation method) formula is as follows:

[0097] ;

[0098] ;

[0099] ;

[0100] ;

[0101] ;

[0102] ;

[0103] ;

[0104] In the formula, Indicates the position point on the straight line; Indicates the Cartesian coordinates of the starting point; represents the Cartesian coordinates of the end point; k represents the ratio of the length from the start point to the length from the start point to the end point; Represents the quaternion of the starting point attitude; The quaternion representing the end point attitude; Indicates the posture of a point on a straight line.

[0105] The calculation formula for the obstacle avoidance exploration direction group is as follows:

[0106] ;

[0107] ;

[0108] ;

[0109] ;

[0110] In the formula, A point indicating the location of the target point; Indicates the Cartesian coordinates of the starting point; Indicates the Cartesian coordinates of the end point; represents the Cartesian coordinates of the vertical upward point; θ represents the exploration step angle; Normal is the normalization function; Vecdir is the task direction vector; x, y, z are the x-axis value, y-axis value and z-axis value respectively; Qua is the quaternion generation function; RountToInt is the approximate conversion to integer function; Vecdir i Explore directions.

[0111] According to an embodiment of the present invention, controlling the robot arm to move directly to the nearest recorded collision-free point according to the minimum obstacle avoidance direction specifically includes:

[0112] Calculate the maximum obstacle avoidance distance of all path points in each obstacle avoidance direction;

[0113] Select the smallest obstacle avoidance direction among the maximum obstacle avoidance distances of the path points in all directions as the minimum obstacle avoidance direction, and record the minimum obstacle avoidance distance in the minimum obstacle avoidance direction;

[0114] Move directly to the nearest collision-free point recorded in the direction of minimum obstacle avoidance.

[0115] It should be noted that the minimum obstacle avoidance direction is screened out by analyzing the maximum obstacle avoidance distance of all path points. The calculation formula for the nearest collision-free point is as follows:

[0116] ,

[0117] In the formula, Indicates the current location point; represents the exploration direction (unit length); Minimum moving step length.

[0118] According to an embodiment of the present invention, the step of acquiring a reachable obstacle avoidance path specifically includes:

[0119] Get the status of all path points and check whether there is any collision in the status of all path points;

[0120] If there is no collision, the exploration phase ends, and the reachable obstacle avoidance path points are recorded to obtain the reachable obstacle avoidance path;

[0121] If there is a collision situation at a path point, the minimum obstacle avoidance direction is set, and the maximum exploration rounds are set. The number of explorations for collision situations is analyzed until the maximum exploration rounds are reached. The exploration fails, the obstacle avoidance planning ends, and no reachable obstacle avoidance path is found.

[0122] According to an embodiment of the present invention, a reachable obstacle avoidance path is optimized by using a wire pulling strategy based on the path point state, an optimized target displacement point is obtained, and an optimized path of a robotic arm is generated, specifically including:

[0123] Calculate the optimal target displacement point of the path point, and use odd-numbered and even-numbered path points for alternate optimization to speed up the path point optimization;

[0124] Among them, the optimization steps of a single path point are as follows:

[0125] Target point collision detection: detect whether the current target point (the default is the midpoint between adjacent path points) has no collision and no collision with the linear interpolation motion between adjacent points, and record the status;

[0126] Determine whether to move: If the target point is in a collision-free state, the path point moves to the target point. If the current target point is the midpoint between adjacent path points, remove the current path point and connect the two adjacent path points. If it is not the midpoint between adjacent path points, the current path point optimization ends. If the target point is not in a collision-free state, the path point does not move and switches to the next target point until all target points are trained in rotation and the current path point optimization ends.

[0127] The optimization operation is performed once for all path points, which is called a round. If all path points are not displaced and the number of path points is unchanged for three consecutive rounds, the path optimization stage is ended, and the current path point data is recorded to obtain the optimized obstacle avoidance path.

[0128] It should be noted that the specific method for calculating the optimized target displacement point of the path point is as follows: based on the idea that the sum of the two sides of a triangle is greater than the third side, the wire-pulling optimization strategy is designed, and the middle point is pulled to the straight line between the two adjacent points to obtain the first target point - the midpoint between the adjacent path points; the half midpoint, the front adjacent midpoint, and the rear adjacent midpoint are set; the calculation formulas for the four target points are as follows:

[0129] Adjacency path midpoint:

[0130] ;

[0131] ;

[0132] ;

[0133] ;

[0134] ;

[0135] Half midpoint:

[0136] ;

[0137] ;

[0138] Front adjacent midpoint:

[0139] ;

[0140] ;

[0141] Back adjacent midpoint:

[0142] ;

[0143] ;

[0144] In the formula, Indicates the coordinates of the midpoint between adjacent path points; Indicates the position coordinates of the previous adjacent point; Indicates the position coordinates of the subsequent adjacent points; Represents the quaternion of the front neighbor point attitude; Represents the quaternion of the attitude of the back neighbor point; Represents the midpoint attitude quaternion between adjacent path points; Indicates the current point position coordinates; Represents the quaternion of the current point attitude; Sqrt is an open function; α is the angle between two adjacent points.

[0145] In a second aspect, an embodiment of the present application provides a robot arm obstacle avoidance path planning system, the system comprising: a memory and a processor, the memory comprising a program of a robot arm obstacle avoidance path planning method, and when the program of the robot arm obstacle avoidance path planning method is executed by the processor, the following steps are implemented:

[0146] Build static scene models and dynamic models based on raster point clouds, and reconstruct obstacle avoidance scenarios with additional safety distances;

[0147] Obtain the obstacle avoidance path that can be reached by the robot arm's collaborative exploration, find the obstacle avoidance path points, calculate the obstacle avoidance exploration direction, and control the robot arm to move directly to the nearest collision-free point recorded according to the minimum obstacle avoidance direction;

[0148] Get the current position of the robot arm, and determine whether the current position of the robot arm is collision-free and whether there is no collision with the linear interpolation motion between adjacent points. If there is no collision, keep the current position unchanged and record the path point status;

[0149] If there is a collision, the robot arm is controlled to move one step unit length in the minimum obstacle avoidance direction;

[0150] Based on the state of the path points, the wire pulling strategy is used to optimize the accessible obstacle avoidance path, obtain the optimized target displacement point, and generate the optimized path of the robotic arm.

[0151] It should be noted that the static scene model and dynamic model are constructed with grid point cloud, the obstacle avoidance scene with additional safety distance is reconstructed, the obstacle avoidance path is explored by multi-agents, and the accessible path is optimized by the wire drawing method. This method is applicable to obstacle avoidance planning scenarios with different collision detection accuracy, and at the same time reduces the computing power requirements of the robot arm's obstacle avoidance planning.

[0152] According to an embodiment of the present invention, a static scene model and a dynamic model are constructed based on a grid point cloud to reconstruct an obstacle avoidance scene with an additional safety distance, specifically including:

[0153] Obtain model file data with posture transformation; rasterize the model physical data using voxelization method and convert the model into a point cloud model;

[0154] Combine the corresponding kinematic data to build a point cloud algorithm-driven dynamic model;

[0155] Obtain static model file data, rasterize the model physical data using voxelization method, convert the model into a point cloud model, expand the model point cloud according to the safety distance and model point cloud accuracy, and obtain a point cloud that meets the safety distance requirements;

[0156] Arrange the dynamic models according to their respective initial positions; arrange the static models according to their initial positions and integrate them into a point cloud model;

[0157] Remove duplicate points based on the point cloud model, generate an integrated model, and build an obstacle avoidance scenario based on the integrated model.

[0158] It should be noted that the model file data of posture transformation includes the robot arm, tools and mobile materials. The static models are mostly static obstacles, and the obstacle avoidance range, that is, the safety distance, needs to be increased. The static model file data includes the robot arm bracket, table and feeder.

[0159] According to an embodiment of the present invention, the model point cloud is expanded according to the safety distance and the model point cloud accuracy to obtain a point cloud that meets the safety distance requirement, specifically including:

[0160] Based on the point cloud accuracy requirement of the model, the point cloud model represented by non-integer numbers is converted into a point cloud model represented by integer numbers;

[0161] Based on the safety distance and model point cloud accuracy requirements, calculate the number of layers that the point cloud model needs to expand;

[0162] Taking the current outermost point cloud as the core, generate adjacent points in six directions (front, back, left, right, up, and down) for each single point in the point cloud to generate a new point cloud;

[0163] Remove duplicate points in the new point cloud and the reference point cloud points as the new outermost point cloud, and iteratively generate multiple expanded point cloud layers according to the specified number of layers;

[0164] Integrate the expanded point cloud, take the outermost point cloud as the core, remove the points that are repeated with the inner point cloud, and generate a point cloud that meets the safety distance requirements.

[0165] It should be noted that the formula for calculating the number of layers required to expand the point cloud model is as follows:

[0166] ,

[0167] Wherein, RoundToInt represents the approximate integer function; Indicates safe distance; Indicates the point cloud accuracy.

[0168] According to an embodiment of the present invention, obtaining a reachable obstacle avoidance path for collaborative exploration by a robotic arm, finding obstacle avoidance path points, and calculating an obstacle avoidance exploration direction specifically include:

[0169] Connect the line from the starting point to the end point of the task, perform straight line pose interpolation, and use the interpolation point as the path point; calculate the motion collision detection status between all path points and adjacent path points;

[0170] Eliminate completely collision-free path points based on the status of each path point;

[0171] Connect the task starting point to the end point to construct the task unit vector; take the task starting point as the center point and translate upward by one unit to construct the upward unit vector;

[0172] Based on the upward unit vector, calculate the vertical vector perpendicular to the task vector; the vertical vector is rotated around the axis at a specified angle to construct an obstacle avoidance exploration direction group.

[0173] It should be noted that the formula for linear interpolation (i.e. linear interpolation motion, where the position uses the Cartesian linear interpolation method and the attitude uses the quaternion spherical interpolation method) is as follows:

[0174] ;

[0175] ;

[0176] ;

[0177] ;

[0178] ;

[0179] ;

[0180] ;

[0181] The calculation formula for the obstacle avoidance exploration direction group is as follows:

[0182] ;

[0183] ;

[0184] ;

[0185] ;

[0186] In the formula, Indicates the position point on the straight line; Indicates the Cartesian coordinates of the starting point; represents the Cartesian coordinates of the end point; k represents the ratio of the length from the start point to the length from the start point to the end point; Represents the starting point attitude quaternion; The quaternion representing the end point attitude; Indicates the posture of a point on a straight line.

[0187] According to an embodiment of the present invention, controlling the robot arm to move directly to the nearest recorded collision-free point according to the minimum obstacle avoidance direction specifically includes:

[0188] Calculate the maximum obstacle avoidance distance of all path points in each obstacle avoidance direction;

[0189] Select the smallest obstacle avoidance direction among the maximum obstacle avoidance distances of the path points in all directions as the minimum obstacle avoidance direction, and record the minimum obstacle avoidance distance in the minimum obstacle avoidance direction;

[0190] Move directly to the nearest collision-free point recorded in the direction of minimum obstacle avoidance.

[0191] It should be noted that the minimum obstacle avoidance direction is screened out by analyzing the maximum obstacle avoidance distance of all path points. The calculation formula for the nearest collision-free point is as follows:

[0192] .

[0193] According to an embodiment of the present invention, the step of acquiring a reachable obstacle avoidance path specifically includes:

[0194] Get the status of all path points and check whether there is any collision in the status of all path points;

[0195] If there is no collision, the exploration phase ends, and the reachable obstacle avoidance path points are recorded to obtain the reachable obstacle avoidance path;

[0196] If there is a collision situation at a path point, the minimum obstacle avoidance direction is set, and the maximum exploration rounds are set. The number of explorations for collision situations is analyzed until the maximum exploration rounds are reached. The exploration fails, the obstacle avoidance planning ends, and no reachable obstacle avoidance path is found.

[0197] According to an embodiment of the present invention, a reachable obstacle avoidance path is optimized by using a wire pulling strategy based on the path point state, an optimized target displacement point is obtained, and an optimized path of a robotic arm is generated, specifically including:

[0198] Calculate the optimal target displacement point of the path point, and use odd-numbered and even-numbered path points for alternate optimization to speed up the path point optimization;

[0199] Among them, the optimization steps of a single path point are as follows:

[0200] Target point collision detection: detect whether the current target point (the default is the midpoint between adjacent path points) has no collision and no collision with the linear interpolation motion between adjacent points, and record the status;

[0201] Determine whether to move: If the target point is in a collision-free state, the path point moves to the target point. If the current target point is the midpoint between adjacent path points, remove the current path point and connect the two adjacent path points. If it is not the midpoint between adjacent path points, the current path point optimization ends. If the target point is not in a collision-free state, the path point does not move and switches to the next target point until all target points are trained in rotation and the current path point optimization ends.

[0202] The optimization operation is performed once for all path points, which is called a round. If all path points are not displaced and the number of path points is unchanged for three consecutive rounds, the path optimization stage is ended, and the current path point data is recorded to obtain the optimized obstacle avoidance path.

[0203] It should be noted that the specific method for calculating the optimized target displacement point of the path point is as follows: based on the idea that the sum of the two sides of a triangle is greater than the third side, the wire-pulling optimization strategy is designed, and the middle point is pulled to the straight line between the two adjacent points to obtain the first target point - the midpoint between the adjacent path points; the half midpoint, the front adjacent midpoint, and the rear adjacent midpoint are set; the calculation formulas for the four target points are as follows:

[0204] Adjacency path midpoint:

[0205] ;

[0206] ;

[0207] ;

[0208] ;

[0209] ;

[0210] Half midpoint:

[0211] ;

[0212] ;

[0213] Front adjacent midpoint:

[0214] ;

[0215] ;

[0216] Back adjacent midpoint:

[0217] ;

[0218] ;

[0219] In the formula, Indicates the coordinates of the midpoint between adjacent path points; Indicates the position coordinates of the previous adjacent point; Indicates the position coordinates of the subsequent adjacent points; Represents the quaternion of the front neighbor point attitude; Represents the quaternion of the attitude of the back neighbor point; Represents the midpoint attitude quaternion between adjacent path points; Indicates the current point position coordinates; Represents the quaternion of the current point attitude; Sqrt is an open function; α is the angle between two adjacent points.

[0220] A third aspect of the present invention provides a computer-readable storage medium, which includes a robot arm obstacle avoidance path planning method program. When the robot arm obstacle avoidance path planning method program is executed by a processor, the steps of the robot arm obstacle avoidance path planning method as described in any of the above items are implemented.

[0221] The present invention discloses a robot arm obstacle avoidance path planning method, system and medium. The method constructs a static scene model and a dynamic model based on a grid point cloud to reconstruct an obstacle avoidance scene with an additional safety distance; obtains a reachable obstacle avoidance path for collaborative exploration of the robot arm, searches for obstacle avoidance path points, calculates the obstacle avoidance exploration direction, and controls the robot arm to move directly to the nearest recorded collision-free point according to the minimum obstacle avoidance direction; obtains the current posture of the robot arm, determines whether the current posture of the robot arm is collision-free and whether there is collision-free linear interpolation motion between adjacent points, and if there is no collision, keeps the current posture unchanged and records the path point status; if there is a collision, controls the robot arm to move a step unit length according to the minimum obstacle avoidance direction; optimizes the reachable obstacle avoidance path based on the path point status by adopting a wire pulling strategy to obtain an optimized target displacement point and generate an optimized path for the robot arm; reconstructs the scene based on the grid point cloud, supports collision detection with adjustable accuracy, and supports obstacle avoidance planning with a safe distance; obtains the reachable obstacle avoidance path for collaborative exploration of the robot arm, reduces the number of invalid explorations, and reduces the computing power requirement; adopts a wire pulling strategy to optimize the reachable obstacle avoidance path, and improves the convergence speed of the path optimization.

[0222] In the several embodiments provided in the present application, it should be understood that the disclosed devices and methods can be implemented in other ways. The device embodiments described above are only schematic. For example, the division of units is only a logical function division. There may be other division methods in actual implementation, such as: multiple units or components can be combined, or can be integrated into another system, or some features can be ignored or not executed. In addition, the coupling, direct coupling, or communication connection between the components shown or discussed can be through some interfaces, and the indirect coupling or communication connection of devices or units can be electrical, mechanical or other forms.

[0223] The units described above as separate components may or may not be physically separated, and the components displayed as units may or may not be physical units; they may be located in one place or distributed on multiple network units; some or all of the units may be selected according to actual needs to achieve the purpose of the present embodiment.

[0224] In addition, all functional units in the embodiments of the present invention may be integrated into one processing unit, or each unit may be separately used as a unit, or two or more units may be integrated into one unit; the above-mentioned integrated units may be implemented in the form of hardware or in the form of hardware plus software functional units.

[0225] Those skilled in the art can understand that: all or part of the steps of implementing the above method embodiments can be completed by hardware related to program instructions, the aforementioned program can be stored in a readable storage medium, and when the program is executed, it executes the steps of the above method embodiments; and the aforementioned storage medium includes: mobile storage devices, read-only memories (ROM, Read-Only Memory), random access memories (RAM, Random Access Memory), disks or optical disks, and other media that can store program codes.

[0226] Alternatively, if the above-mentioned integrated unit of the present invention is implemented in the form of a software function module and sold or used as an independent product, it can also be stored in a readable storage medium. Based on this understanding, the technical solution of the embodiment of the present invention can be essentially or partly reflected in the form of a software product that contributes to the prior art. The software product is stored in a storage medium and includes several instructions for a computer device (which can be a personal computer, server, or network device, etc.) to execute all or part of the methods described in each embodiment of the present invention. The aforementioned storage medium includes: various media that can store program codes, such as mobile storage devices, ROM, RAM, magnetic disks or optical disks.

Claims

1. A robot arm obstacle avoidance path planning method, characterized in that: include: Build static scene models and dynamic models based on raster point clouds, and reconstruct obstacle avoidance scenarios with additional safety distances; Obtain the obstacle avoidance path that can be reached by the robot arm's collaborative exploration, find the obstacle avoidance path points, calculate the obstacle avoidance exploration direction, and control the robot arm to move directly to the nearest collision-free point recorded according to the minimum obstacle avoidance direction; Get the current position of the robot arm, and determine whether the current position of the robot arm is collision-free and whether there is no collision with the linear interpolation motion between adjacent points. If there is no collision, keep the current position unchanged and record the path point status; If there is a collision, the robot arm is controlled to move one step unit length in the minimum obstacle avoidance direction; Based on the path point status, the wire pulling strategy is used to optimize the reachable obstacle avoidance path, obtain the optimized target displacement point, and generate the robot arm optimization path, which includes: Calculating the optimized target displacement point of the path point, the optimizing target displacement point of the path point includes: designing a line-pulling optimization strategy based on the idea that the sum of two sides of a triangle is greater than the third side, pulling the middle point to the straight line between two adjacent points, and obtaining the first target point - the midpoint between adjacent path points; setting a half midpoint, a front adjacent midpoint, and a rear adjacent midpoint; Use odd-numbered and even-numbered path points for alternate optimization; The alternating optimization includes: detecting the current target point, determining whether the current target point has no collision and whether the linear interpolation motion between adjacent points has no collision, and recording the status; If the target point is in a collision-free state, the path point moves to the target point; If the current target point is the midpoint between adjacent path points, remove the current path point and connect the two adjacent path points; If it is not the midpoint between adjacent path points, the optimization of the current path point ends; If the target point is not in a collision-free state, the path point will not move and will switch to the next target point until all target points are trained in rotation and the current path point optimization ends; The optimization operation is performed once for all path points, which is called a round. If all path points are not displaced and the number of path points is unchanged for three consecutive rounds, the path optimization stage is ended, and the current path point data is recorded to obtain the optimized obstacle avoidance path.

2. The robot arm obstacle avoidance path planning method according to claim 1, characterized in that: Static scene models and dynamic models are constructed based on raster point clouds to reconstruct obstacle avoidance scenarios with additional safety distances, including: Obtain model file data with posture transformation; rasterize the model physical data using voxelization method and convert the model into a point cloud model; Combine the corresponding kinematic data to build a point cloud algorithm-driven dynamic model; Obtain static model file data, rasterize the model physical data using voxelization method, convert the model into a point cloud model, expand the model point cloud according to the safety distance and model point cloud accuracy, and obtain a point cloud that meets the safety distance requirements; Arrange the dynamic models according to their respective initial positions; arrange the static models according to their initial positions and integrate them into a point cloud model; Remove duplicate points based on the point cloud model, generate an integrated model, and build an obstacle avoidance scenario based on the integrated model.

3. The robot arm obstacle avoidance path planning method according to claim 2, characterized in that: The step of expanding the model point cloud according to the safety distance and the model point cloud accuracy to obtain a point cloud that meets the safety distance requirement specifically includes: Based on the point cloud accuracy requirement of the model, the point cloud model represented by non-integer numbers is converted into a point cloud model represented by integer numbers; Based on the safety distance and model point cloud accuracy requirements, calculate the number of layers that the point cloud model needs to expand; Taking the current outermost point cloud as the core, generate adjacent points in six directions (front, back, left, right, up, and down) for each single point in the point cloud to generate a new point cloud; Remove duplicate points in the new point cloud and the reference point cloud points as the new outermost point cloud, and iteratively generate multiple expanded point cloud layers according to the specified number of layers; Integrate the expanded point cloud, take the outermost point cloud as the core, remove the points that are repeated with the inner point cloud, and generate a point cloud that meets the safety distance requirements.

4. The robot arm obstacle avoidance path planning method according to claim 3, characterized in that: Obtain the obstacle avoidance path that can be reached through collaborative exploration of the robot arm, find the obstacle avoidance path points, and calculate the obstacle avoidance exploration direction, including: Connect the line from the starting point to the end point of the task, perform straight line pose interpolation, and use the interpolation point as the path point; calculate the motion collision detection status of all path points and adjacent path points; Eliminate completely collision-free path points based on the status of each path point; Connect the task starting point to the end point to construct the task unit vector; take the task starting point as the center point and translate upward by one unit to construct the upward unit vector; Based on the upward unit vector, calculate the vertical vector perpendicular to the task vector; the vertical vector is rotated around the axis at a specified angle to construct an obstacle avoidance exploration direction group.

5. The robot arm obstacle avoidance path planning method according to claim 4, characterized in that: According to the minimum obstacle avoidance direction, control the robot arm to move directly to the nearest collision-free point recorded, including: Calculate the maximum obstacle avoidance distance of all path points in each obstacle avoidance direction; Select the smallest obstacle avoidance direction among the maximum obstacle avoidance distances of the path points in all directions as the minimum obstacle avoidance direction, and record the minimum obstacle avoidance distance in the minimum obstacle avoidance direction; Move directly to the nearest collision-free point recorded in the direction of minimum obstacle avoidance.

6. A robot arm obstacle avoidance path planning system, characterized in that: The system includes: a memory and a processor, wherein the memory includes a program of a robot arm obstacle avoidance path planning method, and when the program of the robot arm obstacle avoidance path planning method is executed by the processor, the following steps are implemented: Build static scene models and dynamic models based on raster point clouds, and reconstruct obstacle avoidance scenarios with additional safety distances; Obtain the obstacle avoidance path that can be reached by the robot arm's collaborative exploration, find the obstacle avoidance path points, calculate the obstacle avoidance exploration direction, and control the robot arm to move directly to the nearest collision-free point recorded according to the minimum obstacle avoidance direction; Get the current position of the robot arm, and determine whether the current position of the robot arm is collision-free and whether there is no collision with the linear interpolation motion between adjacent points. If there is no collision, keep the current position unchanged and record the path point status; If there is a collision, the robot arm is controlled to move one step unit length in the minimum obstacle avoidance direction; Based on the path point status, the wire pulling strategy is used to optimize the reachable obstacle avoidance path, obtain the optimized target displacement point, and generate the robot arm optimization path, which includes: Calculating the optimized target displacement point of the path point, the optimizing target displacement point of the path point includes: designing a line-pulling optimization strategy based on the idea that the sum of two sides of a triangle is greater than the third side, pulling the middle point to the straight line between two adjacent points, and obtaining the first target point - the midpoint between adjacent path points; setting a half midpoint, a front adjacent midpoint, and a rear adjacent midpoint; Use odd-numbered and even-numbered path points for alternate optimization; The alternating optimization includes: detecting the current target point, determining whether the current target point has no collision and whether the linear interpolation motion between adjacent points has no collision, and recording the status; If the target point is in a collision-free state, the path point moves to the target point; If the current target point is the midpoint between adjacent path points, remove the current path point and connect the two adjacent path points; If it is not the midpoint between adjacent path points, the optimization of the current path point ends; If the target point is not in a collision-free state, the path point will not move and will switch to the next target point until all target points are trained in rotation and the current path point optimization ends; The optimization operation is performed once for all path points, which is called a round. If all path points are not displaced and the number of path points is unchanged for three consecutive rounds, the path optimization stage is ended, and the current path point data is recorded to obtain the optimized obstacle avoidance path.

7. The robot arm obstacle avoidance path planning system according to claim 6, characterized in that: Static scene models and dynamic models are constructed based on raster point clouds to reconstruct obstacle avoidance scenarios with additional safety distances, including: Obtain model file data with posture transformation; rasterize the model physical data using voxelization method and convert the model into a point cloud model; Combine the corresponding kinematic data to build a point cloud algorithm-driven dynamic model; Obtain static model file data, rasterize the model physical data using voxelization method, convert the model into a point cloud model, expand the model point cloud according to the safety distance and model point cloud accuracy, and obtain a point cloud that meets the safety distance requirements; Arrange the dynamic models according to their respective initial positions; arrange the static models according to their initial positions and integrate them into a point cloud model; Remove duplicate points based on the point cloud model, generate an integrated model, and build an obstacle avoidance scenario based on the integrated model.

8. The robot arm obstacle avoidance path planning system according to claim 7, characterized in that: The step of expanding the model point cloud according to the safety distance and the model point cloud accuracy to obtain a point cloud that meets the safety distance requirement specifically includes: Based on the point cloud accuracy requirement of the model, the point cloud model represented by non-integer numbers is converted into a point cloud model represented by integer numbers; Based on the safety distance and model point cloud accuracy requirements, calculate the number of layers that the point cloud model needs to expand; Taking the current outermost point cloud as the core, generate adjacent points in six directions (front, back, left, right, up, and down) for each single point in the point cloud to generate a new point cloud; Remove duplicate points in the new point cloud and the reference point cloud points as the new outermost point cloud, and iteratively generate multiple expanded point cloud layers according to the specified number of layers; Integrate the expanded point cloud, take the outermost point cloud as the core, remove the points that are repeated with the inner point cloud, and generate a point cloud that meets the safety distance requirements.

9. A computer-readable storage medium, characterized in that: The computer-readable storage medium includes a robot arm obstacle avoidance path planning method program, and when the robot arm obstacle avoidance path planning method program is executed by a processor, the steps of the robot arm obstacle avoidance path planning method as described in any one of claims 1 to 5 are implemented.

Images