A method for constructing a continuous potential field for an assembly that adapts to real shapes

Abstract

The present invention discloses a method for constructing a continuous potential avoidance field for a combination that adapts to real shapes. The method comprises the following steps: splitting a combination obstacle into a number of basic geometric units, constructing an artificial potential field for the combination obstacle based on the distance from a given point to each basic geometric unit and the calculation of the nearest neighbor point, analytically calculating the artificial potential function and the artificial potential field value of the geometric unit at a given point, and then constructing an analytical form of the combined artificial potential field. The present invention can construct an artificial potential field for irregular obstacles based on local cognitive information about the obstacle. And because the present invention only relies on the distance from a given point to each basic geometric unit and the nearest neighbor point information to solve the artificial potential field, it is easy to execute in an actual obstacle environment. The artificial potential function and potential field in the present invention are continuous in areas other than the interior of the combination, eliminating the motion jitter phenomenon of individuals at the junction of different areas outside the combination, and being able to achieve a relatively stable obstacle avoidance motion control effect.

Description

Technical Field

[0001] The invention relates to the field of robot / intelligent body motion control, and relates to a method for constructing a continuous potential avoidance field for a combined body that adapts to a real shape. Background Art

[0002] When applying control theory to the motion control of real objects such as unmanned vehicles, drones or spacecraft, it is necessary to consider obstacle avoidance in the control law to ensure the safe operation of individuals due to the complex obstacle constraints in the real environment. At the planning level, this can be achieved by making the individual track a path that keeps a sufficient distance from the obstacle; at the control level, this can be achieved by adding appropriate collision avoidance terms based on the original control law of the individual. For obstacles with complex geometric structures, it is inconvenient to construct artificial potential fields for them. Most of the existing obstacle avoidance schemes are solved from the planning perspective, which not only brings challenges to the stability and convergence proof of the closed-loop system, but also is not suitable for avoiding moving obstacles (such as other individuals) or obstacles discovered in real time. Although it is also possible to design collision avoidance forces using the artificial potential field method after using a spherical or ellipsoidal surface to approximate or approach the shape of the obstacle, the control accuracy is relatively limited, which may cause the individual to be too close or too far away 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 problem of complex-shaped obstacles, they either do not meet the potential field conditions (i.e., the collision avoidance force is not the gradient of the designed potential function), lack proof of closed-loop system stability, or have discontinuities, which can easily cause chattering at the boundaries of the area. Summary of the invention

[0003] The present invention aims to provide a method for constructing a continuous potential field-avoiding assembly that is adaptable to a real shape, so as to overcome the problems existing in the prior art.

[0004] To achieve the above object, the present invention provides the following technical solutions:

[0005] A method for constructing a continuous potential avoidance field for a combination body adapted to a real shape comprises:

[0006] S1: Obtain the combined obstacle model, the coordinates of the test points, the collision avoidance detection distance threshold and the artificial potential function coefficient;

[0007] S2: Decompose the combined obstacle model in S1 into several basic geometric units;

[0008] S3: Determine whether part of the geometric units in S2 can form a relatively independent substructure. If part of the geometric units in S2 can form a relatively independent substructure, the geometric units are divided into several bounding boxes according to the substructures they belong to. If any part of the geometric units in S2 cannot form a relatively independent substructure, all the geometric units are regarded as a substructure as a whole and covered with a bounding box.

[0009] S4: construct a distance formula from the test point to each basic geometric unit based on the size, shape and position of the basic geometric unit obtained in S2;

[0010] S5: using the distance formula from the test point to the basic geometric unit constructed in S4, and according to the collision avoidance detection distance threshold and the artificial potential function coefficient obtained in S1, construct a potential function formula for each basic geometric unit;

[0011] S6: construct the potential field formula of each basic geometric unit by calculating the negative gradient of the potential function formula constructed in S5;

[0012] S7: Determine the bounding box to which the test point belongs, and return the union of the geometric unit sets in these bounding boxes;

