Handling robot collision detection method based on improved composite collision detector

Abstract

The application discloses a kind of based on the carrying mechanical arm collision detection method of improved composite collision detector.The steps of the present application are as follows: step 1, describe the carrying mechanical arm and workspace;Step 2, design accurate collision detector ECD;Step 3, design reasoning collision detector ICD.The application proposes that improved composite collision detector based on SEPD can obtain good balance between the efficiency and accuracy of mechanical arm collision detection.The performance of the collision detector can be improved for the online motion planning task of RRT algorithm, which improves the ability of carrying mechanical arm to respond to sudden environmental changes in actual work, and also ensures the safety of working environment.The application has faster calculation speed in collision detection based on local data, and also avoids high-frequency false positive output in narrow channel.

Description

Technical Field

[0001] This invention relates to the field of warehouse logistics handling robot technology, specifically to the comprehensive performance improvement of the efficiency and accuracy of collision detectors for RRT path planning. Background Technology

[0002] In recent years, with the development of robotic arms in warehousing and logistics applications, the working environment is sometimes unpredictable. In such unknown working environments, robotic arms need to perform path planning at startup and even adjust the planned path online in real time. This poses a serious challenge to the efficiency of path planning algorithms. Currently, sampled path planners (SBPs) are one of the main path planning methods for high-dimensional configuration spaces, and collision detection is a basic module of SBPs, accounting for more than 90% of the total planning time. Its detection accuracy and efficiency affect the safety and efficiency of robotic arms during handling. The purpose of this invention is to improve the efficiency and safety of handling robotic arms when handling goods in warehouses by balancing the efficiency and accuracy of collision detection through a composite collision detection algorithm, providing a balanced collision detection strategy during robotic arm operation. In the past, the most common design was the kinematic and geometric collision detector (KCD), which was considered the "gold standard" and was also known as the precise collision detector (ECD).

[0003] Here's a brief overview of existing collision detection algorithms: 1. KCD has high accuracy but high computational load. 2. Deep learning-based collision detectors rely on a large number of training samples to achieve model generalization. The large number of labeled training samples leads to frequent KCD calls, resulting in significant computational overhead. 3. As a type of CCD, probabilistic collision detectors (PCD) require only a small amount of local data and maintain high efficiency, but suffer from high-frequency false positives when targeting narrow channel query points.

[0004] In summary, a new detection mechanism is needed for RRT path planning of handling robotic arms. This mechanism has a fast calculation speed in collision detection based on local data, while also avoiding high-frequency false positive outputs in narrow channels. This mechanism achieves a good balance between efficiency and accuracy. Summary of the Invention

[0005] The purpose of this invention is to improve the efficiency and safety of handling robots when working in warehouses. It proposes a collision detection method for handling robots based on an improved composite collision detector. The handling robot balances its work efficiency and accuracy through the new collision detection strategy.

[0006] The technical solution adopted by this invention to solve its technical problem is as follows:

[0007] Step 1: Description of the robotic arm and workspace.

[0008] For motion planning of a series rigid robot, the joint variable vectors are considered as configurations, and the joint space is considered as configuration space (C). In Cartesian space, a world coordinate system {F} is established. w}, and a series of coordinate systems {F i}, i = 0, 1, ..., n + m1 + m2, where {F i The link is fixed to the i-th object. Ellipsoids are used to represent links and end effectors. Convex bodies, such as cuboids and cylinders, are used to represent the manipulator's base and obstacles. Each link is represented by only one ellipsoid, and the end effector is represented by one or more ellipsoids. All these regular approximations are treated as objects in the environment, numbered from 0 to n+m1+m2. Number 0 corresponds to the manipulator base. Numbers 1 to n correspond to the links from the base to the end effectors, respectively. Numbers n+1 to n+m1 correspond to the ellipses representing the effectors. Numbers n+m1+1 to n+m1+m2 correspond to obstacles.

[0009] For a frame connecting an ellipsoid, its center point is located at the geometric center of the ellipsoid, and its axes are parallel to the axes of the ellipsoid. For configuration q, let T... i (q)∈R 3×3 From {F i} to {F w The transformation matrix of}, X i (q)∈R 3×1 It is {F i} with {F w} is the center point of the reference frame. Additionally, from {F... i} to {F w The homogeneous transformation matrix of} is expressed as Right now

[0010]

[0011] When 1≤i≤n+m1 T i (·) and X i (·) is continuous and can be obtained using the DH method.

[0012] When i > n + m1 or i = 0 T i (q) and X i (q) is a constant, but for convenience, a mapping form is generally used. Let Θ i The coordinate system {F} points to the i-th object. i The set of point vectors}, and denoted by the symbol Θ i (·) represents Θ i Affine transformation, i.e.

