A continuous analytical construction method for convex polyhedron artificial potential field and gradient force

Abstract

The present invention discloses a continuous analytical construction method for the artificial potential field and gradient force of a convex polyhedron. Based on the analytical expression of the convex polyhedron model and the logical judgment of the orientation of the test point relative to the convex polyhedron, the present invention gives the analytical expressions of the distance d from a point to the convex polyhedron and its gradient ▽d at point P. Then, substituting them into the classical artificial potential function and its gradient expression, the artificial potential function and its gradient force adapted to the shape of the convex polyhedron are obtained. Compared with the prior art, the artificial potential function and gradient force constructed by the present invention are analytical, without numerical iterative calculation processes. Therefore, it can provide the repulsive force of the artificial potential field adapted to the shape of the convex polyhedron obstacle for real-time and accurate obstacle avoidance motion control. And when the orientation of the test point relative to the convex polyhedron is known a priori, the expressions of the artificial potential function and gradient force at this point are determined, eliminating the logical judgment step of the orientation of this point relative to the convex polyhedron.

Description

Technical Field

[0001] The present invention belongs to the technical field of robot / agent motion control, and particularly relates to a continuous analytical construction method for a convex polyhedron artificial potential field and gradient force. Background Art

[0002] When applying control theory to the motion control of real objects such as unmanned vehicles, unmanned aerial vehicles, or spacecraft, due to the complex obstacle constraints in the real environment, in order to ensure the safe operation of individuals, it is necessary to consider obstacle avoidance in the control law. At the planning level, it can be achieved by making the individual track a path that maintains a sufficient distance from the obstacles; at the control level, it can be achieved by adding a suitable collision avoidance term to the original control law of the individual. For obstacles with complex geometric structures, since it is not convenient to construct an artificial potential field for them, most of the existing obstacle avoidance schemes are solved from the planning perspective, which not only brings challenges to the proof of the stability and convergence of the closed-loop system, but also is not applicable to the avoidance of moving obstacles (such as other individuals) or obstacles discovered immediately. Although it is also possible to design a collision avoidance force using the artificial potential field method after approximating or approaching the obstacle shape with a spherical or ellipsoidal surface, the control accuracy is relatively limited, which may cause the individual to be too close or too far from the local area of the obstacle, thereby limiting the application of fine operation tasks near the obstacle area. Although some improved artificial potential field methods can solve the collision avoidance of obstacles with complex shapes, either they do not meet the potential field conditions (that is, the collision avoidance force is not the gradient of the designed potential function), lack the proof of the stability of the closed-loop system, or there are discontinuities, which are prone to chattering phenomena at the regional boundaries. Summary of the Invention

[0003] The purpose of the present invention is to overcome the above-mentioned disadvantages of the prior art, and provide a continuous analytical construction method for a convex polyhedron artificial potential field and gradient force, which is used to achieve refined motion control near convex polyhedron obstacles, so as to achieve tasks such as the formation of complex formation configurations and the fine operation of complex structure objects.

[0004] To achieve the above purpose, the present invention adopts the following technical solutions:

[0005] A continuous analytical construction method for a convex polyhedron artificial potential field and gradient force, comprising the following steps:

[0006] Step 1, establish a convex polyhedron model, test point coordinates, and parameter values; the convex polyhedron model is represented in the form of faces or in the form of vertices;

[0007] Step 2, transform the convex polyhedron model represented in the form of faces to obtain a convex polyhedron model represented in the form of vertices, or transform the convex polyhedron model represented in the form of vertices to obtain a convex polyhedron model represented in the form of faces;

[0008] Step 3: Determine the connection relationships between each face and vertex, between vertices, and between each face in the convex polyhedron through the convex polyhedron model represented in the form of faces and the convex polyhedron model represented in the form of vertices;

[0009] Step 4: Based on the convex polyhedron model represented in the form of faces and the convex polyhedron model represented in the form of vertices, and in combination with the connection relationships between each face and vertex, between vertices, and between each face in the convex polyhedron, determine the orientation of the test point relative to the convex polyhedron; the orientation of the test point relative to the convex polyhedron includes four forms: inside the convex polyhedron with boundaries, above the face of the convex polyhedron, above the edge of the convex polyhedron, and above the vertex of the convex polyhedron;

[0010] Step 5: According to the orientation form of the test point relative to the convex polyhedron, analytically calculate the distance between the test point and the convex polyhedron, and the gradient of the distance at the test point;

[0011] Step 6: Based on the distance between the test point and the convex polyhedron, calculate the artificial potential function of the convex polyhedron at the test point; based on the distance between the test point and the convex polyhedron and the gradient of the distance at the test point, calculate the gradient force of the convex polyhedron at the test point.

[0012] A further improvement of the present invention lies in:

[0013] Preferably, in Step 1, the convex polyhedron model represented in the form of faces is:

[0014]