[0013] S8: Use the potential function formula in S5 and the potential field formula in S6 respectively to calculate the artificial potential function and artificial potential field of each geometric unit in the union of the geometric unit sets returned in S7 at the test point, and superimpose them respectively to obtain the artificial potential function and artificial potential field of the combined obstacle model at the test point.

[0014] Preferably, the formula for decomposing the combined obstacle model into several basic geometric units is:

[0015]

[0016] Among them, B is the combined obstacle model, B1, B2,…, It is the basic geometric unit.

[0017] Preferably, the basic geometric unit is a convex basic geometric unit.

[0018] Preferably, the method of constructing the bounding box in S3 is:

[0019] E i =[x mini ,x maxi ]×[y mini ,y maxi ]×[z mini ,z maxi ]:

[0020]

[0021] Where D o is the collision avoidance detection distance threshold, E i is the bounding box.

[0022] Preferably, the distance from the bounding box surface to each basic geometric unit inside the bounding box is greater than or equal to a collision avoidance detection distance threshold.

[0023] Preferably, for the distance formula from the test point to the basic geometric unit in S4, when it is a simple geometric body with a complex surface, the following formula is used for calculation, otherwise it is explicitly given:

[0024]

[0025] Among them, P is the test point, B i is the basic geometric unit, (x P ,y P ,z P ) is the distance from the test point P to the basic geometric unit, From the test point P to the basic geometric unit B i The gradient of Nearesti For the test point P in the basic geometric unit B i The nearest neighbor point on (x i ,y i ,z i ) is P Nearesti The coordinates of .

[0026] Preferably, the basic geometric unit B in S5 i The potential function form is

[0027]

[0028] Where D o is the collision avoidance detection distance threshold, α is the artificial potential function coefficient, d i This is the distance formula from the test point to each basic geometric unit.

[0029] Preferably, the basic geometric unit B in S6 i The potential field form is

[0030]

[0031] Where D o is the collision avoidance detection distance threshold, d i is the distance from the test point to each basic geometric unit, From point P to B i The distance d i The gradient at point P, α is the coefficient of the artificial potential function.

[0032] Preferably, the potential function of the assembly in S8 at the test point is in the form of

[0033]

[0034] Where S is the union of the geometric unit sets returned in S7, n b is the number of basic geometric units, V i is the potential function form of the basic geometric unit in S5, B i It is the basic geometric unit.

[0035] Preferably, the potential field of the assembly constructed in S8 at the test point is in the form of

[0036]

[0037] Where S is the union of the geometric unit sets returned in S7, n b is the number of basic geometric units, B i is the basic geometric unit, F i is the potential field form of the basic geometric unit.

[0038] Compared with the prior art, the present invention has the following beneficial effects: the present invention provides a method for constructing a continuous potential field for a combination that is adapted to a real shape, that is, by splitting the combination obstacle into several basic geometric units, and constructing the artificial potential field of the combination obstacle based on the distance from a given point to each basic geometric unit and the calculation of the nearest neighbor point, which is applicable to a wider range of combination and basic geometric unit structure types. In the case where the distance from the basic geometric unit to the given point has an analytical expression, the present invention can analytically calculate the artificial potential function and the artificial potential field value of the geometric unit at the given point, and then construct an analytical form of the combined artificial potential field. Compared with the existing obstacle artificial potential field construction method, the present invention can construct an artificial potential field for irregular obstacles based on the local cognitive information of the obstacle, which is applicable to the robot's fine obstacle avoidance motion control in a real obstacle environment. And because the present invention only relies on the distance from the given point to each basic geometric unit and the nearest neighbor point information to perform artificial potential field solution, there is no need to obtain the difficult-to-measure obstacle center position and velocity information, so it is easy to execute in an actual obstacle environment. At the same time, the artificial potential function and potential field in the present invention are continuous in the area except the interior of the assembly, eliminating the motion jitter phenomenon of individuals at the junction of different areas outside the assembly, and achieving a relatively stable obstacle avoidance motion control effect. BRIEF DESCRIPTION OF THE DRAWINGS