[0013]

[0014] in, Represents the alignment transformation matrix Affine transformation, z∈R 3×1 It is a column vector, T∈R 3×3 It is an invertible matrix. It can be represented by T and z as follows:

[0015]

[0016] Therefore, the i-th object point set in the reference frame is represented as

[0017] Assume that under joint constraints, no collisions occur between interconnected objects; that is, no collisions occur between the first link and the base, between two adjacent links, between the last link and the end effector, and between the two parts of the end effector. It is the set of all index pairs corresponding to possible collision object pairs. For a configuration q∈C, if there exists Make Then q∈C obs Otherwise, q∈C free C obs C represents the set of configurations that collide. free This represents the set of configurations that do not collide.

[0018] Step 2, Design the Precision Collision Detector (ECD), which consists of two stages.

[0019] Phase 1: Propose a variant of the Gilbert–Johnson–Keerthi (GJK) algorithm to calculate ellipsoidal pseudorange (EPD). Phase 2: Propose sphere-ellipsoidal pseudorange (SEPD) to describe the state of two objects. Details are as follows:

[0020] (1) In this invention, boundary volumes are used to reduce the number of intersection tests, wherein spheres are used as enclosing volumes for rapid initial rejection tests. Each object is compactly enclosed by a sphere, the boundary sphere of the i-th object is numbered i, and the center point and radius of the object are denoted as X. i (q) and R i For 1≤i≤n+m1, R i =max{r i1 ,r i2 ,r i3 And X i (q)=x i(q). Since a sphere can be considered a special type of ellipsoid, the EPD can also be used to characterize the relationship between two bounding boxes. For configuration q, the EPD from the center of the i-th (1≤i≤n+m1) object to point x is defined as:

[0021]

[0022] Among them Λ i =diag{r i1 -1 ,r i2 -1 ,r i3 -1}, where r i1 r i2 and r i3 It is the length of the semi-axis of the ellipsoid. For a fixed configuration q, Ω i The value of (q,·) will be R 3 The space is divided into three parts, which correspond precisely to the surface of the ellipsoid (Ω). i (q,x)=1), the interior of the ellipsoid (Ω) i (q,x)<1) and the outer edge of the ellipsoid (Ω) i (q,x)>1). Therefore, the geometric relationship between the i-th object (1≤i≤n+m1) and the j-th object can be determined by d. i.j (q) is used to reflect this, and it is represented as follows:

[0023]

[0024] If the i-th and j-th objects intersect, then d i.j ≤1 must be true; otherwise, d i.j >1 is true. In fact, d i.j (q) is from X i The EPD from (q) (the center of the elliptic) to the j-th object. Therefore, the EPD of configuration q can be expressed as:

[0025]

[0026] Clearly, the collision or non-collision state of configuration q can be reflected by E(q), that is, if q∈C obs If , then E(q)≤1 holds true; otherwise, E(q)>1 holds true.

[0027] Based on equation (5), equation (6) can be rewritten as follows:

[0028]

[0029] in

[0030] The main loop of the GJK algorithm is to calculate the minimum Euclidean norm of a point on a convex set. This invention recalculates the EPD by adjusting the GJK algorithm through transforming the EPD expression of configuration q.

[0031] (2) The EPD between two spheres in a special scenario is called the sphere pseudo-distance (SPD). The SPD from the center of the i-th sphere to the j-th sphere is denoted as: It can be represented as:

[0032]

[0033] Obviously, if the i-th and j-th spheres intersect, then It is valid. Otherwise, Established. The value of can be calculated according to equation (8), where Right now

