SDF function-based robotic arm collision detection method

Abstract

An SDF function-based robotic arm collision detection method. The method is divided into an offline stage and an online stage. The offline stage comprises: constructing an SDF function for describing the reachability of an end effector of a robotic arm; constructing an SDF function for describing a geometric shape of each link of the robotic arm; constructing an SDF function for describing spatial geometric information of the working environment where a robot is located; and extracting the geometric boundary of the part of the robot's reachable space intruded by an environmental obstacle. The online stage comprises: querying a symbol value of a target pose in the SDF function for describing the reachability of the end effector of the robotic arm, to determine whether the target pose is reachable or not; calculating a transformation matrix of each link coordinate system relative to a robot base coordinate system; and for each point in the geometric boundary, transforming the point into each link coordinate system, and querying a symbol value of the point in the SDF function for describing the geometric shape of each link, to determine whether the point collides with the link or not. The time of the collision detection stage with the maximum time consumption proportion in the robotic arm trajectory planning process can be greatly shortened, and the trajectory planning performance is improved.

Description

A collision detection method for robotic arms based on SDF function Technical Field

[0001] The present invention belongs to the field of collision detection, and in particular relates to a robot arm collision detection method based on SDF function. Background Art

[0002] In the field of robotic arm motion planning, it is often necessary to determine whether the robotic arm has collided with the external environment. This algorithm, which determines whether the robotic arm has collided with the environment, is a collision detection method, which ensures the safety and reliability of the robotic arm's operation. However, collision detection algorithms based on different principles have different advantages and disadvantages.

[0003] Common collision detection techniques currently include the bounding box method and the separating axis method. The bounding box method uses shapes such as cubes, spheres, and cylinders to enclose the robot's links and obstacles, then determines whether these bounding boxes collide. While the bounding box method is simple and easy to understand, it can lose significant free space when used to enclose complex models, resulting in poor accuracy and misjudging collisions where none should have occurred. Using more complex bounding boxes, such as those made up of multiple triangles, can reduce the loss of free space but significantly increase collision detection time. The separating axis method works on the principle that if two objects do not collide, a plane must exist that separates them. This plane is called a separating plane. Assume that a parallel beam of light illuminates the two objects from different angles. When a suitable angle exists so that their shadows do not overlap, the separating plane is found. This method can be used to detect objects with complex shapes without losing significant free space, but its computational complexity is relatively complex and it is only applicable to collision detection between convex objects.

[0004] SDF, short for signed distance function, is a function that determines the distance from a point to the boundary of a finite region in space and also defines the sign of that distance. It implicitly describes the geometry of two-dimensional or three-dimensional objects in space and is commonly used in two-dimensional and three-dimensional model representations.

[0005] As the application scenarios of robotic arms become increasingly diversified, it is difficult for existing collision detection technologies to simultaneously achieve both minimal free space loss and low-time detection. Summary of the Invention

[0006] In response to the shortcomings of the prior art, the present invention proposes a robot arm collision detection method based on SDF function, which aims to achieve high-precision and low-time collision detection of the environment when the robot is in working state.

[0007] The purpose of the present invention is mainly achieved through the following technical solutions:

[0008] On one hand, the present invention discloses a robot arm collision detection method based on SDF function, which includes an offline stage and an online stage;

[0009] The offline stage includes: constructing the workspace bounding box, the geometric shape SDF function SDF_ee of the reachable space of the end tool of the robot, the geometric shape SDF function SDF_link_i of each link of the robot, and the geometric information SDF function SDF_env of the working environment of the robot;

[0010] The online stage includes: inputting the robot configuration and target pose, combining SDF_ee and SDF_env, obtaining the SDF function SDF_ee_new for the fusion of the manipulator and the working environment, and the geometric boundary of the part of the space accessible to the robot where obstacles in the environment invade;

[0011] The reachability is determined based on the relationship between the target position and SDF_ee_new. If it is unreachable or exceeds the bounding box, a collision occurs. Then, the point in the geometric boundary is transformed into the link coordinate system. The reachability of the point in SDF_link_i is queried. If it is unreachable or exceeds the bounding box, a collision occurs. If all links are not colliding, it is determined to be no collision.