[0015] wherein, the subscript of each inequality is the number of each face in the convex polyhedron, and n f is the number of faces of the convex polyhedron;

[0016] The convex polyhedron model represented in the form of vertices is:

[0017]

[0018] wherein, P i (i = 1, 2,..., n v ) are respectively the vertices of the convex polyhedron, n v is the number of vertices of the convex polyhedron, and O is the coordinate origin.

[0019] Preferably, in step 2, in the process of transforming the convex polyhedron model represented in the form of faces into the convex polyhedron model represented in the form of vertices, any three inequalities are selected from the convex polyhedron model represented in the form of faces, transformed into equations and solved; if there is no solution, two of the three faces are parallel; if there is a solution, it is verified whether the solution satisfies the other inequalities in the convex polyhedron model. If it satisfies, it is a vertex of the convex polyhedron; if it does not satisfy the other inequalities, the solution is discarded. After traversing all the inequalities, the convex polyhedron model represented in the form of faces is rewritten as the convex polyhedron model represented in the form of vertices.

[0020] In the process of transforming the convex polyhedron model represented in the form of vertices into the convex polyhedron model represented in the form of faces, any three vertices are selected from the convex polyhedron model represented in the form of vertices to construct a plane constraint equation, and the coordinates of other points are substituted. If the substituted point coordinates make the plane constraint equation hold, it is a face constraint of the convex polyhedron. If the substituted point coordinates make the plane constraint equation not hold, other points are substituted again until all three vertices are traversed, and the face constraints of the convex polyhedron are obtained, so as to obtain the convex polyhedron model represented in the form of faces.

[0021] Preferably, in step 3, the process of determining the connection relationship is as follows:

[0022] (1) The incidence matrix of each face and vertex in the convex polyhedron is If vertex P j (j = 1, 2,..., n v , n v is the number of vertices) is located on face F i (i = 1, 2,..., n f ), then Otherwise

[0023] (2) The adjacency matrix between the vertices is If vertex P i and vertex P j are both located on two faces of the polyhedron, then vertex P i is said to be adjacent to vertex P j , and correspondingly Otherwise

[0024] (3) The adjacency matrix between the faces is If face F i and face F j have two common vertices, then face F i is said to be adjacent to face F j , and correspondingly Otherwise

[0025] Preferably, in step 4, the process of determining the orientation of the test point relative to the convex polyhedron is as follows:

[0026] (1) When the test point is located inside the convex polyhedron including the boundary, the test point satisfies the constraint of the following formula (1):

[0027]

[0028] where the subscript corresponds to the numbers of each face of the convex polyhedron, and n f is the number of faces of the convex polyhedron;

[0029] (2) When the test point is located above the face of the convex polyhedron, the foot of the perpendicular of the test point in the plane F i satisfies the constraint formed by each adjacent face F in formula (7) from the plane F i j

[0030]

[0031] where the coordinates of the foot of the perpendicular H fi are obtained through the following formula (4):

[0032]

[0033] (3) The constraint condition when the test point is located above the common edge of the face F i and the face F j of the convex polyhedron is:

[0034]

[0035] where the definition and calculation method of the point H fi are as shown in formula (4), the points P1 and P2 are the common vertices of the face F i and the face F j of the convex polyhedron, and P i (i ∈ {1, 2,..., n v}) is the spatial vector corresponding to the point P i ;

[0036] (4) The constraint condition when the test point is located above the vertex P k of the convex polyhedron is:

[0037]

[0038] where the point is the adjacent vertex of the point P k (i ∈ {1, 2,..., n v ​​}), that is, satisfying the vertex, P i (i ∈ {1, 2,..., n v}) has the same meaning as above, and x is the spatial vector corresponding to point P.

[0039] Preferably, in step 5, according to the orientation form of the test point relative to the convex polyhedron, the process of analytically calculating the distance d between the test point and the convex polyhedron is as follows:

[0040] (1) When the test point is inside the convex polyhedron including the boundary, d = 0;

[0041] (2) When the test point P is above the face F i of the convex polyhedron, the calculation formula for the distance d is:

[0042]

[0043] (3) When the test point P is above the common edge P1P2 of the face F i and the face F j of the convex polyhedron, the calculation formula for the distance d is:

[0044]

[0045] (4) When the test point P is above the vertex P k of the convex polyhedron, the calculation formula for the distance d is:

[0046]

[0047] Preferably, in step 5, according to the orientation form of the test point relative to the convex polyhedron, the process of analytically calculating the gradient of the distance at the test point is as follows:

[0048] (1) When the test point is inside the convex polyhedron including the boundary, not defined;

[0049] (2) When the test point P is above the face F i of the convex polyhedron, the gradient is calculated by the formula:

[0050]

[0051] (3) When the test point P is above the common edge P1P2 of the face F i and the face F j of the convex polyhedron, the gradient is calculated by the formula:

[0052]