[0039] Figure 1 A flow chart for constructing an artificial potential function of an obstacle of the combination of the present invention;

[0040] Figure 2 A simplified structural diagram of the Chinese space station considered in the specific embodiment;

[0041] Figure 3 It is a distribution diagram of the substructure bounding box and test point positions of the Chinese space station in a specific embodiment;

[0042] Figure 4 It is a potential function distribution cloud diagram near the simplified structure of the Chinese space station in a specific embodiment. DETAILED DESCRIPTION

[0043] The present invention is further described in detail below in conjunction with specific embodiments, which are intended to explain the present invention rather than to limit it.

[0044] The purpose of the present invention is to provide a method for constructing a continuous avoidance potential field for an assembly that adapts to real shapes, so as to achieve refined motion control of the assembly near obstacles, thereby meeting the obstacle avoidance requirements in real application scenarios such as spacecraft companion flight control near space stations.

[0045] In order to solve the above problems, the technical solution adopted by the present invention is as follows:

[0046] like Figure 1 As shown in the figure, a method for constructing the artificial potential field of a combined obstacle is to calculate the given point P(x P ,y P ,z P ), including the following steps:

[0047] S1: Given the combined obstacle model, the coordinates of the test point P and the values ​​of various parameters, including the collision avoidance detection distance threshold D o , and the coefficient value α of the artificial potential function.

[0048] S2: Express the combination B as n b convex basic geometric units B1, B2, ..., (If the geometric unit is not convex, the decomposition should continue until the basic geometric unit is convex), that is,

[0049]

[0050] And there is no inclusion relationship between these geometric units.

[0051] Since the above basic geometric units may have a subset The geometric bodies intersect with each other, and the intersecting parts can be merged into any basic geometric unit in S, or belong to multiple basic geometric units at the same time, so the geometric unit decomposition shown in formula (1) is not unique. However, the different artificial potential fields obtained do not hinder the realization of the final collision avoidance effect.

[0052] S3: If part of the above geometric units can form a relatively independent substructure, the above geometric units are divided into several bounding boxes according to the "substructure". Together they form a substructure, then construct a bounding box E that satisfies the following conditions i =[x mini ,x maxi ]×[y mini ,y maxi ]×[z mini ,z maxi ]:

[0053]

[0054] (where D o is the collision avoidance detection distance threshold), and constructs the following subinterval

[0055] E′ i =[x mini -D o ,x maxi +D o ]×[y mini -D o ,y maxi +D o ]×[z mini -D o ,z maxi +D o ] (3)

[0056] Let the subset S i Each basic geometric unit in belongs to the subinterval E i ' (i.e. the basic geometric unit together with its collision detection area is within the interval E i Inside).

[0057] In particular, if any part of the geometric units in S2 cannot constitute a relatively independent substructure, all geometric units are regarded as a substructure as a whole, and all geometric units are covered by a bounding box.

[0058] S4: Give the distance formula from point P to each basic geometric unit. Specifically, according to the representation form of the basic geometric unit model, it is divided into the following three forms:

[0059] (1) If the basic geometric unit B i The distance to a given point has an analytical expression d i (x,y,z), then point P to B i The distance can be directly expressed by the formula d i (x P ,y P,z P );

[0060] (2) If the basic geometric unit B i The surface equation f i (x, y, z) = 0, but there is no analytical expression for the distance to a given point. Then, by solving the following optimization problem about (x, y, z) and parameter λ, we can find the distance between point P and the basic geometric unit B. i The nearest neighbor point P on Nearesti The coordinates (x i ,y i ,z i ):

[0061]

[0062] The solution to the following system of equations

[0063]

[0064] So we can move point P to B i The distance is expressed as

[0065]