[0012] Furthermore, the SDF function for constructing the geometric shape of the reachable space of the end-of-arm tool in the offline stage is specifically SDF_ee: the entire bounding box is uniformly discretized into a number of voxels and a voxelized grid is constructed, and all voxelized grids are initialized to negative values;

[0013] Traverse the entire joint space of the robot arm and mark the reachable voxelized grids as positive values ​​according to positive kinematics. If the grid is positive and the adjacent grid has a negative value, the grid is marked as 0 to represent the boundary information; this series of voxelized grids is stored as the geometric shape SDF function SDF_ee of the reachable space of the tool at the end of the robot arm.

[0014] Furthermore, the traversal of the entire joint space of the robotic arm and marking the reachable voxelized grid as positive values ​​according to positive kinematics is specifically as follows: from top to bottom in the order of the joints, each joint is rotated a small angle each time, and then the positive kinematic solution of the robotic arm under this set of joint angles is calculated to obtain the end position information of the robotic arm, the position information is rasterized in the X, Y, and Z directions, the corresponding unit voxel is found, and the SDF value of the voxelized grid is assigned a positive value.

[0015] Furthermore, in the offline stage, the SDF function SDF_link_i for constructing the geometric shape of each link of the robotic arm is specifically as follows: the entire bounding box is uniformly discretized into a number of voxels and a voxelized grid is constructed, all voxelized grids are initialized to negative values, the geometric shape information of the link is rasterized, and the voxel units occupied by the link in the voxelized grid are assigned positive values. This series of voxelized grids is stored, thereby obtaining the SDF function SDF_link_i of a single link.

[0016] Furthermore, the SDF function SDF_env for constructing the geometric information of the robot's working environment in the offline stage is specifically as follows: the entire bounding box is uniformly discretized into a number of voxels and a voxelized grid is constructed, all voxelized grids are initialized to negative values, all obstacles in the space are rasterized, and the voxel units occupied by obstacles in the voxelized grid are assigned positive values. If the grid is positive and the adjacent grid has a negative value, the grid is marked as 0, representing the boundary information of the obstacle. This series of voxelized grids is stored, and the SDF function SDF_env describing the geometric information of the robot's working environment is obtained.

[0017] Furthermore, the geometric boundary of the portion of the space accessible to the robot where obstacles in the environment intrude is achieved by the following steps:

[0018] Transform all voxel grids with positive sign values ​​in SDF_ee to the world coordinate system of the working environment through the transformation matrix between the base coordinate system of the manipulator and the world coordinate system of the working environment, and then check the SDF value of the corresponding voxel grid in SDF_env; take the smaller value of the return value of SDF_ee and SDF_env as the updated SDF function, marked as SDF_ee_new; compare SDF_ee_new with SDF_ee, and the newly added point set with SDF 0 is the geometric boundary of the part of the robot's reachable space where obstacles in the environment need to be extracted.

[0019] Furthermore, the transformation matrix between the base coordinate system of the robot arm and the world coordinate system of the working environment is realized by the following steps: when the target posture is known, the inverse kinematics solution is performed according to the target posture to obtain the corresponding joint angle q, and the transformation matrix of each link coordinate system relative to the robot base coordinate system is calculated.

[0020] Furthermore, the transformation matrix between the base coordinate system of the manipulator and the world coordinate system of the working environment is realized by the following steps: when the joint angles are known, the position T of the manipulator end under the set of joint angles is calculated by forward kinematics, and the transformation matrix of the link coordinate system of each link relative to the world coordinate system is calculated. Where i represents the connecting rod number.

[0021] Furthermore, the determination of reachability based on the relationship between the target position and SDF_ee_new is specifically as follows: rasterizing the target position, and querying the symbol value of the position in SDF_ee_new to determine its reachability.

[0022] According to another aspect of the specification, it includes a sensor, an industrial computer and a robotic arm, the sensor is used to obtain external information, and the industrial computer stores executable code. It is characterized in that when the industrial computer executes the executable code, it is used to implement a robotic arm collision detection method based on SDF function.