[0053] where, (xi , y i , z i )(i ∈ {1, 2,..., n v}) is the coordinate of point P i .

[0054] (4) When the test point P is above the vertex P of the convex polyhedron k , the calculation formula for the gradient is:

[0055]

[0056] Preferably, in step 6, the calculation formula for the artificial potential function is:

[0057]

[0058] where D o is the distance detection threshold, α is the artificial potential function coefficient, and d is the distance from the test point P to the convex polyhedron.

[0059] Preferably, in step 6, the calculation formula for the gradient force of the convex polyhedron at the test point is:

[0060]

[0061] where D o is the distance detection threshold, d and are respectively the distance from the test point P to the convex polyhedron and the gradient of the distance at point P.

[0062] Compared with the prior art, the present invention has the following beneficial effects:

[0063] The present invention discloses a continuous analytical construction method for the artificial potential field and gradient force of a convex polyhedron. Based on the analytical expression of the convex polyhedron model and the logical judgment of the orientation of the test point relative to the convex polyhedron, the distance d from a point to the convex polyhedron and its gradient at point P are given The analytical expression is obtained. Then it is substituted into the classical artificial potential function and its gradient expression, and the artificial potential function and its gradient force adapted to the convex polyhedron shape are obtained. Compared with the existing technology, the artificial potential function and gradient force constructed in the present invention are analytical, without numerical iteration calculation process. Therefore, it can provide the repulsive force of the artificial potential field adapted to the convex polyhedron obstacle shape for the obstacle avoidance motion control in real time and accurately. And when the orientation of the test point relative to the convex polyhedron is known a priori, the expressions of the artificial potential function and gradient force at this point are determined, eliminating the logical judgment step of the orientation of this point relative to the convex polyhedron. On the other hand, the artificial potential function and gradient force constructed in the present invention are continuous in the region except the interior (including the boundary) of the convex polyhedron, which is convenient for the proof of the stability and convergence of the closed-loop system under the obstacle avoidance control law, and also eliminates the motion jitter phenomenon of the individual at the junction of different regions outside the convex polyhedron. Therefore, it is applicable to the fine obstacle avoidance motion control in the standard convex polyhedron obstacle environment. This method is applicable to the motion control of unmanned vehicles, unmanned aerial vehicles or spacecraft and their clusters, especially to the collision avoidance motion control method for convex polyhedron obstacles. This method can better and more easily realize the collision avoidance control of obstacles with complex geometric structures. BRIEF DESCRIPTION OF THE DRAWINGS

[0064] Figure 1 FIG. is a schematic diagram of the calculation flow of the convex polyhedron artificial potential field and its gradient force of the present invention.

[0065] Figure 2 FIG. is a distribution diagram of each face, vertex and test point of a regular tetrahedron in a specific embodiment.

[0066] Figure 3 FIG. is a distribution cloud diagram of the artificial potential function near a regular tetrahedron in a specific embodiment.

[0067] Figure 4 FIG. is a contour map of the artificial potential function value of 1 near a regular tetrahedron in a specific embodiment. DETAILED DESCRIPTION OF THE EMBODIMENTS

[0068] The present invention will be further described in detail below with reference to the drawings and specific embodiments:

[0069] The present invention discloses a continuous analytical construction method for the potential field and gradient force of a convex polyhedron artificial potential function, including the following steps: S1: Provide a convex polyhedron model, the coordinates of a test point, and the values of each parameter; S2: Calculate the convex polyhedron model represented by faces or vertices; S3: Analyze the connection relationships between the faces and vertices of the convex polyhedron, between vertices, and between each face; S4: Determine the orientation of the test point relative to the convex polyhedron; S5: According to the different orientations of the test point, calculate its distance to the polyhedron and its gradient at the test point respectively; S6: Calculate the value of the artificial potential function of the convex polyhedron at the test point according to the distance from the test point to the polyhedron; S7: Calculate the gradient force of the artificial potential field of the convex polyhedron at the test point according to the distance from the test point to the polyhedron and its gradient value at the test point. The present invention can construct an artificial potential function and its gradient force that are adapted to the shape, continuous and analytical for a convex polyhedron model with an analytical expression, which is convenient for proving the stability and convergence of the closed-loop system under the obstacle avoidance control law, and also eliminates the chattering phenomenon of the obstacle avoidance movement at the junction of different regions outside the convex polyhedron. Therefore, the present invention is applicable to the precise obstacle avoidance movement control in a standard convex polyhedron obstacle environment.

[0070] As shown in the attached Figure 1 figure, a continuous analytical construction method for the artificial potential field and gradient force of a convex polyhedron proposed by the present invention, that is, to calculate the value of the artificial potential function at the test point P(x P , y P , z P ), includes the following steps:

[0071] S1: Provide a convex polyhedron model, the coordinates of the test point P, and the values of each parameter. The model of the convex polyhedron represented in the form of faces is shown in the following formula (1), where the i-th row inequality in the formula corresponds to the constraint formed by the i-th face of the convex polyhedron.