[0034]

[0035] Based on the SPD (Square Sphere Design Process) describing two bounding spheres and the EPD (Extended Perspective Design Process) describing two objects, the SEPD (Separate Perspective Design Process) is used to describe the relationship between these two objects. Let the parameter δ > 0, and the distance from the center of the i-th object to the j-th object... Defined as Where f δ :R ≥0 ×R ≥0 →R is represented as:

[0036]

[0037] Furthermore, the SEPD of the robot relative to configuration q is defined as:

[0038]

[0039] It is easy to see that l δ (·,·) is continuous. When When calculating the SEPD from the center of the i-th object to the j-th object:

[0040]

[0041] From the above equation, we can obtain... When q∈C obs ; When q∈C free If and only if q∈C obs hour, Like EPD, the feature variable SEPD is also continuous. Its values ​​contain rich local information about the configuration space and can be used to estimate the boundary C of configuration q. obs .

[0042] Step 3: Design the Inference Collision Detector (ICD). The ICD's functions include inferring collision detection results and setting two rejection rules to verify the predicted collision detection results. After querying the ICD, the query point along with its precise SEPD value is stored in the database. For point queries, local data is first loaded from the database using an approximate KNN search method, and the result of the approximate KNN search, E... AKNN It is represented as:

[0043]

[0044] Among them, E AKNN It is all in the database The set of r nb Given the radius of the nearest neighbor, q data This indicates the configuration of the data points.

[0045] For query points that pass the first rejection rule, inference based on the locally weighted regression (LWR) method is performed, using LWR to predict the SEPD value for that query point. After calculating the predicted value, for If the accuracy of the collision result returned by the reasoning is lower than the set threshold, an ambiguity index needs to be set through the second rejection rule to indicate the reliability of the collision result of the reasoning.

[0046] The beneficial effects of this invention are as follows:

[0047] 1. An improved composite collision detector based on SEPD is proposed, which can achieve a good balance between efficiency and accuracy in collision detection of robotic arms.

[0048] 2. This collision detector can be used to improve the performance of online motion planning tasks of the RRT algorithm, enhance the ability of the handling robot to cope with sudden environmental changes in actual work, and also ensure the safety of the working environment.

[0049] 3. It has a faster calculation speed in collision detection based on local data, and also avoids high-frequency false positive output in narrow channels. Attached Figure Description

[0050] Figure 1 A schematic diagram illustrating the robotic arm and workspace;

[0051] Figure 2 This is a schematic diagram of the coordinate system for the connecting rod.

[0052] Figure 3 This is a schematic diagram of the composite collision detector system structure;

[0053] Figure 4 Here is the flowchart for the ECD algorithm;

[0054] Figure 5 Here is the flowchart for the ICD algorithm;

[0055] Figure 6 This is a schematic diagram of three motion planning tasks. Detailed Implementation

[0056] To address the problems existing in the prior art, this invention discloses a collision detection method for a handling robot based on an improved composite collision detector. The invention will be described in detail below with reference to the accompanying drawings.

[0057] like Figure 1 The diagram illustrates the handling robot and its workspace. Ellipsoids are used to represent links and end effectors. Convex bodies, such as cuboids and cylinders, are used to represent the robot's base and obstacles. Each link is represented by only one ellipsoid, and the end effector is approximated by one or more ellipsoids. All these regular approximations are treated as objects in the environment and numbered from 0 to n+m1+m2. Number 0 corresponds to the manipulator base. Numbers 1 to n correspond to the links from the base to the end effectors, respectively. Numbers n+1 to n+m1 correspond to the ellipses representing the end effectors. Numbers n+m1+1 to n+m1+m2 correspond to obstacles.

[0058] like Figure 2 The diagram shows the establishment of a coordinate system attached to the link ellipsoid. This coordinate system should be fixed at the center of the ellipsoid, that is, its center point is located at the geometric center, and its coordinate axes are parallel to the ellipsoidal axes.

[0059] like Figure 3 The diagram shows the structure of a composite collision detector system, which consists of three parts: the first part is the precise collision detector; the second part is the inference collision detector; and the third part is the database.