[0023] Beneficial effects of the present invention:

[0024] First, the collision detection method proposed in the present invention does not increase the time consumption and loss of free space as the shape of the obstacle becomes irregular and complicated, and is therefore suitable for collision detection with obstacles of various shapes.

[0025] Second, the present invention optimizes collision detection into a table lookup task by constructing multiple SDF functions, thereby reducing the time complexity to a constant level. BRIEF DESCRIPTION OF THE DRAWINGS

[0026] FIG1 is a flow chart of a robot arm collision detection method based on SDF function proposed by the present invention.

[0027] FIG2 is a structural diagram of a collision detection device based on an SDF function according to the present invention. DETAILED DESCRIPTION

[0028] In order to make the objectives, technical methods and advantages of the present invention more clearly understood, the specific embodiments of the present invention are further described in detail below with reference to the accompanying drawings.

[0029] As shown in Figure 1, the SDF function-based robot arm collision detection method of the present invention is mainly divided into two stages. The first stage is to construct three SDF functions offline to implicitly store: 1) the geometry of the reachable space of the end tool of the manipulator; 2) the geometry of each link of the robot arm; 3) the geometric information of the robot's working environment. The second part is to use the SDF function to look up the table to realize the rapid detection of whether the robot arm has collided with the environment. The first stage includes:

[0030] A predefined cubic bounding box E that is sufficient to cover the entire working space of the robot;

[0031] The upper and lower limits of the bounding box E in the X, Y, and Z axis directions: X max , Y max , Z max , X min , Y min , Z minThe size of the voxel unit in space is resolution_c. The number of voxels in the X, Y, and Z axis directions is N x , N y , N z . N x =ceil((X max -X min ) / resolution_c)

[0032] Where ceil means rounding up. Define a three-dimensional matrix SDF_ee with an initial value of -1 as the SDF function. The size of each dimension of the matrix is ​​N x , N y , N z .

[0033] Then, the joint angle space of the robot arm is traversed, that is, each joint angle of the robot arm is rotated by a small angle θ in turn. The obtained set of joint angles is solved by forward kinematics to obtain the three-dimensional coordinates X of the end of the robot arm. ee , Y ee , Z ee . Then, this set of three-dimensional coordinates is rasterized to obtain its voxel coordinate n x , n y , n z .n x =ceil((X ee -X min ) / resolution_c)

[0034] According to the voxel coordinates, the voxel with the corresponding subscript is assigned +1 in the three-dimensional matrix, SDF_ee[n x ][n y ][n z]=1, then the entire three-dimensional matrix is ​​traversed once. If a cell has a value of 1 and there is a cell with a value of -1 in the adjacent cell, the cell is assigned a value of 0. This constructs the SDF function of the geometric shape of the reachable space of the end tool of the robot. Further, it is stored. This step only needs to be performed once for a robot with the same configuration. The function takes any position in the space as input, and its return value is a symbolic value that describes the relative position relationship between the position and the reachable space of the robot: if the position is within the reachable space of the robot, the return value is a positive value (or negative value); if the position is within the unreachable space of the robot, the return value is a negative value (or positive value); if the position is at the boundary of the reachable space of the robot, the return value is 0. In this way, the geometric information of the reachable space of the robot is not explicitly stored as a collection of a series of points, lines, triangles, cubes or other simple geometric bodies, but is implicitly embedded in the SDF function. The above-mentioned base coordinate system that describes the spatial position coordinates can be any coordinate system, including but not limited to the base coordinate system of the robot.

[0035] SDF_ee can not only store the reachability and inaccessibility information of each position in the space, but also further update the SDF value of each voxelized grid to store more useful information, including but not limited to:

[0036] 1) The signed distance value between each voxelized grid and the boundary of the reachable space: the SDF value of the reachable voxelized grid is updated to +d (or -d), and the SDF value of the unreachable voxelized grid is updated to -d (or +d). Where d is the Euclidean distance from the point to the nearest point on the boundary. d can be obtained by traversing the voxelized grids with an SDF value of 0;