[0066] (3) If the basic geometric unit B i The surface cannot be expressed analytically by equations. Therefore, when calculating the value of the artificial potential function at point P, a heuristic search method is used to determine the closest point P to point P on the basic geometric unit. Nearesti (x i ,y i ,z i ), and then calculate the distance from point P to B according to formula (6). i distance.

[0067] S5: basic geometric unit B i Construct an artificial potential function of the following form

[0068]

[0069] Where D o is the collision avoidance detection distance threshold, α is the artificial potential function coefficient, d i From point P to B constructed in S4 i The distance formula.

[0070] S6: By the basic geometric unit B i The artificial potential function (7) is used to find the negative gradient, which is the basic geometric unit B i Construct an artificial potential field of the following form

[0071]

[0072] Where D o is the collision avoidance detection distance threshold, d i From point P to B constructed in S4 i The distance formula is: From point P to B i The distance d i The gradient formula of the formula at point P is given by the basic geometric unit B at the known point P. i The nearest neighbor point P on Nearesti The coordinates (x, y, z) can be constructed as follows

[0073]

[0074] S7: Determine the bounding box to which point P belongs, that is, satisfy

[0075] The set of all bounding boxes E = {E j |j∈Λ}, Λ is the subscript set of all elements in E. And return each bounding box E determined in S3 j The union of the basic geometric unit sets in S = ∪ j∈Λ S j , used to calculate the artificial potential field at point P.

[0076] S8: According to formulas (7) and (8), calculate the artificial potential function of the combined obstacle model at point P

[0077]

[0078] And artificial potential field

[0079]

[0080] Where S is the union of the basic geometric unit sets in each bounding box to which point P belongs.

[0081] Example:

[0082] We use the simplified geometry of the Chinese space station Figure 2 Take the example to illustrate the specific implementation method of the present invention.

[0083] S1: Given the combined obstacle model, the coordinates of the test point P and the values ​​of various parameters. The obstacle considered is a simplified model of the Chinese space station, consisting of two capsules (cylinders whose two end faces are replaced by hemispheres of the same radius) and five cuboids. The specific parameters of each geometric body are shown in Table 1. Given the test point P (x P ,y P ,z P )'s specific coordinates (10m, 20m, 5m) such as Figure 3As shown, the collision avoidance detection distance threshold D o =10m, and the coefficient value of the artificial potential function α = 10 -8 J.m 2 .

[0084] S2: The simplified geometric structure of the considered assembly, the Chinese space station, is represented as n b =7 basic geometric units:

[0085] B=B1∪B2∪…∪B7 (13)

[0086] And each basic geometric unit is a convex body (if it is not convex, it should be further decomposed). i is the i-th (i=1,2,...,n b ) geometric bodies, so B1~B2 are capsule bodies, and B3~B7 are cuboids.

[0087]

[0088] Table 1 Basic geometrical components and parameters of the simplified geometric structure of the Chinese space station

[0089] Since the above convex body can be further decomposed into smaller convex bodies (such as a capsule body can be decomposed into a cylinder and two hemispheres, a cuboid can be divided into two cuboids, etc.), the above decomposition is not unique.

[0090] S3: Divide the above geometric units into several bounding boxes according to "substructures". Specifically, we regard capsule B1 and cuboids B3, B4, B7 as a substructure S1, and capsule B2 and cuboids B5, B6 as a substructure S2, so as to construct the bounding box E1 of substructure S1 and the bounding box E2 of substructure S2 as follows: Figure 3 As shown:

[0091] E1=[-12.1m,12.1m]×[-20m,20m]×[-23.2m,37.2m] (14)

[0092] E2=[-18.5m,18.5m]×[-29.75m,29.75m]×[-12.1m,12.1m] (15)

[0093] And construct its subinterval

[0094]

[0095]