[0060] The specific description is as follows:

[0061] (1) The precise collision detector calculates the EPD value through a variant GJK algorithm, and calculates the SEPD value based on the EPD value and SPD value. It returns the collision detection result using the SEPD value, and stores the query point and SEPD value in local data.

[0062] (2) The inference collision detector includes an inference process and a decision module based on the first and second rules. The inference collision detector calls the local data set of query points obtained from the precise collision detector to perform an approximate KNN search, with the search result being E. AKNN The decision module of the first and second rules is used to infer the collision result of the query point and estimate the reliability of the collision result of the query point. Unreliable collision results are then transferred to the accurate collision detector for judgment.

[0063] (3) The configuration and SEPD value of the query point in the precise collision detector are stored in the local dataset of the database and are invoked by the inference collision detector.

[0064] The flowchart of the precise collision detector algorithm in the first part above is as follows: Figure 4 As shown, the details are as follows:

[0065] For a given configuration q, first initialize the SEPD value to infinity, then the objects in the configuration space... The SPD value of the enclosing ball pair is calculated using equation (10). For SPD values ​​less than 1+δ, the SEPD value is taken as... Otherwise, the value is Then, the SEPD value is used to determine whether the bounding spheres collide, and the SEPD value and collision state of each pair of objects are recorded.

[0066] The aforementioned second part, the collision detector, includes an inference process and a decision module based on the first and second rules.

[0067] In the first rejection rule of ICD: E AKNN The number of elements in the matrix is ​​denoted by K, and is used to represent E. AKNN Sufficiency. Given the minimum limit K of sufficiency. m When K≥K m At that time, E AKNN It is considered to contain sufficient information. If E AKNN Insufficient, i.e., K <K m The point was rejected by the ICD and detected by the ECD.

[0068] Additionally, LWR-based inference is performed on points that pass the first rejection rule. Based on the provided data, the LWR method is used to predict their SEPD values. The LWR method assigns weights based on the similarity between the query point and points in the database. This invention uses a Gaussian kernel to represent similarity, i.e.:

[0069] G(q data ,q)=exp(-γ||q data -q|| 2 (15)

[0070] Where γ is the kernel width. Let And assigned to The weights are set as follows:

[0071]

[0072] Therefore, q data,i Represents database q data The SEPD prediction value for the i-th point in the query point q can be obtained by the following formula:

[0073]

[0074] The second rejection rule of ICD is described as follows: When calculating the predicted value... After that, for Return the result of the collision or for Returning a collision-free result seems quite natural. Additionally, an ambiguity index is needed to indicate the reliability of the inference result. In this method, the predicted value... It is used not only to infer query results but also to estimate the reliability of the results. Parameters U1∈(0,1] and U2∈(0,+∞) are set to determine the uncertainty region [1-U1,1+U2]. If... The inference result is then considered ambiguous, and the query point needs to be detected via ECD. If If not in the uncertain region, then for Returns to a collision state, for Return to a collision-free state.

[0075] The ICD algorithm flow is as follows: For a given configuration q, the local data set E of the query point is used... KNN Perform an approximate KNN search, and the search result is E. AKNN E AKNN If the size is K, and E AKNN Insufficient, i.e., K <K m The point is rejected by the ICD but detected by the ECD; when K≥K m At that time, calculate the weight W corresponding to the SEPD value. i This is used to predict the SEPD value of the query point. if The inference result is then considered ambiguous, and the query point needs to be detected via ECD. If If not in the uncertain region, then for Returns to a collision state, for Returns to a collision-free state. The ICD algorithm flowchart is as follows: Figure 5 As shown.

[0076] As above Figure 6 The diagram shows three motion planning tasks where the robot is required to grasp a cylinder (the manipulators with initial and target configurations are shown in different schematics): (a) Task 1, where the space is mostly empty. (b) Task 2, where the cylinder is surrounded by a cuboid, and the end effector must enter a narrow passage to grasp the cylinder. (c) Task 3, where, in addition to the narrow passage, there are seven axially aligned obstacle boxes.

[0077] The experimental results are shown in Table 1 below:

[0078]

[0079] In Table 1, R, TP, TN, Ptime, and Ntime represent the proportion of inference queries, true positive rate, true negative rate, average detection time for positive points, and average detection time for negative points, respectively. Compared to KCD, PCD and SEPD-CCD show decreased TP and TN (especially TP), but significantly improved efficiency. Both PCD and SEPD-CCD have negative point detection times within approximately 0.3ms, but significant differences exist in positive point detection times. In each task, PCD's Ptime is shorter than SEPD-CCD's Ptime, with a difference greater than 1ms, sometimes even approaching 2.5ms. There is also a significant difference in TP between PCD and SEPD-CCD; in each task, SEPD-CCD's TP is greater than PCD's, with a difference exceeding 10%, sometimes even exceeding 30%. Although both CCDs improve efficiency at the expense of accuracy, some performance differences remain. If their inference query proportions are similar, SEPD-CCD can achieve higher accuracy by comparison.

Claims

1. A collision detection method for a handling robot based on an improved composite collision detector, characterized in that... Includes the following steps: Step 1: Describe the robotic arm and workspace; Step 2: Design a precise collision detector (ECD); Step 3: Design an inference collision detector (ICD); The description in step 1 is as follows: For motion planning of a series rigid robot, the joint variable vectors are considered as configurations, and the joint space is considered as configuration space. ; Establish a world coordinate system in Cartesian space. and a series of coordinate systems , ,in Fixed in the Each link is represented by an ellipsoid; links and end effectors are represented by ellipsoids; convex bodies are used to represent the base and obstacles of the manipulator; each link is represented by only one ellipsoid, and the end effector is represented by one or more ellipsoids. All these regular approximations are treated as objects in the environment, numbered from 0 to... ; The number 0 corresponds to the base of the controller; the numbers 1 to... These correspond to the links from the connecting base to the connecting end effector; digital to Corresponding to the ellipse representing the effector; digital to Corresponding to obstacles; For a frame connecting an ellipsoid, its center point is located at the geometric center of the ellipsoid, and its axes are parallel to the axes of the ellipsoid; for the configuration ,set up From arrive The transformation matrix, yes by The center point of the reference frame; in addition, from arrive The homogeneous transformation matrix is ​​expressed as ,Right now ； when hour, , and It is continuous, obtained using the DH method; when or hour, , and Let it be a constant. It is the first The coordinate system pointed to by the object The set of point vectors, and using the symbol to indicate The affine transformation, namely: ； in, Represents the alignment transformation matrix Affine transformation, It is a column vector. It is an invertible matrix. Depend on and Represented as: ； Therefore, the first in the frame of reference The set of object points is represented as ; Assume that under joint constraints, no collisions occur between interconnected objects; that is, no collisions occur between the first link and the base, between two adjacent links, between the last link and the end effector, and between the two parts of the end effector. It is the set of all index pairs corresponding to possible collision object pairs; for a configuration If it exists Make ,but ;otherwise, , This represents the set of configurations where collisions occur. Represents the set of configurations that do not collide; The design of the precise collision detector described in step 2 includes two stages: Stage 1: Proposing a variant of the GJK algorithm to calculate the ellipsoidal pseudorange EPD; Stage 2: Proposing the sphere-ellipsoidal pseudorange SEPD to describe the state of two objects. Step 3 describes the functions of the inference collision detector (ICD), including collision detection result inference and setting two rejection rules to verify the predicted collision detection results. After querying the ECD, the query point along with its precise SEPD value is stored in the database. For the query point, local data is first loaded from the database using an approximate KNN search method, and the results of the approximate KNN search are... It is represented as: ； in, The set, It is the given radius of the nearest neighbor. Represents the configuration of data points; For query points that pass the first rejection rule, inference based on the locally weighted regression (LWR) method is performed, and LWR is used to predict the SEPD value of that query point; after calculating the predicted value, for If the accuracy of the collision result returned by the inference is lower than the set threshold, an ambiguity index needs to be set through the second rejection rule to indicate the reliability of the collision result of the inference.

2. The collision detection method for a handling robot based on an improved composite collision detector according to claim 1, characterized in that... The specific implementation of Phase One is as follows: Each object is compactly encapsulated by a sphere, the first The boundary spheres of the objects are numbered as follows: And the center point and radius of the object are denoted as , respectively. ;for , and = Use EPD to characterize the relationship between two bounding boxes; for configuration From the first From the center of each object to the point The EPD is defined as: ； in, , ,in , and It is the length of the semi-axis of the ellipsoid; for a fixed configuration , The value will The space is divided into three parts, which correspond precisely to the surface of the ellipsoid.

1. The interior of the ellipsoid and the outside of the ellipsoid Therefore, the first The first object and the first The geometric relationships between objects can be achieved through... To reflect this, it is expressed as: ； If the The and the first If the objects intersect, then It must be true, otherwise, Established; in fact, From the center of the ellipsoid To the EPD of each object, so configuration EPD is represented as: ； Obviously, configuration The collision or non-collision state can be determined by Reflection, that is, if ,but Valid, otherwise Established; Based on equation (5), equation (6) can be rewritten as: ； in ; The main loop of the GJK algorithm is to calculate the minimum Euclidean norm of points on a convex set. This stage involves transforming the configuration. The EPD expression is adjusted using the GJK algorithm to recalculate the EPD.

3. The collision detection method for a handling robot based on an improved composite collision detector according to claim 1, characterized in that... The specific implementation of Phase Two is as follows: The EPD of two spheres in a special scenario is called the sphere pseudo-distance SPD; the SPD from the center of the i-th sphere to the j-th sphere is denoted as... It can be represented as: ； Obviously, if the i-th and j-th spheres intersect, then Valid; otherwise, Established; The value of can be calculated according to equation (8), where ,Right now ； Based on the SPD (Square Sphere Design Process) describing two bounding spheres and the EPD (Electronic Process Design Process) describing two objects, the SEPD (Separate Sphere Design Process) is used to describe the relationship between these two objects; let the parameter δ > 0, and define the SEPD from the center of the i-th object to the j-th object. Defined as ,in Represented as: ； And the robot relative to the configuration SEPD is defined as: ； because It is continuous, when When calculating the SEPD from the center of the i-th object to the j-th object: ； From the above equation, we can obtain... hour, ; hour, If and only if hour, The feature variable SEPD, like EPD, is continuous; its values ​​contain a rich local information space and can be used to estimate... boundary .

4. The collision detection method for a handling robot based on an improved composite collision detector according to claim 3, characterized in that... In the first rejection rule of the inference collision detector ICD: The number of elements in is expressed as , and used to represent Sufficiency; the minimum limit given sufficiency ,when hour, It is considered to contain sufficient information; if Insufficiency, that is The point was rejected by the ICD and detected by the ECD. For query points that pass the first rejection rule, perform LWR-based inference; based on the provided data, use the LWR method to predict their SEPD values; the LWR method sets weights according to the similarity between the query point and points in the database, and uses a Gaussian kernel to represent the similarity, i.e.: ； in It is the kernel width; let and assigned to The weights are set as follows: ； thus, Represents database The i-th point in the query point The predicted value of SEPD can be obtained from the following formula: 。 5. The collision detection method for a handling robot based on an improved composite collision detector according to claim 4, characterized in that... The second rejection rule of the inference collision detector (ICD) is described as follows: When calculating the predicted value... After that, for Return the result of the collision or for Returns a collision-free result; predicted value It is used not only to infer query results, but also to estimate the reliability of the results; setting parameters and To determine the uncertain region ;if If the inference result is ambiguous, the query point needs to be detected via ECD; if If not in the uncertain region, then for Returns to a collision state, for Return to a collision-free state; The algorithm flow of ICD is as follows: For a given configuration Through the local data set of the query point Perform an approximate KNN search, and the search results are: ,remember If the size is K, Insufficiency, that is The query point was rejected by the ICD and detected by the ECD; when When calculating the weights corresponding to the SEPD values, Predict the SEPD value of the query point ;if If the inference result is ambiguous, the query point needs to be detected via ECD; if If not in the uncertain region, then for Returns to a collision state, for Return to the collision-free state.

Images