[0037] 2) The coordinates of the boundary point (or the voxelized grid corresponding to the boundary point) that is closest to each voxelized grid. The closest boundary point can be obtained by traversing the voxelized grid with an SDF value of 0;

[0038] When constructing the SDF function of the geometric information of each link of the robotic arm, the same as N x , N y , N z Define a three-dimensional matrix SDF_link_i with an initial value of -1. Use the origin of the link coordinate system as the origin of the bounding box E. Based on this, discretize the physical appearance of the link and rasterize it. Then assign +1 to the corresponding three-dimensional matrix voxel cells and store them. The specific steps are as follows:

[0039] The first step is to create a bounding box E in space that is large enough to encompass the robot link and discretize it into a series of voxel grids. The SDF value of each voxel grid is initialized to the negative maximum distance value (or the positive maximum distance value), such as -d_max (or +d_max). The resolution of the voxel grid can be determined by the user according to different needs;

[0040] In the second step, each voxelized grid is checked and its SDF value is updated based on the design model of the robot link (including but not limited to CAD, Solidework, etc.). This function is only related to the robot model, not the specific task. Therefore, for the same type of robot, the above SDF_ee and SDF_link_i need only be calculated once after the robot is manufactured, without repeated calculation.

[0041] When constructing the SDF function that describes the geometric information of the robot's working environment, we also use N x , N y , N z Define a three-dimensional matrix SDF_env with an initial value of -1. Rasterize all obstacle information in the space obtained by sensors such as cameras and lidar. Assign a value of +1 to the voxel cells occupied by obstacles in the three-dimensional matrix. Then, traverse the entire three-dimensional matrix. If a cell has a value of 1 and there is an adjacent cell with a value of -1, assign the cell a value of 0. The specific steps are as follows:

[0042] The first step is to create a bounding box E that sufficiently encompasses the workspace and discretize it into a series of voxelized grids. The SDF value of each voxelized grid is initialized to the maximum positive distance value (or the maximum negative distance value), such as +d_max (or -d_max). The resolution of the voxelized grid can be determined by the user according to different needs.

[0043] In the second step, each voxelized grid is checked and its SDF value is updated according to the model describing the robot workspace (including but not limited to CAD, Solidework, point clouds obtained by laser scanning, etc.). This function only needs to be calculated once after the task initialization is completed, and there is no need to calculate it repeatedly.

[0044] Secondly, SDF_env and SDF_ee are fused to extract the geometric boundaries of the part of the environment where obstacles invade the robot's reachable space Get an SDF function SDF_ee_new that contains the reachability of the end tool of the robot and the geometric information of the working environment of the robot. The specific fusion method is to first calculate the transformation matrix between the base coordinate system of the robot and the world coordinate system of the working environment through forward kinematics according to the configuration of the robot. According to the transformation matrix Transform all voxel units with a value of +1 in SDF_ee to the world coordinate system of the working environment, then check the SDF value of the corresponding voxel unit in SDF_env, and take the smaller value of the return value of SDF_ee and SDF_env as the updated SDF function, marked as SDF_ee_new. Compare SDF_ee_new with SDF_ee, and the newly added point set with SDF of 0 is the geometric boundary of the part of the robot's reachable space where obstacles in the environment need to be extracted.

[0045] Phase II includes:

[0046] First, read various SDF functions.

[0047] Furthermore, in order to determine whether the end of the manipulator is in a reachable state at the target position P_ee generated by the planner, the target position P_ee is first rasterized, and then the symbol value of the position in SDF_ee_new is queried to determine its reachability. SDF_ee_new[ceil(P_ee_x-X min ) / resolution_c][ceil(P_ee_y -Y min ) / resolution_c][ceil(P_ee_z-Z min ) / resolution_c]==1

[0048] Where P_ee_x, P_ee_y, and P_ee_z are the X, Y, and Z coordinates of the target position in the robot's base coordinate system, respectively. If unreachable, "true" is returned, indicating a collision and exiting the collision detection algorithm. If the position is outside the bounding box described by SDF_ee, "true" is also returned, indicating a collision and exiting the collision detection algorithm.