[0096] S4: Given a point P(x P ,y P ,zP ) to each basic geometric unit. Specifically, the formula from point P to capsule B1 can be expressed as (where is the center coordinate of capsule B1, h1=109m is the height of the cylinder after removing the two hemispheres of capsule B1, r=2.5m is the radius of the bottom surface of capsule B1)

[0097]

[0098] The formula from point P to capsule B2 can be expressed as (where is the center coordinate of capsule B2, h2=73m is the height of the cylinder after removing the two hemispheres of capsule B2, r=2.5m is the radius of the bottom surface of capsule B2)

[0099]

[0100] The formula from point to cuboid B3~B7 can be uniformly expressed as (where (x C ,y C ,z C ) are the center coordinates of the cuboid, a, b, c are the lengths of the edges of the cuboid parallel to the x, y, z axes respectively)

[0101]

[0102] S5: basic geometric unit B i (i=1,2,...,n b ) constructs an artificial potential function of the following form

[0103]

[0104] Where D o is the collision avoidance detection distance threshold, α is the artificial potential function coefficient, d i From point P to B constructed in S4 i The distance formula.

[0105] S6: By the basic geometric unit B i The artificial potential function (21) is used to find the negative gradient, which is the basic geometric unit B i Construct an artificial potential field of the following form

[0106]

[0107] Where D o is the collision avoidance detection distance threshold, d i From point P to B constructed in S4 i The distance formula is: From point P to B i The distance d iThe gradient of the formula at point P is obtained by calculating the gradient of point P through formulas (18) to (20), where

[0108]

[0109]

[0110]

[0111] S7: Determine the bounding box to which point P belongs. From the specific coordinates of point P (10m, 20m, 5m) and the definitions of each bounding box (14) to (15), it can be seen that point P is located in both bounding boxes E1 and E2. And return the set of basic geometric units corresponding to the two bounding boxes, that is, the set of all basic geometric units of the combination {B1, B2, ..., B7}.

[0112] However, for the bounding box E1, point P is located on its boundary, and the sum of the artificial potential functions of the geometric units in the bounding box E1 is at most 0. Therefore, as a simplified processing, the geometric units in the bounding box E1 can be ignored, and only the geometric units in the bounding box E2 can be considered.

[0113] S8: Calculate the artificial potential function and artificial potential field of the assembly at point P. First, calculate the distances from point P to each geometric unit according to the formula: d1≈20.2607m, d2≈9.0803m, d3≈22.8837m, d4≈20.0714m, d5≈37.0607m, d6≈10.8394m, d7≈14.1068m. Only d2 is less than the collision avoidance detection distance threshold D o , that is, except for the basic geometric unit B2, other geometric units have no effect at point P. Therefore, when calculating the artificial potential function and artificial potential field at point P, only the basic geometric unit B2 needs to be considered.

[0114] Therefore, the value of the artificial potential function at point P can be obtained

[0115]

[0116] And the artificial potential field value

[0117]

[0118] In order to intuitively display the potential function distribution near the assembly, we randomly sampled near the assembly and drew a potential function distribution cloud map near the assembly, such as Figure 4 shown.

[0119] Although the embodiments of the present invention are described above in conjunction with the accompanying drawings, the present invention is not limited to the above-mentioned specific embodiments and application fields, and the above-mentioned specific embodiments are only illustrative and instructive, rather than restrictive. Under the guidance of the specification, a person skilled in the art can also make many forms without departing from the scope of protection of the claims of the present invention, all of which belong to the scope of protection of the present invention.

Claims