[0072]

[0073] Among them, x, y, and z are independent variables, A i , B i , C i (i = 1, 2,..., n f ) are coefficients, and their subscripts correspond to the numbers of each face of the convex polyhedron, and n f is the number of faces of the convex polyhedron.

[0074] Or in vertex form:

[0075]

[0076] Among them, λ i (i = 1, 2,..., n v ) are independent variables, P i (i = 1, 2,..., n v) are the vertices of the convex polyhedron, n v is the number of vertices of the convex polyhedron, O is the origin of the coordinate system. The coordinates of the test point P are (x P ,y P ,z P ). Parameters include distance detection threshold D o And the artificial potential function coefficient α. The above-mentioned surface form model and point form model can be transformed into each other. Distance detection threshold D o It refers to the distance detection threshold from the point to the convex polyhedron surface. Specifically, when the distance from the point to the convex polyhedron surface is less than D o When , the potential function and potential field values ​​are not 0.

[0077] S2: Calculate the convex polyhedron model represented by faces and vertices. That is, given the face representation (1) or vertex representation (2) of the convex polyhedron model, calculate another representation of the convex polyhedron as follows:

[0078] (1) Given a convex polyhedron model represented by faces, select any three inequalities (representing three faces) from the inequality group (1), change their inequality signs to equal signs, and then solve them jointly. If there is no solution, it means that two of the three selected faces are parallel, which means that these three faces cannot intersect to obtain the convex polyhedron vertex, so the group of inequality combinations is discarded and other combinations are tried; if there is a solution, verify whether the solution satisfies the other inequalities in the inequality group (1). If it does, it is a vertex of the convex polyhedron. If it does not, the solution is discarded. This continues until all combinations of any three inequalities in the inequality group (1) are traversed. Then, using the solved convex polyhedron vertex coordinates, according to formula (2), the convex polyhedron model is rewritten into vertex form.

[0079] (2) Given a convex polyhedron model represented by vertices, from {P i |i=1,2,...,n v} Take any three vertices The plane constraint equation is constructed as follows

[0080]

[0081] Where k is any positive real number, and then substitute the coordinates of other points. If the coordinates of these points can make the above inequality hold, then the above equation is a face constraint of the convex polyhedron; if the coordinates of these points can make the left side of the inequality sign of the above inequality greater than or equal to 0, then swap any two subscripts in i1, i2, i3, and re-substitute them into the above inequality to obtain a face constraint of the convex polyhedron; if the coordinates of these points cannot make the above inequality hold or not hold at the same time, then re-substitute them from {P i |i=1,2,...,n v} Take any 3 vertices until {P i |i = 1, 2,..., n v} until all combinations of any 3 vertices are traversed.

[0082] Through this step, for any convex polyhedron model, its face representation form and vertex representation form are obtained simultaneously.

[0083] S3: Based on the convex polyhedron model represented by faces and vertices obtained in step 2, analyze the connection relationships between the faces and vertices, vertices, and between the faces of the convex polyhedron. Specifically, the following 3 matrices need to be calculated:

[0084] (1) Face - vertex incidence matrix Used to characterize the connection relationship between each face and each vertex of the convex polyhedron. That is, if the vertex P j (corresponding to the j - th point in the vertex representation form in S2, the same below, j = 1, 2,..., n v , n v is the number of vertices) is located on the face F i (corresponding to the i - th face in the face representation form in S2, also corresponding to the i - th inequality in the inequality group (1), the same below, i = 1, 2,..., n f ) of the model represented by the face form of the convex polyhedron (that is, the coordinates of the point P j satisfy the i - th inequality in the inequality group (1) and the equal sign holds), then Otherwise

[0085] (2) Vertex adjacency matrix Used to characterize the adjacency relationship between the vertices of the convex polyhedron. That is, if the vertex P i and the vertex P j (j = 1, 2,..., n v , n v is the number of vertices) are simultaneously located on two faces of the convex polyhedron (that is, at both ends of the same edge of the convex polyhedron, that is, the coordinates of the point P i and the point P j simultaneously satisfy two inequalities in the inequality group (1) and the equal sign holds), then the vertex P i is adjacent to the vertex P j , corresponding to Otherwise Each vertex is not adjacent to itself.

[0086] (3) Face adjacency matrix Used to characterize the adjacency relationship between the faces of the convex polyhedron. That is, if the face F i and the face F j (i, j = 1, 2,..., n f) If there is a common edge (where two faces meet at two vertices, that is, two vertex coordinates in the vertex representation in S2 simultaneously satisfy two of the inequalities in the inequality group (1) and the equal sign holds), then the face F is called i adjacent to the face F j , corresponding to Otherwise, the face F is called i non - adjacent to the face F j , corresponding to Each face is not adjacent to itself.