[0049] In order to determine whether the robot arm collides with the external environment under a set of joint angles, it is first necessary to calculate the position T of the end of the robot arm under this set of joint angles through forward kinematics, as well as the transformation matrix of the link coordinate system of each link relative to the world coordinate system. Where i represents the connecting rod number.

[0050] When the target posture is known, the inverse kinematics solution is performed according to the target posture to obtain the corresponding joint angle q, and the transformation matrix of each link coordinate system relative to the robot base coordinate system is calculated

[0051] Furthermore, for the geometric boundaries in SDF_ee_new Every point O in i, point O in the world coordinate system i Through the transformation matrix Transform to the link coordinate system:

[0052] Where V_link_i is the coordinate of the boundary point of the spatial obstacle in the link i coordinate system, which is a 3*1 matrix. In order to prevent the true value coordinate of the obstacle in the link coordinate system from exceeding the preset bounding box E, X is used. max , Y max , Z max , X min , Y min , Z min Limit the coordinates in V_link_i to ensure they are within the range.

[0053] By querying its symbol value in SDF_link_i, it is determined whether it collides with link i: SDF_link_i[ceil(V_link_i(1,1)-X min ) / resolution_c][ceil(V_link_i(2,1) -Y min ) / resolution_c][ceil(V_link_i(3,1)-Z min ) / resolution_c]==1

[0054] If a collision occurs, it returns "true" and exits the collision detection algorithm.

[0055] Referring to Figure 2, an embodiment of the present invention provides a robotic arm collision detection device based on the SDF function, including a sensor, an industrial computer and a robotic arm. The sensor is used to obtain external information. The industrial computer stores executable code. When the industrial computer executes the executable code, it is used to implement a robotic arm collision detection method based on the SDF function in the above embodiment.

[0056] The embodiments of the SDF-based robotic arm collision detection device provided by the present invention can be applied to any device with data processing capabilities, such as a computer or industrial personal computer. The device embodiments can be implemented using software, hardware, or a combination of software and hardware.

[0057] The implementation process of the functions and effects of each unit in the above-mentioned device is specifically described in the implementation process of the corresponding steps in the above-mentioned method, and will not be repeated here.

[0058] For the device embodiments, since they basically correspond to the method embodiments, the relevant parts can be referred to the partial description of the method embodiments. The device embodiments described above are merely illustrative, wherein the units described as separate components may or may not be physically separated, and the components shown as units may or may not be physical units, that is, they may be located in one place or distributed across multiple network units. Some or all of the modules may be selected according to actual needs to achieve the purpose of the present invention. A person of ordinary skill in the art can understand and implement the present invention without inventive work.

[0059] The above embodiments are used to illustrate the present invention rather than to limit the present invention. Any modifications and changes made to the present invention within the spirit of the present invention and the protection scope of the claims shall fall within the protection scope of the present invention.

Claims

1. A robot arm collision detection method based on SDF function, characterized in that: Includes offline and online stages; The offline stage includes: constructing a workspace bounding box, a geometric shape SDF function SDF_ee of the reachable space of the end tool of the robot, a geometric shape SDF function SDF_link_i of each link of the robot, and an SDF function SDF_env of the geometric information of the working environment of the robot; The online stage includes: inputting the robot configuration and target posture, combining SDF_ee and SDF_env, obtaining the SDF function SDF_ee_new of the fusion of the robot arm and the working environment and the geometric boundary of the part of the space accessible to the robot where obstacles in the environment invade; The reachability is determined based on the relationship between the target position and SDF_ee_new. If it is unreachable or exceeds the bounding box, a collision occurs. Then the point in the geometric boundary is transformed into the link coordinate system. The reachability of the point in SDF_link_i is queried. If it is unreachable or exceeds the bounding box, a collision occurs. If all links are not collided, it is determined to be no collision.

2. A robot arm collision detection method based on SDF function according to claim 1, characterized in that: The SDF function for constructing the geometric shape of the reachable space of the end tool of the robot arm in the offline stage is specifically SDF_ee: the entire bounding box is evenly discretized into a number of voxels and a voxelized grid is constructed, and all voxelized grids are initialized to negative values; Traverse the entire joint space of the robot and mark the reachable voxelized grids as positive values ​​according to positive kinematics. If the grid is positive and the adjacent grids have negative values, the grid is marked as 0, representing the boundary information. Store this series of voxelized grids, which is the geometric shape SDF function SDF_ee of the reachable space of the tool at the end of the robot.