1. A method for constructing a continuous potential field avoidance system for a combination body that adapts to a real shape, characterized in that: include: S1: Obtain the combined obstacle model, the coordinates of the test points, the collision avoidance detection distance threshold and the artificial potential function coefficient; S2: Decompose the combined obstacle model in S1 into several basic geometric units; S3: Determine whether part of the geometric units in S2 can form a relatively independent substructure. If part of the geometric units in S2 can form a relatively independent substructure, the geometric units are divided into several bounding boxes according to the substructures they belong to. If any part of the geometric units in S2 cannot form a relatively independent substructure, all the geometric units are regarded as a substructure as a whole and covered with a bounding box. S4: construct a distance formula from the test point to each basic geometric unit based on the size, shape and position of the basic geometric unit obtained in S2; S5: using the distance formula from the test point to the basic geometric unit constructed in S4, and according to the collision avoidance detection distance threshold and the artificial potential function coefficient obtained in S1, construct a potential function formula for each basic geometric unit; S6: construct the potential field formula of each basic geometric unit by calculating the negative gradient of the potential function formula constructed in S5; S7: Determine the bounding box to which the test point belongs, and return the union of the geometric unit sets in these bounding boxes; S8: Use the potential function formula in S5 and the potential field formula in S6 respectively to calculate the artificial potential function and artificial potential field of each geometric unit in the union of the geometric unit sets returned in S7 at the test point, and superimpose them respectively to obtain the artificial potential function and artificial potential field of the combined obstacle model at the test point.

2. The method for constructing a continuous potential field avoidance assembly that adapts to a real shape according to claim 1, characterized in that: The formula for decomposing the combined obstacle model into several basic geometric units is: Among them, B is the combined obstacle model, B1, B2,…, It is the basic geometric unit.

3. The method for constructing a continuous potential field avoidance assembly that adapts to a real shape according to claim 2, characterized in that: The basic geometric unit is a convex basic geometric unit.

4. The method for constructing a continuous potential field avoidance assembly that adapts to a real shape according to claim 1, characterized in that: The method of constructing the bounding box in S3 is: E i =[x mini ,x maxi ]×[y mini ,y maxi ]×[z mini ,z maxi ]: Where D o is the collision avoidance detection distance threshold, E i is the bounding box.

5. The method for constructing a continuous potential field avoidance assembly that adapts to a real shape according to claim 1, characterized in that: The distances from the surface of the bounding box to each basic geometric unit inside the bounding box are greater than or equal to a collision avoidance detection distance threshold.

6. The method for constructing a continuous potential field avoidance assembly that adapts to a real shape according to claim 1, characterized in that: For the distance formula from the test point to the basic geometric unit in S4, when it is a simple geometric body with complex surfaces, the following formula is used for calculation, otherwise it is explicitly given: Among them, P is the test point, B i is the basic geometric unit, (x P ,y P ,z P ) is the distance from the test point P to the basic geometric unit, From the test point P to the basic geometric unit B i The gradient of Nearesti For the test point P in the basic geometric unit B i The nearest neighbor point on (x i ,y i ,z i ) is P Nearesti The coordinates of .

7. The method for constructing a continuous potential field avoidance assembly that adapts to a real shape according to claim 1, characterized in that: Basic geometry unit B in S5 i The potential function form is Where D o is the collision avoidance detection distance threshold, α is the artificial potential function coefficient, d i This is the distance formula from the test point to each basic geometric unit.

8. The method for constructing a continuous potential field avoidance assembly that adapts to a real shape according to claim 1, characterized in that: Basic geometry unit B in S6 i The potential field form is Where D o is the collision avoidance detection distance threshold, d i is the distance from the test point to each basic geometric unit, From point P to B i The distance d i The gradient at point P, α is the coefficient of the artificial potential function.

9. The method for constructing a continuous potential field avoidance assembly that adapts to a real shape according to claim 1, characterized in that: The potential function of the combination in S8 at the test point is: Where S is the union of the geometric unit sets returned in S7, n b is the number of basic geometric units, V i is the potential function form of the basic geometric unit in S5, B i It is the basic geometric unit.

10. The method for constructing a continuous potential field avoidance assembly that adapts to a real shape according to claim 1, characterized in that: The potential field of the assembly constructed in S8 at the test point is: Where S is the union of the geometric unit sets returned in S7, n b is the number of basic geometric units, B i is the basic geometric unit, F i is the potential field form of the basic geometric unit.

Images