[0087] S4: Determine the orientation of the test point P relative to the convex polyhedron. It includes four cases: inside the convex polyhedron (including the boundary), above the face of the convex polyhedron, above the edge of the convex polyhedron, and above the vertex of the convex polyhedron. Specifically, it is divided into the following steps:

[0088] (1) If the coordinates of point P satisfy the constraints of the inequality group (1), then point P is located inside (including the boundary) the given convex polyhedron, and its potential function value is taken as +∞. Otherwise, if the coordinates of point P do not satisfy some of the inequalities in the inequality group (1), then the following steps are carried out.

[0089] (2) Reverse the inequality signs in the convex polyhedron model (1) to obtain the following inequality group:

[0090]

[0091] Starting from i = 1, successively determine whether the test point P satisfies the i - th inequality in the inequality group (4). If it is detected that point P satisfies the inequality i (i.e., is located outside the convex polyhedron relative to the plane F i , i = 1, 2,..., n f ), then calculate the foot of the perpendicular i from point P to the plane F according to the following formula

[0092]

[0093] and determine whether H fi is located inside each adjacent face F i of the face F j (the face that makes hold in S3, the same below), that is, determine whether H fi satisfies the constraints of the face j in the inequality group (1) that makes hold in S3:

[0094]

[0095] Since the convex polyhedron can be represented as the solution set of the inequality group (1), the above constraints are for the non - adjacent faces of the face F i (the face that makes The established plane (the same below) also holds, and since H fi is in the plane F i above, the above constraints can be simplified into the following form

[0096]

[0097] If it is satisfied, the foot of the perpendicular H i of point P in the plane F fi falls within the face F i of the convex polyhedron, and it is said that point P is above the face F i of the convex polyhedron (briefly said to be above the face of the convex polyhedron). If it is not satisfied, go to the following steps.

[0098] (1) Assume that the first unsatisfied constraint in formula (6) is inequality j (based on the coefficient subscript), then judge in the same way whether the foot of the perpendicular i of point P to the adjacent face F j falls within the face F of the convex polyhedron. If it is satisfied, it is said that point P is above the face F j of the convex polyhedron (briefly said to be above the face of the convex polyhedron). Otherwise, under the conditions that point P satisfies the inequality i constraint in (4), H j does not fall within the face F fi of the convex polyhedron, and H i does not fall within the face F fj of the convex polyhedron, go to the following steps. j

[0099] (2) Consider the common edge of face F i and face F j (the vertex P k of the convex polyhedron corresponding to the endpoints of which has a subscript k that is an integer for which S3 holds). Assume that the vertices of this edge are P1(x1, y1, z1) and P2(x2, y2, z2), and the two points are the common vertices of the face F of the convex polyhedron and the face F i and the face F j . P i (i ∈ {1, 2,..., n v}) is the spatial vector corresponding to point P i . And and have the same direction, where n i = (A i , B i , C i ) T and n j = (A j , B j , C j )​T are the normal vectors of plane F i and plane F j respectively. Then the following four planes can be constructed: Using P1, P2 and the normal vector to construct side surface S i , using P1, P2 and the normal vector to construct side surface S j , using P1 and the normal vector to construct end surface E1, and using P2 and the normal vector to construct end surface E2. If point P falls within the region enclosed by the above four planes, that is, it satisfies the following constraints:

[0100]

[0101] Or equivalently rewritten as

[0102]

[0103] where H fi and H fj are the feet of the perpendiculars from point P to plane F i and plane F j respectively. Then it is said that point P is above the edge P1P2 of the convex polyhedron (abbreviated as above the edge of the convex polyhedron, i, j ∈ {1, 2,..., n f} and i ≠ j). Otherwise, when point P does not satisfy the constraint of end surface E k , go to the following steps.

[0104] Consider the vertex P of the convex polyhedron on end surface E k (i.e., P1 or P2 in the above steps). Assume that the vertices adjacent to vertex P k are respectively vertex k (i.e., the points that form edges with these points), that is, the vertices that make hold in S3, P (i ∈ {1, 2,..., n i} has the same meaning as above, and x is the spatial vector corresponding to point P. Then judge whether point P satisfies the following constraints v}

[0105]

[0106] If the above constraints are satisfied, then it is said that point P is above vertex P of the convex polyhedron k (abbreviated as above the vertex of the convex polyhedron), and calculate the distance from point P to vertex P k according to the following formula

[0107]

[0108] where (xk , y k , z k ) are the coordinates of vertex P k . If the above constraints are still not satisfied, then go to the following steps.

[0109] (3) Return to step (2), let j ← j + 1, and continue to determine whether the test point P is above the other adjacent face F i of the plane F, or above its common edge (or its endpoints) with the plane F j . If neither is satisfied, then go to the following steps. i

[0110] (4) Return to step (2), let i ← i + 1, and continue to determine whether the test point P satisfies each subsequent inequality in the inequality group (4) until it is found that the point P is above a certain face, a certain edge, or a certain vertex of the convex polyhedron.

[0111] S5: According to the above different orientations of the point P relative to the polyhedron, give the formula for the distance d from the point P to the convex polyhedron and the gradient of this distance at the point P according to the following formula

[0112] (1) If the point P is inside (including the boundary) of the given convex polyhedron, then d = 0, not defined;

[0113] (2) If the point P is above the face F i of the given convex polyhedron, then

[0114]

[0115]

[0116] (3) If the point P is above the edge P1P2 of the given convex polyhedron, then

[0117]

[0118]

[0119] where (x i , y i , z i )(i ∈ {1, 2,..., n v}) are the coordinates of the point P i .

[0120] (1) If the point P is above the vertex P k of the given convex polyhedron, then

[0121] ​

[0122]

[0123] Specifically, since in formulas (13), (15), and (17), both have a magnitude of 1 and the directions are all along the direction from the nearest neighbor point P of point P on the convex polyhedron Nearest to point P, therefore, when the coordinates of point P Nearest are known or can be calculated, in the case of replacing the coordinates of point P k (x k , y k , z k ) with the coordinates of point P Nearest (x Nearest , y Nearest , z Nearest ), the calculation of

[0124] S6: According to the distance d from point P to the polyhedron, calculate the value V of the artificial potential function of the convex polyhedron at point P P , and the specific calculation formula is:

[0125]

[0126] where D o is the distance detection threshold, α is the artificial potential function coefficient, and d is the distance from point P to the convex polyhedron obtained in S5.

[0127] S7: According to the following formula, calculate the gradient force F corresponding to the artificial potential field of the convex polyhedron at point P P :

[0128]

[0129] where D o is the distance detection threshold, d and are respectively the distance from point P to the convex polyhedron obtained in S5, and the gradient of this distance value at point P.

[0130] Embodiment

[0131] The following lists a specific embodiment to illustrate the specific calculation process of the method for constructing the artificial potential field and gradient force of the convex polyhedron according to the present invention. It should be understood that the embodiments described below are only some embodiments of the present invention, rather than all embodiments. All other embodiments obtained by those skilled in the art and related fields based on the embodiments of the present invention without creative efforts shall fall within the protection scope of the present invention.

[0132] Take a regular tetrahedron as an example. Using the method described in the present invention, the artificial potential function and its gradient distribution around it can be given, and for any test point P in space, the artificial potential function and the numerical value of its gradient force at this point can be analytically calculated. The specific calculation steps are as follows:

[0133] S1: Give the convex polyhedron model, the coordinates of the test point, and the values of each parameter. Assuming that the edge length of the regular tetrahedron is a, its vertices can be taken as The coordinates of the test point P are taken as The collision detection distance threshold D o is taken as a. The coefficient α of the artificial potential function is taken as 1. The distribution of the vertices, faces, and the test point of the convex polyhedron is as shown in the appendix Figure 2 where P i (i = 1, 2,..., n v , n v is the number of vertices of the convex polyhedron) represents the i-th vertex of the convex polyhedron, and F i (i = 1, 2,..., n f , n f is the number of faces of the convex polyhedron) represents the i-th face of the convex polyhedron.

[0134] S2: Respectively give the forms of the convex polyhedron represented by vertices and faces. For a regular tetrahedron with vertices P1 - P4, only the constraints formed by its faces need to be calculated. It can be represented by the following inequality group:

[0135]

[0136] S3: Establish the connection relationships between the faces and vertices, between vertices, and between faces of the convex polyhedron. The following three matrices are obtained:

[0137] (1) The face - vertex incidence matrix M f-v :

[0138] (2) The vertex adjacency matrix M v :

[0139] (3) The face adjacency matrix M f :

[0140] S4: Determine the orientation of the test point P relative to the polyhedron. Specifically, it is divided into the following steps:

[0141] (1) Determine whether point P is inside the convex polyhedron. Since the coordinates of point P do not satisfy the 2nd and 4th inequalities in the inequality group (20) (i.e., point P is on the outer side of plane F2 and plane F4 relative to the convex polyhedron), point P is not inside the convex polyhedron (including the boundary), and the potential function value at this point is not +∞.

[0142] (2) Reverse the signs of the inequality group (20) to obtain the following inequality group

[0143]

[0144] Since the coordinates of point P satisfy the 2nd inequality in the above inequality group, calculate the coordinates of the foot of the perpendicular H of point P in plane F2, which are approximately (0.381282, 0.424204, 0.946404)a. However, it is found that the coordinates of point H do not satisfy the 4th inequality constraint in the inequality group (20). Therefore, point P is not above the plane. Then calculate the feet of the perpendiculars of point P in other planes, and the results are listed as follows:

[0145] Table 1 Feet of the perpendiculars of point P in each plane of the convex polyhedron and the convex polyhedron face constraints they do not satisfy

[0146] Plane number Foot of a perpendicular Inequality number not satisfied in (20) 1 (0.25,0.5,-0.204124)a 4 2 (0.381282,0.424204,0.946404)a 4 3 (0.25,0.177375,1.11407)a 2,4 4 (-0.214616,0.231754,0.810321)a 2

[0147] So point P is not above any plane.

[0148] (3) Consider the common edge of face F4 and face F2. It has vertices P2 and P4 as endpoints. Determine whether point P satisfies the end-face constraint of this edge according to the following formula

[0149]

[0150] It is found that the coordinates of point P satisfy the constraint corresponding to end-face E2, but do not satisfy the constraint corresponding to end-face E4. Therefore, the point is not above the edge above

[0151] (4) Consider vertex P4. Its adjacent vertices are P1, P2, and P3. Determine whether point P satisfies the end-face constraint of the edge at point P4

[0152]

[0153] It is found that the coordinates of point P satisfy the above inequality group constraints. Therefore, point P is in the region above vertex P4 of the convex polyhedron. The determination of the orientation of point P is completed.

[0154] If the result of the previous step is that point P is not above vertex P4 of the convex polyhedron, then return to step (2) to determine whether the coordinates of point P satisfy the other inequalities in inequality group (20) until it is determined that point P is above a certain face, edge, or vertex of the convex polyhedron.

[0155] S5: According to formula (16), calculate the distance between point P and point P4 That is, the distance d from point P to the convex polyhedron is

[0156]

[0157] And according to formula (17), calculate the gradient of the above distance value at point P

[0158]

[0159] S6: Calculate the artificial potential function value V of the convex polyhedron at point P P . Since the distance value d from point P to the convex polyhedron obtained in S5 satisfies d < D o , thus, according to the first equation in formula (18), the artificial potential function value at point P can be obtained as

[0160]

[0161] To visually display the distribution of the artificial potential function values constructed by the described method around the regular tetrahedron, we can also draw the distribution cloud map of the artificial potential function of this regular tetrahedron according to formula (18) under the constraints shown in formula (6) (above the face), formula (9) (above the edge), and formula (10) (above the vertex), as shown in the appendix Figure 3 shown, and draw the contour map of the artificial potential function value of 1 of this regular tetrahedron, as shown in the appendix Figure 4 shown. Where the side length a of the regular tetrahedron is taken as 3.

[0162] S7: Calculate the artificial potential field gradient force F of the convex polyhedron at point P P . Since the distance value d from point P to the convex polyhedron obtained in S5 satisfies d < D o , thus, according to the first equation in formula (19), we get

[0163]

[0164] The above are only the preferred embodiments of the present invention and are not intended to limit the present invention. Any modifications, equivalent replacements, improvements, etc. made within the spirit and principles of the present invention shall be included within the protection scope of the present invention.

Claims

1. A continuous analytical construction method for a convex polyhedron artificial potential field and gradient force, characterized in that It includes the following steps: Step 1, establish a convex polyhedron model, test point coordinates, and parameter values; the convex polyhedron model is represented in the form of faces or in the form of vertices; Step 2, transform the convex polyhedron model represented in the form of faces to obtain a convex polyhedron model represented in the form of vertices, or transform the convex polyhedron model represented in the form of vertices to obtain a convex polyhedron model represented in the form of faces; Step 3, determine the connection relationships between each face and vertex, the connection relationships between vertices, and the connection relationships between each face in the convex polyhedron through the convex polyhedron model represented in the form of faces and the convex polyhedron model represented in the form of vertices; Step 4, based on the convex polyhedron model represented in the form of faces and the convex polyhedron model represented in the form of vertices, combined with the connection relationships between each face and vertex, the connection relationships between vertices, and the connection relationships between each face in the convex polyhedron, judge the orientation of the test point relative to the convex polyhedron; The orientation of the test point relative to the convex polyhedron includes four forms: inside the convex polyhedron with boundaries, above the face of the convex polyhedron, above the edge of the convex polyhedron, and above the vertex of the convex polyhedron; Step 5, according to the orientation form of the test point relative to the convex polyhedron, analytically calculate the distance between the test point and the convex polyhedron, and the gradient of the distance at the test point; In step 5, according to the orientation form of the test point relative to the convex polyhedron, the gradient of the distance at the test point is analytically calculated The process is as follows: When the test point is inside the convex polyhedron containing the boundary, It is not defined; (2) Test point When it is above the face of the convex polyhedron the gradient is calculated by the formula: (13) (3) Test point Located on the face of a convex polyhedron With the face Common edge When above, the gradient The calculation formula is: (15) Among them, ( ) is the coordinate of point . (4) Test point When it is above the vertex of the convex polyhedron the gradient is calculated by the formula: (17) Step 6, calculate the artificial potential function of the convex polyhedron at the test point based on the distance between the test point and the convex polyhedron; calculate the gradient force of the convex polyhedron at the test point based on the distance between the test point and the convex polyhedron and the gradient of the distance at the test point; In Step 6, the calculation formula for the gradient force of the convex polyhedron at the test point is: (19) in, is the distance detection threshold, and Test points The distance to the convex polyhedron, and the distance at the point The gradient at .

2. The continuous analytical construction method of a convex polyhedron artificial potential field and gradient force according to claim 1, characterized in that, In Step 1, the convex polyhedron model represented in the form of faces is: (1) Among them, the subscript of each inequality is the number of each face in the convex polyhedron, which is the number of faces of the convex polyhedron; The convex polyhedron model represented in the form of vertices is: (2) Among them, are the vertices of the convex polyhedron respectively, is the number of vertices of the convex polyhedron, is the origin of coordinates.

3. A continuous analytical construction method of a convex polyhedron artificial potential field and gradient force according to claim 1, characterized in that In Step 2, during the process of transforming the convex polyhedron model represented in the form of faces to obtain a convex polyhedron model represented in the form of vertices, select any 3 inequalities from the convex polyhedron model represented in the form of faces, transform them into equalities, and then solve; If there is no solution, then 2 of the 3 faces are parallel; if there is a solution, verify whether the solution satisfies other inequalities in the convex polyhedron model. If it satisfies, it is a vertex of the convex polyhedron; if it does not satisfy other inequalities, discard the solution. After traversing all inequalities, rewrite the convex polyhedron model represented in the form of faces as a convex polyhedron model represented in the form of vertices; During the process of transforming the convex polyhedron model represented in the form of vertices to obtain a convex polyhedron model represented in the form of faces, construct a plane constraint equation from any three vertices in the convex polyhedron model represented in the form of vertices, substitute the coordinates of other points. If the substituted point coordinates make the plane constraint equation hold, it is a face constraint of the convex polyhedron; if the substituted point coordinates make the plane constraint equation not hold, substitute other points again to obtain a face constraint of the convex polyhedron until all three vertices are traversed, and obtain the face constraints of each face of the convex polyhedron to obtain a convex polyhedron model represented in the form of faces.

4. A continuous analytical construction method of a convex polyhedron artificial potential field and gradient force according to claim 1, characterized in that In Step 3, the process of determining the connection relationship is: (1) The incidence matrix of each face and vertex in the convex polyhedron is ; If vertex ( , is the number of vertices) is located on face ( ), then , otherwise ; (2) The adjacency matrix between the vertices is , if vertex and vertex are both located on two faces of the polyhedron at the same time, then vertex is said to be adjacent to vertex , corresponding to , otherwise there is ; (3) The adjacency matrix between each face is , if face and face have two common vertices, then it is said that face and face are adjacent, corresponding to , otherwise there is .

5. A continuous analytical construction method of a convex polyhedron artificial potential field and gradient force according to claim 1, characterized in that In Step 4, the process of judging the orientation of the test point relative to the convex polyhedron is: (1) When the test point is located inside the convex polyhedron with boundaries, the test point satisfies the constraint of the following formula (1), (1) where the subscript corresponds to the number of each face of the convex polyhedron, is the number of faces of the convex polyhedron; (2) When the test point is above the face of the convex polyhedron, the foot of the perpendicular of the test point on the plane satisfies the constraints formed by the adjacent planes in Equation (7) of each adjacent plane ( , , ) (7) where the foot of the perpendicular has its coordinates obtained by the following formula (5) (5) (3) The test point is located on the face of the convex polyhedron When it is above the common edge of the face The constraint condition is as follows: (9) Among them, the point The definition and calculation method are as shown in Equation (5). The point and are the common vertices of the faces and of the convex polyhedron. ( ) is the space vector corresponding to the point . (4) The test point is located at the vertex of the convex polyhedron The constraint condition when it is above is: (10) Among them, the point is the adjacent vertex of the point ( ), that is, the vertex that satisfies . The meaning of ( ) is the same as above. is the spatial vector corresponding to the point .

6. A continuous analytical construction method of a convex polyhedron artificial potential field and gradient force according to claim 1, characterized in that In step 5, according to the orientation form of the test point relative to the convex polyhedron, the distance between the test point and the convex polyhedron is analytically calculated. d The process is as follows: (1) When the test point is inside the convex polyhedron with boundaries, ; (2) Test point When it is above the face of the convex polyhedron The distance d is calculated by the formula: (12) (3) Test point Located on the face of a convex polyhedron With the face Of the common edge When above, the distance d The calculation formula is: (14) (4) Test point When it is located at the vertex of a convex polyhedron above, the distance d is calculated by the formula: (16)。 7. A continuous analytical construction method of a convex polyhedron artificial potential field and gradient force according to claim 1, characterized in that, In step 6, the calculation formula of the artificial potential function is as follows: (18) Among them, is the distance detection threshold, is the artificial potential function coefficient, is the test point to the distance of the convex polyhedron.

Images