3. A robot arm collision detection method based on SDF function according to claim 2, characterized in that: The method of traversing the entire joint space of the robotic arm and marking the reachable voxelized grid as positive values ​​according to positive kinematics is as follows: from top to bottom in the order of the joints, each joint is rotated a small angle each time, and then the positive kinematic solution of the robotic arm under this set of joint angles is calculated to obtain the end position information of the robotic arm, rasterize the position information in the X, Y, and Z directions, find the corresponding unit voxel, and assign a positive value to the SDF value of the voxelized grid.

4. The robot arm collision detection method based on SDF function according to claim 1, characterized in that: The SDF function SDF_link_i for constructing the geometric shape of each link of the robotic arm in the offline stage is specifically as follows: the entire bounding box is evenly discretized into a number of voxels and a voxelized grid is constructed, all voxelized grids are initialized to negative values, the geometric shape information of the link is rasterized, and the voxel units occupied by the link in the voxelized grid are assigned positive values. The series of voxelized grids are stored, and the SDF function SDF_link_i of a single link is obtained.

5. The robot arm collision detection method based on SDF function according to claim 1, characterized in that: The SDF function SDF_env for constructing the geometric information of the robot's working environment in the offline stage is specifically as follows: the entire bounding box is evenly discretized into a number of voxels and a voxelized grid is constructed, all voxelized grids are initialized to negative values, all obstacles in the space are rasterized, and the voxel units occupied by obstacles in the voxelized grid are assigned positive values. If the grid is a positive value and there is a negative value in the adjacent grid, the grid is marked as 0, representing the boundary information of the obstacle. This series of voxelized grids are stored, and the SDF function SDF_env describing the geometric information of the robot's working environment is obtained.

6. The robot arm collision detection method based on SDF function according to claim 1, characterized in that: The geometric boundary of the part of the robot's accessible space where obstacles in the environment intrude is achieved by the following steps: Transform all voxel grids with positive sign values ​​in SDF_ee to the world coordinate system of the working environment through the transformation matrix between the base coordinate system of the robot and the world coordinate system of the working environment, and then check the SDF value of the corresponding voxel grid in SDF_env; take the smaller value of the return value of SDF_ee and SDF_env as the updated SDF function, marked as SDF_ee_new; compare SDF_ee_new with SDF_ee, and the newly added point set with SDF 0 is the geometric boundary of the part of the robot's reachable space where obstacles in the environment need to be extracted.

7. The robot arm collision detection method based on SDF function according to claim 1 is characterized in that: The transformation matrix between the base coordinate system of the robot and the world coordinate system of the working environment is realized by the following steps: when the target posture is known, the inverse kinematics solution is performed according to the target posture to obtain the corresponding joint angle q, and the transformation matrix of each link coordinate system relative to the robot base coordinate system is calculated.

8. The robot arm collision detection method based on SDF function according to claim 1 is characterized in that: The transformation matrix between the base coordinate system of the manipulator and the world coordinate system of the working environment is realized by the following steps: when the joint angles are known, the posture T of the end of the manipulator under the set of joint angles is calculated by forward kinematics, and the transformation matrix of the link coordinate system of each link relative to the world coordinate system is calculated Where i represents the connecting rod number.

9. The robot arm collision detection method based on SDF function according to claim 1, characterized in that: The method of determining the reachability based on the relationship between the target position and SDF_ee_new specifically includes: rasterizing the target position, and querying the symbol value of the position in SDF_ee_new to determine its reachability.

10. A robot arm collision detection device based on SDF function, comprising a sensor, an industrial computer and a robot arm, wherein the sensor is used to obtain external information, and the industrial computer stores executable code, characterized in that: When the industrial computer executes the executable code, a robot arm collision detection method based on SDF function as described in any one of claims 1 to 9 is implemented.

Images