Artificial moment method based on basic direction endpoints

Abstract

The invention provides an artificial torque method based on a basic direction endpoint. The method is applied to path planning of an individual robot in an unknown and extremely complex environment. Comprising the following steps: initializing a system; the robot updates knowledge obstacle line segments according to sensor information and sets N-type connection function line segments; determining a current attraction line segment, an attraction point and a key blocking line segment according to the attraction line segment at the last moment and the updating distance thereof; setting and deleting related B-type connection function line segments; obtaining a positive direction line segment set, a negative direction line segment set and a basic direction endpoint set according to the key blocking line segment; if the positive direction line segment set is empty, outputting an unreachable alarm and stopping the algorithm, otherwise, obtaining a new attraction line segment based on the basic direction end point and taking the end point of the new attraction line segment as an attraction point; and the individual robot moves under the guidance of the attraction point at each moment until the task is completed. According to the method, the motion direction which not only can guarantee completeness but also can obtain a short solution path can be obtained with a small amount of calculation.

Description

Technical Field

[0001] This application belongs to the field of robot path planning technology, and in particular relates to an artificial torque method based on basic direction endpoints. Background Technology

[0002] In tasks such as search and rescue in earthquake and disaster environments, autonomous exploration and mapping of the seabed or other complex terrains, and navigation in maze-like areas and battlefield environments, the working environment of robots is usually extremely complex. This complexity manifests in the fact that the environment not only contains many dead ends, but these dead ends are also interconnected with other paths, forming a complex maze-like network. In addition to polygonal obstacles, there are also non-polygonal obstacles in the environment, which may be unknown, in motion, or even make the destination unreachable. For a robot R working in such an environment, one of the key tasks is to find a collision-free and optimal (or suboptimal) path from its initial position to a given destination T, or to determine that T is unreachable. This task is the well-known individual robot path planning problem, a prerequisite for R to complete complex tasks. Individual robot path planning technology also has promising applications in fields such as multi-robot path planning and conflict resolution, simulation and computer-aided design, and biological cell transport.

[0003] Existing algorithms for path planning of individual robots include road map-based algorithms, sampling-based algorithms, heuristic methods, artificial potential field (APF) methods, dynamic window methods (DWAs), bug algorithms, reinforcement learning (RL)-based methods, and artificial torque methods. However, path planning for individual robots in highly complex environments remains a challenging unsolved problem. This is because existing methods either cannot guarantee completeness, yield excessively long solution paths, or incur excessive computational burdens, making them difficult to meet the requirements.

[0004] In artificial torque methods, to ensure completeness and optimize the path, knowledge barrier walls are constructed using detected line segments along obstacle edges (called knowledge barrier segments). Therefore, the solution path is often shorter than that of sampling-based algorithms, APF, DWA, and bug algorithms. Furthermore, the computational burden of artificial torque methods is significantly lower than that of road map-based and RL-based methods. This is because when a straight path from R to T is blocked by an obstacle, artificial torque methods only need to compute a local attraction segment and a local attraction point to guide R's movement, without requiring a training process. Simultaneously, since each attraction segment can typically be reused multiple sampling times, the computational burden of artificial torque methods is further reduced. However, when the robot is escaping a dead-end area with only one exit, the solution path of existing artificial torque methods can become catastrophically long. In the worst case, R will move endlessly in these areas without eventually escaping. This is because, in complex situations, existing artificial torque methods may cause two or more points to alternately become the starting points of attraction segments. Summary of the Invention

[0005] In view of this, the purpose of this application is to provide an artificial torque method based on the endpoints of the basic direction, which can solve the following sub-problems simultaneously in a shorter computation time: determining the reachability of the destination, quickly escaping dead ends, and obtaining the motion direction that can reduce the solution path length and ensure completeness.

[0006] This application provides an artificial torque method based on basic direction endpoints, which is applied to path planning of individual robots in unknown and extremely complex environments. The method includes: S1. Initialize the individual robot R, target position T, working environment, and knowledge barrier wall, and set the current time t. k =t0, let vector M D Let A be a set of zeros where all elements in A are 0. st (-1) and A ed (-1) are all empty, and other parameters are set; S2. Based on the sensor information, update all knowledge barrier segments and set up N-type connection function segments; S3, based on the attraction segment A from the previous moment. st (k-1)A ed (k-1) and its update distance D upd (k-1) Determine the current attracting segment A st A ed Attraction Point P att The key blocking segment BS of T T If possible, also set and delete the relevant B-type connection function segments at the same time; S4, if P attIf empty, then execute S4.1-S4.4; S4.1, based on T and BS T Obtain the set of positive direction line segments KOS P The set of negative direction line segments KOS N and the set of basic direction endpoints BEND; S4.2, if KOS P If empty, output an "unreachable" warning and stop the algorithm; S4.3. Obtain a new attracting line segment A based on the endpoints of the basic direction. st A ed ; S4.4, Let P att =A ed and D upd =min(|P R A st | / c +D S / 2,D V -D S / 2); S5, R in P att Under the guidance of the instrument and the control of the artificial torque motion controller, the movement takes one step to reach the next sampling time t. k+1 ; S6. If R reaches T, stop the algorithm; otherwise, let t k = t k+1 Then, return to S2.

[0007] The artificial torque method based on basic direction endpoints provided in this application can simultaneously solve the following sub-problems with a short computation time: determining the reachability of the destination, quickly escaping dead ends, and obtaining a motion direction that reduces the solution path length while ensuring completeness. The basic direction endpoints are used to guide the robot's motion. This is because the artificial torque method requires that the starting point of the attracting line segment must be a basic direction endpoint, and it also provides a new rule for setting and deleting B-type connecting functional line segments, thus enabling the proposed method to obtain a motion direction that guarantees completeness and provides a shorter solution path with less computation. Attached Figure Description

[0008] Figure 1 The set of positive direction line segments KOS obtained by Algorithm 1 provided in this application embodiment is shown. P The set of negative direction line segments KOS N A schematic diagram of the endpoints of the basic directions; Figure 2 The illustration shows a simulation diagram of calculating the attraction line segment based on the basic direction endpoint and setting the B-type connection function line segment using existing methods, provided in an embodiment of this application. Figure 3 Shows a schematic diagram of the destination unreachable simulation environment provided by an embodiment of the present application; Figure 4 Shows a schematic diagram of the dynamic obstacle simulation environment provided by an embodiment of the present application; Figure 5 Shows a schematic diagram of the complex maze-like simulation environment provided by an embodiment of the present application. Detailed implementation manners

[0009] To make the objectives, technical solutions, and advantages of the present technical solution clearer and more understandable, the present technical solution will be further described in detail below in combination with specific implementation manners. It should be understood that these descriptions are only exemplary and are not intended to limit the scope of the present technical solution.

[0010] Embodiment 1: (1) Introduction to the robot model and related concepts: Please refer to the set of positive direction line segments KOS Figure 1 obtained according to Algorithm 1 as shown P , the set of negative direction line segments KOS N and the schematic diagram of the basic direction endpoints. The thick line segments on the obstacle boundary are knowledge obstacle line segments. Figure 1 In a, KOS<( P ={BA, BC}, KOS N ={AB, CB, CD, DF, EG, GH, HI, IJ, JK, KL, LM}; B is the only positive direction endpoint, and the negative direction endpoints are A, F, and J. Figure 1 )]]In b, KOS P ={BA, BC, CD, DF, EG, GH, HJ, JK, KL, LM, MN}, KOS N ={AB}; B and N are positive direction endpoints, and A is the only negative direction endpoint; the circumferential angles of B and N are | 1| and 2 -| 2| respectively.

[0011] The model of the robot R is a circle centered on P R with a radius of D R and having a basic motion direction line (PMDline), where the PMDline of R is a ray starting from P R . The target position T of R is a point also with a PMDline. At the same time, it is assumed that R knows the position of T and the direction of the PMDline at any time.

[0012] R operates in a two-dimensional (2D) bounded plane, where all non-polygonal obstacles (such as circular obstacles) are approximated by polygonal obstacles. R has no prior knowledge of its operating environment and needs to continuously probe the surroundings using sensors to acquire relevant information. V R is the effective radius of the sensor. Let AB represent the line segment with endpoints A and B, |AB| represent its length, and (A+B) / 2 represent its midpoint. Then P... R B has no intersection with the obstacle and |P R B|≤D V Only then can point B be detected by R. For example, in... Figure 1 In a, only the yellow area and the points on its boundaries can be detected by R at the current moment.

[0013] set up Let KS be a constant integer not less than 3, and let KS be a line segment on the edge of the obstacle. If all points on KS have been detected, and for any KS*... KS is a knowledge obstacle segment if at least one point in KS* is never detected or is not on an obstacle. A knowledge obstacle segment KS is detectable if it has two points A and B that satisfy the following conditions: A and B can both be detected by R at the current time and D... R c |AB| D R .

[0014] Normally, Type B and Type N connection function segments are used as knowledge barrier segments. This application sets up Type N connection function segments using the following method: For two points A and B on knowledge barrier line segments A1A2 and B1B2, AB can be set as an N-type connecting function line segment if the following conditions are met: (1) |AB| is the shortest distance between A1A2 and B1B2 and 0 < |AB| <D S D S It is an important constant related to R security. (2) A1A2 or B1B2 are detectable knowledge barrier segments. (3) For any other knowledge barrier segment, its distance to A1A2 and B1B2 is not simultaneously less than |AB|.

[0015] set up It is a polyline composed of knowledge barrier segments and connecting function segments. If all of them are related to... Intersecting knowledge barrier line segments and connecting function line segments are all present. Above that is a knowledge barrier wall.

[0016] A directed line (ray or directed line segment) is rotated around its origin until its direction aligns with the X-axis of the global coordinate system. This rotation is considered complete if the absolute value of the angle formed is not greater than [value missing]. Then the angle is the direction angle of the directed line, where the PMDline direction angle of R is used. R express.

[0017] Let DS(A, B) and RL(A, B) represent the directed line segment from A to B and the ray originating from A and passing through B, respectively. (A, B) represents the direction angle of DS(A, B) or RL(A, B). Define the function agl(x) as shown in (1).

[0018] (1) So, agl( (A1, A2)- (B1, B2) is the angle from DS(A1, A2) or RL(A1, A2) to DS(B1, B2) or RL(B1, B2). The angle from A2 to RL(P) is... R A1) The distance is not greater than D S In the case of agl( (P R A2)- (P R If A1))>0, then RL(P) R A2) is a ray in the positive direction of A1; such as agl( (P R A2)- (P R If A1))<0, then RL(P) R A2) is the ray in the negative direction of A1, such as Figure 1 As shown in a.

[0019] (II) Introduction to Basic Directional Endpoints: The concept of basic directional endpoints stems from the observation that, in a 2D environment, if we divide the knowledge obstacle segments to be avoided and the connecting functional segments into two sets: the positive direction segment set KOS P and the set of negative direction line segments KOS N ,like Figure 1 As shown, this bypasses KOS. P The endpoint of the midline segment requires R to move along the positive direction of that endpoint, while bypassing KOS. N The endpoint of the midline segment requires R to move along the negative direction of that endpoint.

[0020] Definition 1: Let B p For KOS PThe endpoints of a line segment, if for any line segment B1B2 containing Bp, agl( (P R B p )- (B p If (B1+B2) / 2)))>0, then B p Let B be a positive direction endpoint. n For KOS N The endpoints of a line segment, if for any line segment containing B n Line segments B1 and B2 both have agl( (P R B n )- (B n If (B1+B2) / 2)))<0, then B n It is a negative direction endpoint.

[0021] The positive and negative direction endpoints are collectively referred to as the basic direction endpoints. Algorithm 1 provides the set of positive direction line segments, KOS. P The set of negative direction line segments KOS N The calculation method for the basic direction endpoint set BEND, where BS T The key blocking segment for T is calculated using Algorithm 4. Unless otherwise stated, the segments mentioned below refer to knowledge barrier segments and connecting function segments.

[0022] Algorithm 1: Calculate KOS P KOS N And the method of BEND: Main input: P R T and BS T .

[0023] Output: KOS P KOS N And BEND.

[0024] Step 1, make KOS P KOS P2 KOS N and KOS N2 Both are BS T END P END N Both BEND and BEND are empty sets.

[0025] Step 2, for KOS P2 Each line segment E in p E0, let b=1 and E p E0 from KOS P2After deletion, proceed to steps 2.1-2.2, where it is assumed that agl( (P R E p )- (E p ,E0))>0.

[0026] Step 2.1, for E p Each line segment E1E2 intersects E0 (let the intersection point be E0). i ), proceed with steps 2.1.1 - 2.1.4.

[0027] Step 2.1.1, if E1E2 is in KOS N In China but not in KOS P Middle and E p E0 and P R If T does not intersect, then let KOS P If the value is empty, the algorithm will stop.

[0028] Step 2.1.2, if E1E2 and E p E0 and P R T do not intersect and E1E2 are not in KOS P In the middle, add E1E2 to KOS P and KOS P2 middle.

[0029] Step 2.1.3, not in KOS at E1E2 P Or E1E2 and P R T intersects but is not at KOS N In the case of agl( (P R E i )- (P R T))>0 and E1E2 and P R If T has an intersection point, then add E1 and E2 to KOS simultaneously. P KOS P2 KOS N and KOS N2 Otherwise if agl( (P R ,Ei)- (P R If T))>0, then add E1E2 to KOS P and KOS P2 middle.

[0030] Step 2.1.4, if E p On E1E2 and agl( (P RE p )- If (Ep, (E1+E2) / 2)<0, then let b=0, and simultaneously add E1E2 to KOS. P middle.

[0031] Step 2.2, if b=1, E p E0 is the knowledge barrier line segment and E p Not in END P Then, E is arranged in descending order of the angle of revolution. p Join END P .

[0032] Step 3, for KOS N2 Each line segment E n E0, let b=1 and E n E0 from KOS N2 After deletion, proceed to steps 3.1-3.2, where it is assumed that agl( (P R E n )- (E n ,E0))<0.

[0033] Step 3.1, for E n Each line segment E1E2 intersects E0 (let the intersection point be E0). i ), proceed with steps 3.1.1 - 3.1.4.

[0034] Step 3.1.1, if E1E2 is in KOS P In China but not in KOS N Middle and E p E0 and P R If T does not intersect, then let KOS P If the value is empty, the algorithm will stop.

[0035] Step 3.1.2, if E1E2 and E n E0 and P R T do not intersect and E1E2 are not in KOS N In the middle, E1E2 will be added to KOS. N and KOS N2 middle.

[0036] Step 3.1.3, not in KOS at E1E2 N Or E1E2 and P R T intersects at a point but is not within KOS. P In the case of agl( (P R E i )- (P R T))<0 and E1E2 and P R If T has an intersection point, then add E1 and E2 to KOS simultaneously. N KOS N2 KOS P and KOS P2 In; otherwise if agl( (P R E i )- (P R If T))<0, then add E1E2 to KOS N and KOS N2 middle.

[0037] Step 3.1.4, if E n On E1E2 and agl( (P R E n )- (E n If (E1+E2) / 2)>0, then let b=0, and simultaneously add E1E2 to KOS. N .

[0038] Step 3.2, if b=1, E n E0 is the knowledge barrier line segment and E n Not in END N In the middle, E is arranged in order of the angle of revolution (from largest to smallest). n Join END N .

[0039] Step 4, if KOS P2 If not empty, return to step 2.

[0040] Step 5, if END N The first point in the middle to P R The distance is greater than END P If the first point in the sequence is closer, then END N The first point in the middle, END P The point and END N The remaining points in the list are added to BEND in sequence; otherwise, END... P The first point in the middle, END N The point and END P The remaining points are added to BEND in sequence.

[0041] Regarding the endpoints of the basic directions, the following conclusions can be drawn: (1) In order to bypass KOS along the shortest path P and KOS NFor the line segment R in the diagram, at each sampling time, it should move towards the endpoint of the basic direction (positive or negative direction) that is not blocked.

[0042] Definition 2: If B is a fundamental direction endpoint and B =agl( (P R B)- (P R ,T)), then the angle defined by equation (2) (B) is the angle of travel of B.

[0043]

[0044] (2) For two positive or negative endpoints, if they are to P R If no obstacles block the straight path, then the solution path obtained by moving R towards the endpoint with the larger detour angle will be shorter.

[0045] Because it's only in KOS P or KOS N In KOS, the knowledge barrier line segment has at most one basic directional endpoint, therefore... P and KOS N At least half of the endpoints of the line segment are not basic direction endpoints. Therefore, the computational cost of determining the direction of motion of R based on the basic direction endpoints is smaller than that of the ordinary endpoints of the line segment. According to conclusion (2), once a feasible positive / negative direction endpoint is obtained, all remaining positive / negative direction endpoints with smaller detour angles can be directly ignored, thereby further saving computational cost. For this reason, steps 2.2 and 3.2 in Algorithm 1 arrange the basic direction endpoints according to the size of the detour angle.

[0046] (3) If Algorithm 1 is used, the necessary and sufficient condition for T to be unreachable is that Algorithm 1 stops in step 2.1.1 or 3.1.1, i.e., the obtained KOS P It is an empty set.

[0047] This demonstrates that Algorithm 1 can determine the reachability of T with minimal computation. It is worth noting that although most existing algorithms assume T is reachable, there are many cases where T is unreachable, and determining T's reachability often requires significant computation, especially in extremely complex environments.

[0048] (4) In RL(P) R B) with KOS P and KOS N Among the intersection points of the median segments (excluding B), if P c Distance P RRecently and |P R P c |>|P R B|, let P c The line segment containing B is m. If B is the endpoint in the positive direction, then m lies in KOS. N And not in KOS P In the middle; or B is the negative endpoint, and m is in KOS P And not in KOS N In the middle; or B is the endpoint of the basic direction. (B)> And m and P R If T and B intersect, then B lies on the unique path to T.

[0049] Finding the basic directional endpoint on the unique path to T means finding the unique path to T. Through this path R, one can escape an area with only one exit by following the optimal path. Figure 1 a) or enter an area with only one entrance ( Figure 1 (b) Furthermore, finding the fundamental directional endpoints on the unique path to T also helps ensure completeness. This is because if the only possible path to T can be identified, then all possible paths to T can be determined. By searching all possible paths, R can either reach T or determine that T is unreachable.

[0050] (5) If the straight path from R to T is blocked and no fundamental direction endpoint is located on the only path to T, then R should move toward a fundamental direction endpoint B that satisfies the following condition: RL(P R B) with KOS P and KOS N The line segments in the diagram do not intersect (ignoring B).

[0051] (iii) An artificial torque method based on the basic direction endpoints, which is applied to path planning of individual robots in extremely complex environments.

[0052] In the artificial moment method, when the straight path from R to T is blocked by an obstacle, R needs to gradually approach its attraction point Pat to eventually reach T. Since Pat is usually the endpoint of the attraction segment of R at the current moment, the method for calculating the attraction segment significantly affects the performance of the artificial moment method. However, existing methods for calculating the attraction segment have drawbacks. For example, they may cause the solution path of R in a dead end to be catastrophically long. They may also prevent R from obtaining a new attraction segment, thus making it impossible for R to determine the attraction point Pat and the direction of motion. This is because existing methods first select a set of KOS. P or KOS NThen, select one of the endpoints of the selected set of line segments as the starting point of the new attracting line segment. If none of the endpoints of the selected set of line segments are suitable as the starting point of the new attracting line segment (the straight path to them is either blocked by an obstacle or cannot make R gradually approach T), the new attracting line segment cannot be obtained.

[0053] Based on the above conclusions (1)-(5) regarding the basic direction endpoints, this paper proposes a new method for calculating the attraction segment. In this method, the starting point of the new attraction segment is selected from the basic direction endpoints according to the following rules: if a basic direction endpoint is located on the only path to T, then it is directly selected as the starting point of the new attraction segment; otherwise, the starting point of the new attraction segment is the positive direction endpoint B. E1 Or negative direction endpoint B E2 B E1 positive ray and B E2 The negative ray direction is related to KOS P and KOS N The line segments in B do not intersect. E1 and B E2 The detailed calculation method is shown in Algorithm 2.

[0054] Algorithm 2: Calculate the positive direction endpoint B E1 negative direction endpoint B E2 and methods for measuring their related quantities: Input: P R KOS P KOS N And BEND.

[0055] Output: B Ej S Ej , Ej and N Ej (j=1 and 2), where S E1 , E1 and N E1 With B E1 Related, S E2 , E2 and N E2 Then with B E2 Related.

[0056] Step 1: For i=1 and 2, let B Ei Let T be the value of S, and let S be the value of S. Ei , Ei and N Ei All are 0.

[0057] Step 2: For m = 1, 2, 3..., if the m-th point B of BEND...m The endpoint is the positive direction and N E1 =0, or B m The endpoint is in the negative direction and N E2 If the value is 0, then proceed with steps 2.1 to 2.4.

[0058] Step 2.1, let m =min(atan(D S / (|P R B m |+D V )), ).

[0059] Step 2.2, if B m If the endpoint is in the positive direction, then let b=1 and E1 =agl( (P R B m )+ m Otherwise, let b=2 and E2 =agl( (P R B m )- m ).

[0060] Step 2.3, for P R Starting with a direction angle of For ray q, execute steps 2.3.1 - 2.3.2.

[0061] Step 2.3.1, if q and KOS P and KOS N If the line segments in the equation intersect at a point, then let P be an integer. c For distance P R The nearest intersection; otherwise P c It is an infinitely far point.

[0062] Step 2.3.2, if there is no KOS P and KOS N Line segments that intersect q and satisfy the following conditions: they intersect q and the ratio of their intersection points to B. m Closer to P R Then, among these line segments, the distance from the intersection point with q to P is... R The nearest is S Eb Otherwise, S Eb =0.

[0063] Step 2.4, if P c If it is an infinity point, then NEb =b、B Eb =B m Otherwise if B m Located on the only way to reach T, then let N... Eb =b、N Ej =0 (j=3-b) and B Eb =B m Then, stop the algorithm.

[0064] Algorithm 3, which follows, provides the basic steps for calculating new attracting segments based on the endpoints of the fundamental directions, where A st It is the starting point of the attraction line segment obtained based on the endpoints of the basic direction. Based on A st This allows us to obtain the endpoint A of the new attracting line segment. ed As shown in steps 4 and 5.3 of Algorithm 3. If P R A ed If A has no intersection with other knowledge barriers, then A st A ed That is, the required attracting line segment; otherwise, consider A as the point where the line segment is attracted. st A ed The straight path is blocked by an obstacle, so A st A ed Need to be recalculated until P R A ed It does not intersect with any knowledge barrier wall, as shown in step 5 of Algorithm 3.

[0065] In Algorithm 3, M D It is a vector with four elements. M D The first element M D1 It is the attracting line segment A at the previous moment. st (k-1)A ed The fundamental direction of (k-1), where M D1 =1 indicates A st (k-1) is the endpoint of the positive direction, M D1 =2 indicates that it is the negative direction endpoint. If M D2 >2, then the most recent M D2 - The basic direction of both attracting line segments is M. D1 Therefore, if M D2 > +1 and direction M D1 If R cannot be made closer to T, then M D1 This will be abandoned, as shown in step 2.2.1 of Algorithm 3, thus preventing R from moving in this "wrong" direction for a long time. Furthermore, M... D2 The value also affects the setting and deletion of B-type connection function segments, as shown in steps 3.3 and 4.1 of Algorithm 4. M D3It is 0 or a line segment; only when M D3 When it is a line segment, a new B-type connection function line segment will be set (if possible), as shown in step 3 of algorithm 4.

[0066] According to steps 2.1, 2.2.2, and 3.2 of Algorithm 3, if the fundamental direction of a fundamental direction endpoint is related to M... D1 If they are the same, then it has a greater chance of being selected as the starting point A of the new attracting line segment. st This is because this choice allows R to move in the same direction for as long as possible, thus ensuring completeness. Furthermore, according to steps 2.2.1 and 3.1-3.2 of Algorithm 3, in many cases, A... st The basic direction may not be M D1 This is because if R always moves in one direction, the resulting solution path will be similar to the solution path of the bug algorithm, and the resulting solution path may be very long.

[0067] Algorithm 3: A method for calculating new attraction segments based on the endpoints of the basic directions. Main input: M D KOS P KOS N And BEND; Output: A st A ed and M D ; Step 1: Use Algorithm 2 to obtain B Ej S Ej , and N Ej (j=1 and 2); Let b=N E1 +N E2 and M D3 =0.

[0068] Step 2: If b < 3, then M D2 =-1 / ( Otherwise, in order D E1 =|P R B E1 |、D E2 =|P R B E2 Then, proceed with steps 2.1 - 2.2.

[0069] Step 2.1, if S E1 or S E2 Not zero, or (B E1 )+ (B E1 )> / 2, then in MD1 When M > 0, let M D2 =-1 / ( ) and D Ej =D Ej +(D E1 +D E2 min( (B E1 ) / , (B E2 ) / ), where j = 3 - M D1 .

[0070] Step 2.2, otherwise if (B E1 )+ (B E1 )< Then proceed with steps 2.2.1 - 2.2.2.

[0071] Step 2.2.1, in M D2 > +1 and (|B E2 T|-|B E1 T|)(2M D1 -3)>D S In the case of b=3-M D1 and M D2 =-M D2 .

[0072] Step 2.2.2, In other cases, if |M D2 If |>1, then b=M D1 and M D2 =M D2 +1; otherwise M D2 =M D2 +1.

[0073] Step 3: If b < 3, then K E =|P R B Eb |; Otherwise, in order = (T, P) R After that, proceed with steps 3.1 to 3.3.

[0074] Step 3.1: For each line segment P1P2, if it is in KOS P In China but not in KOS N and |agl( (T,(P1+P2) / 2)- TR )|>|agl( (T, B) E1 )- TR )|, then D E1 =D E1 +|P1P2| / Otherwise, if it is in KOS N In China but not in KOS P and |agl( (T,(P1+P2) / 2)- TR )|>|agl( (T, B) E2 )- TR )|, then D E2 =D E2 +|P1P2| / .

[0075] Step 3.2, if D E1 +|B E1 T| <D E2 +|B E2 If T|, then b=1; otherwise b=2.

[0076] Step 3.3, let K E =|P R B Eb | / 2; if S Ej (j=3-b), then M D3 =S Ej .

[0077] Step 4, let A st =B Eb Let A ed It is a point that satisfies the following condition: |P R A ed |=|P R A st |+D S / 2 and (P R A ed )=q Eb Let M D1 =b; Let M D4 For containing B Eb Knowledge barrier line segment.

[0078] Step 5, such as S Eb Then in order BS T =S Eb Then execute steps 5.1-5.3; repeat this process until S.Eb =0.

[0079] Step 5.1, using A ed After replacing T, recalculate KOS using Algorithm 1. P KOS N And BEND; recalculate B using Algorithm 2. Ej S Ej , and N Ej (j=1 and 2); Let b=N E1 +N E2 .

[0080] Step 5.2: When b < 3, if M D3 =0, then let M D2 =1 and M D3 =BS T In other cases, proceed with steps 5.2.1 – 5.2.3.

[0081] Step 5.2.1, if (B E1 )+|agl( E1 - R )|+|P R B E1 | / K E < (B E2 )+|agl( E2 - R )|+|P R B E2 | / K E If the result is positive, then b = 1; otherwise, b = 2.

[0082] Step 5.2.2, if M D3 =0 and (B E1 )+ (B E1 )> / 2, then M D3 =BS T .

[0083] Step 5.2.3, if |agl( (P R A ed )- )|>max(|agl( (P R A ed )- R )|(1+|P R B Eb | / D V ), / 2), then M D2 =1.

[0084] Step 5.3, A st =B Eb A ed It is a point that satisfies the following condition: |P R A ed |=|P R A st |+D S / 2 and (P R A ed )= .

[0085] Because the strategy of calculating the attraction line segment based on the fundamental direction endpoints allows R to escape dead ends in the optimal way and to move approximately towards the global tangent point of the obstacle, it is easy to generate a shorter solution path. This strategy also guarantees that a new attraction line segment starting point will always be obtained, making it less likely for R to be trapped by the environment. This is because if Algorithm 1 does not stop in step 2.1.1 or 3.1.1, then there is at least one fundamental direction endpoint whose straight-line path to R coincides with KOS. P and KOS N The line segments in the algorithm have no intersection points. Furthermore, compared to the A* method based on local tangent points and the existing artificial moment method which calculates the motion direction / attractive line segments of R, the proposed strategy requires less computation. This is because the number of basic direction endpoints is not only smaller than the number of convex vertices and knowledge barrier segment endpoints, but also that most basic direction endpoints do not require collision checks (i.e., steps 2.3.1–2.3.2 in Algorithm 2 do not need to be executed).

[0086] Please see as follows Figure 2 The diagram shows a simulation of the calculation of the attraction segment based on the basic direction endpoint and the setting of the B-type connection function segment using existing methods. Figure 2 a is t 1055 The state at time t, where according to the knowledge barrier containing Q, at t 1055 Initially, Q is chosen as the starting point of the attracting segment. However, the path to Q is blocked by a knowledge barrier containing N, and moving towards N is the optimal path to Q. Therefore, N is ultimately chosen as the starting point of the new attracting segment. Figure 2 b is t 1076 The state at time T, where the starting point of the attracting line segment is P rather than Q, because P lies on the only path to T. According to t... 1055 and t 1076From the state at that time, it can be concluded that the starting points of the attracting line segments will alternate between N and P, so that R will move endlessly between them.

[0087] However, even if the attraction segment is calculated based on the basic direction endpoints, or if R always moves in the same direction, or if a bug-prone algorithm is used directly, R may still result in... Figure 2 The image shows endless movement in certain regions. This is because, without using global search algorithms such as A* and D*, the starting point of the attracting line segment may alternate between two or more fundamental directional endpoints, such as... Figure 2 As shown in Algorithm 4, to address this issue, we propose a new rule for setting and deleting Type B connection function segments. In existing artificial torque methods, although Type B connection function segments are also set, they are primarily used to ensure connection with P. R Among the knowledge barrier segments that intersect at point T, the key blocking segment of T is closest to point P. R In situations where traversing backwards might cause R to move endlessly in certain areas, such as in... Figure 2 In the case shown, the Type B connection function segment may still not be set. However, in the rules of Algorithm 4, the Type B connection function segment is in M. D3 This setting is only applied when the line segment is a line segment, as shown in steps 3, 3.1–3.3 of Algorithm 4. This is because it is only in this case, i.e., when starting from P... R To B E1 Or B E2 The straight path is M D3 Only when blocked can R circulate in certain areas.

[0088] Algorithm 4: Calculate the attraction point P att Methods for setting and deleting Type B connection function segments: Main input: M D The attraction segment A at the previous moment st (k-1)Aed(k-1) and its update distance D upd (k-1).

[0089] Main output: P att A st A ed D upd BS T and M D .

[0090] Step 1, if A ed (k-1) is not empty and |P R A ed (k-1)|>D upd If (k-1), then proceed with steps 1.1 - 1.2.

[0091] Step 1.1, if D upd (k-1)£D V / +D S / 3, then let P att =A ed (k-1); otherwise P att It is a point that satisfies the following conditions: (P R P att )= (P R A ed (k-1))and|P R P att |=D V .

[0092] Step 1.2, if P R P att If A does not intersect with the detectable knowledge barrier segment or the connecting functional segment whose endpoints lie on the detectable knowledge barrier segment, then let A... st A ed D upd and M D With t k-1 Once the time is the same, stop the algorithm.

[0093] Step 2, let S1 be M D4 After S2 is an empty set, for each line segment whose endpoints lie on the line segments in S1, if it is a type B connecting line segment, then add it to set S2; otherwise, add it to set S1.

[0094] Step 3, if M D3 If it is a line segment, then proceed to steps 3.1 - 3.3.

[0095] Step 3.1: If there are detectable knowledge barrier segments in S1, then B1 is the endpoint of these segments that allows |agl( R - (P R B1))| is a point that achieves the maximum value; otherwise, B1 is the midpoint of the last knowledge obstacle line segment in S1.

[0096] Step 3.2: Let B2 be a point that satisfies the following condition: |agl( R - (P R ,B2))|> B2 is an endpoint of a knowledge barrier line segment and is located at M. D3The upper or lower level can be reached via knowledge barrier segments and N-type connecting functional segments to reach M. D3 .

[0097] Step 3.3: If there exists a point B2 that satisfies the condition, and M... D2 =1 or |B1B M |>|P R B M |+D S (B) M For M D4 If the midpoint is B1B2, then set B1B2 as a B-type connecting line segment and add it to S2.

[0098] Step 4: For each line segment E1E2 that is in S2 but not in S1, execute steps 4.1-4.2 until S2 is empty or all line segments in it are in S1.

[0099] Step 4.1: If E1 and E2 are both on the line segment in S1, or |E1E2|+(1-2M) D2 ) |P R A ed (k-1)|>|E1P R |+|E2A ed If (k-1)|, then delete E1E2.

[0100] Step 4.2: Otherwise, add E1E2 to S1. At the same time, for each line segment that is not in S1 but can reach E1E2 through knowledge barrier line segments and N-type connection function line segments, if it is a B-type connection function line segment, add it to S2; otherwise, add it to S1.

[0101] Step 5, in order to let A st A end P att and D upd If the result is empty, proceed to steps 5.1 - 5.3.

[0102] Step 5.1: If there exists a line segment in S1 that satisfies the following condition: it is not only connected to P... R T intersects with S1 and lies within the line segment S1; it intersects with P. R The distance P from the intersection of T R Recently, so in BS T After this line segment, delete all B-type connection function line segments that are not in S1 and stop the algorithm.

[0103] Step 5.2, let M D All elements are set to 0, and all B-type connection function segments are deleted.

[0104] Step 5.3: If there exists a line with P... RIf line segments T intersect with each other, representing knowledge barriers or connecting functional line segments, then let them be BS. T Otherwise, let P att =T and BS T =0.

[0105] Steps 4.1 and 5.1-5.2 of Algorithm 4 show that Type B connecting segments can be deleted in several situations. When E1 and E2 are both on segments in S1, the reason for deleting E1E2 (step 4.1 of Algorithm 4) is that in this case, E1E2 and part of the segments in S1 form a closed curve. This closed curve might lead to the erroneous conclusion that T is unreachable, so E1E2 must be deleted. The reason for deleting Type B connecting segments and carefully setting such segments in other cases is to reduce the negative impact of these segments on path optimization. This is because while these segments can prevent R from moving endlessly in certain areas, they can also prevent R from moving to shorter paths.

[0106] Steps 1 and 1.1-1.2 of Algorithm 4 use a condition based on the updated distance to determine the attraction segment A of the previous time step. st (k-1)A ed Is it possible to determine (k-1) at the current time t? k Continue using it, where if A st A ed If it is a newly obtained attracting line segment, then its updated distance D upd It is min(|P) R A st | / +D S / 2,D V -D S / 2), as shown in step S4.4 of the method based on the artificial torque at the basic direction endpoint. Otherwise D upd =D upd (k-1). The conditions based on the updated distance differ from those in existing artificial torque methods. The conditions based on the updated distance are not only easier to understand and implement (only needing to determine whether |P R A ed (k-1)| <D upd (k-1) and whether P R A ed (k-1) is blocked) and when A st (k-1)A ed When (k-1) is abandoned, |P R A ed (k-1)| is generally less than D V .

[0107] Based on algorithms 1-4 above, this application proposes an artificial torque method based on basic direction endpoints for path planning of individual robots in unknown and extremely complex environments. The method includes: S1. Initialize the individual robot R, target position T, working environment, and knowledge barrier wall, and set the current time t. k =t0, let vector M D Let A be a set of zeros where all elements in A are 0. st (-1) and A ed (-1) are all empty, and other parameters are set; S2. Based on the sensor information, update all knowledge barrier segments and set up N-type connection function segments; S3, based on the attraction segment A from the previous moment. st (k-1)A ed (k-1) and its update distance D upd (k-1) Determine the current attracting segment A st A ed Attraction Point P att The key blocking segment BS of T T If possible, also set and delete the relevant B-type connection function segments at the same time; S4, if P att If empty, then execute S4.1-S4.4; S4.1, based on T and BS T Obtain the set of positive direction line segments KOS P The set of negative direction line segments KOS N and the set of basic direction endpoints BEND; S4.2, if KOS P If empty, output an "unreachable" warning and stop the algorithm; S4.3. Obtain a new attracting line segment A based on the endpoints of the basic direction. st A ed ; S4.4, Let P att =A ed and D upd =min(|P R A st | / +D S / 2,D V -D S / 2); S5, R in P att Under the guidance of the instrument and the control of the artificial torque motion controller, the movement takes one step to reach the next sampling time t. k+1 ; S6. If R reaches T, stop the algorithm; otherwise, let tk = t k+1 Then, return to S2.

[0108] Example 2: Simulation Experiment The simulation platform was developed using MATLAB R2016b, and some parameters are D. R =0.15 meters, D V =3.5 meters, D S =1.2 meters =4. The robot's maximum stride is 0.24 meters, and its stride length for dynamic obstacles and T-junctions is 0.12 meters. Simulations were conducted on a PC equipped with an Intel Core i5 CPU and 8 GB of RAM. Simulation environments included destination-unreachable environments, dynamic obstacle environments, and complex maze-like environments, as shown below. Figure 3-5 As shown. All of these environments are highly complex, with the number of obstacle segments ranging from hundreds to tens of thousands.

[0109] For environments where the destination is inaccessible, Figure 3 The first type of target-inaccessible environment has a total of 279 obstacle edges (120 of which are used to approximate circular obstacles), and the total length of the obstacle edges is 1080.7 meters. Figure 3 b. The second type of target is an unreachable environment, which has 155 obstacle edges with a total length of 816.4 meters. In addition, the target T moves up and down within a given area. Figure 3 c. The third type of target is an unreachable environment, which has a total of 2218 obstacle edges with a total length of 11844 meters. Figure 3 The fourth type of target is unreachable environment, which has 2887 obstacle edges with a total length of 10424 meters.

[0110] For dynamic obstacle environments, Figure 4 In the diagram, O1-O6 represent dynamic obstacles that move within a given area. Figure 4 The first type of environment has 167 static obstacle edges, with a total length of 526.1 meters. Figure 4 b. The second type of environment has 372 static obstacle edges, with a total length of 708.8 meters. Figure 4 c. The third type of environment has a total of 6,730 static obstacle edges, with a total length of 33,806 meters. Figure 4 The fourth type of environment has 18,442 static obstacle edges, with a total length of 37,325 meters.

[0111] For complex maze-like environments, Figure 5 The first type of maze environment has 2182 obstacle edges, with a total length of 11888 meters. Figure 5b. The second type of maze environment has 2893 obstacle edges, with a total length of 10428.1 meters. Figure 5 c. The third type of maze environment has 8002 obstacle edges, with a total length of 40338 meters. Figure 5 The fourth type of maze environment has a total of 20,428 obstacle edges, with a total length of 41,478 meters.

[0112] Using the artificial torque method proposed in this paper, existing artificial torque methods, and different initial positions, in Figure 3-5 Each environment shown underwent 5-11 simulations. Figure 3-5 point S in j This represents the initial position of the j-th simulation in this environment. The statistical results of the simulation are shown in Table 1-4. The reason for performing up to 5-11 simulations in an environment instead of just one is that when R has no prior knowledge about its workspace, even a small change in the initial position of R can produce completely different solution paths. Furthermore, for existing artificial torque methods, when the starting point of the new attraction segment cannot be obtained, one of the obstacle points that allows R to move in a bug-like algorithmic manner is selected as the starting point of the new attraction segment.

[0113] Table 1. Performance statistics of the proposed artificial torque method in environments where the target is inaccessible.

[0114] Table 2. Performance statistics of the proposed artificial torque method in dynamic obstacle environments.

[0115] Table 3. Performance statistics of the proposed artificial torque method in maze-like environments.

[0116] Table 4. Existing methods for artificial torque. Figure 5 The performance statistics in the environment shown in ab indicate that R cannot accurately and safely reach the target point T, which means that R cannot reach T within a given time step or that R can be clearly seen to be cyclically moving within a certain area.

[0117]

[0118] As shown in Tables 1-3, the proposed artificial torque method enables R to stop moving in a timely manner and correctly conclude that the target is unreachable in all unreachable environments; it also enables R to safely and accurately reach T in all other environments, despite their extreme complexity. Existing artificial torque methods, however, fail to enable R to accurately reach destination T in certain situations, as shown in Table 4. These results demonstrate the completeness of the proposed artificial torque method.

[0119] To evaluate the computational efficiency of the proposed artificial torque method, the average number of collision checkpoints (total number of collision checkpoints / total number of attraction segments) and the average computation time (total time for attraction points / total number of time steps) for attraction points are also given in Table 1-4 (online). The time for attraction points includes the time for setting and deleting Type B connecting segments. However, the time for detecting and updating knowledge obstacle segments and setting Type N connecting segments is not considered. This is because this application primarily focuses on the computational efficiency of obtaining a reasonable path or direction of motion using current environmental knowledge. Table 1-4 illustrates that although some environments contain thousands or even tens of thousands of obstacle edges (… Figure 4 There are 18442 static obstacle edges in d. Figure 5 c has 8002 obstacle edges. Figure 5 (d contains 20428 obstacle edges), but the average number of collision checkpoints calculated for a single attraction segment is less than 4.5, with some even less than 2 (approximately 30%). The average time to calculate an attraction point is 0.2 ms–1 ms. Interestingly, the average number of collision checkpoints calculated for attraction segments and the average calculation time for attraction points do not increase significantly with the increase of obstacle edges and knowledge obstacle segments, as shown in Tables 1-2, although the number of obstacle edges ranges from hundreds to tens of thousands. In existing artificial torque methods, the average number of collision checkpoints calculated for attraction segments is 2.91–24.7, and the average calculation time for attraction points is 0.56 ms–2.9 ms, as shown in Table 4. These results indicate that the strategy of calculating attraction segments based on the basic direction endpoints significantly reduces collision detection time; the proposed artificial torque method has high computational efficiency.

[0120] To ensure completeness in unknown and highly complex environments, the best existing strategy might be to search all possible paths using algorithms such as the wall-walking algorithm. Based on these searches, a feasible and optimal path to T can be determined, or T can be determined to be unreachable. However, this strategy may result in solution path lengths that are close to or even greater than the total length of all obstacle edges, because in some areas, such as dead ends, traversing the obstacle edges in these areas may require two or more steps. If the solution path length obtained by the proposed artificial torque method is significantly smaller than the total length of all obstacle edges, then the quality of these paths is not only satisfactory but also higher than that of paths generated by algorithms such as the wall-walking algorithm. For the above reasons, Table 1-4 also gives the ratio of solution path length to the total length of all static obstacle edges. According to Table 1-3, the proposed artificial torque method makes the ratio of solution path length to the total length of all static obstacle edges... Figure 3 The values ​​are all less than 0.2 in the environments shown in c-3d, 4c-4d, and 5a-5d; even in... Figure 3 In the relatively simple environments shown in a-3b and 4a-4b, this ratio is still less than 0.64. The solution paths of existing artificial torque methods are almost all longer than those of the proposed method, as shown in Table 3-4. These results demonstrate that the proposed artificial torque method can always yield a shorter and more satisfactory solution path.

[0121] in conclusion: To solve sub-problems of path planning in unknown and extremely complex environments, such as providing completeness, escaping dead ends, and generating shorter solution paths, it may be necessary to frequently compute all possible solution paths. Because the number of possible solution paths in such environments can reach tens of thousands, a large amount of computation is often required.

[0122] This application discovers the reachability condition of T and the condition that the endpoints of the basic directions are located on the necessary path to T, thus solving the two sub-problems of T reachability determination and escaping dead ends with less computation. Furthermore, this discovery ensures that once R escapes a dead end, R cannot re-enter that dead end, which is beneficial for further shortening the solution path.

[0123] This application also finds that, in order to obtain a shorter solution path, R should move towards the endpoints of the fundamental directions that satisfy the following conditions: they are located on the only path to T, or the rays originating from R and passing through these endpoints do not intersect with the relevant knowledge barrier walls. For the attracting line segments that determine the direction of R's movement for most of the time, the proposed artificial torque method always makes the starting point of the attracting line segment such a fundamental direction endpoint. Therefore, compared with existing artificial torque methods and other methods that do not use global search algorithms, the proposed artificial torque method is more likely to generate shorter solution paths, and the computation time for the direction of R's movement is also shorter. This is because, in the process of calculating such fundamental direction endpoints, the endpoints of most knowledge barrier lines can be directly ignored because they are not fundamental direction endpoints; most fundamental direction endpoints do not require collision checks because once a fundamental direction endpoint located on the only path to T is determined, all other fundamental direction endpoints can be directly ignored; once a fundamental direction endpoint that satisfies the conditions is obtained, other endpoints in the same fundamental direction can also be directly ignored.

[0124] The new setting rules for Type B connecting functional segments presented in this application enable these segments to prevent R from moving endlessly in certain regions. While as many Type B connecting functional segments as possible should be set to prevent endless movement of R in all regions, the new rules impose strict conditions on setting such segments, and most of these segments are promptly deleted. This is because they may hinder R from moving along shorter paths. To compensate for the shortcomings of Type B connecting functional segments, the endpoints of the basic direction that do not change the basic direction of R's movement are more likely to become the starting points of attracting segments. This is because, to a certain extent, prolonged movement in the same direction can also prevent R from moving endlessly in certain regions. Due to these setting and deletion rules for Type B connecting functional segments and this calculation method for attracting segments, the proposed artificial torque method ensures both completeness and consistently provides shorter solution paths, even in unknown and extremely complex environments.

[0125] The above content is only a preferred embodiment of the present invention. For those skilled in the art, many changes can be made in the specific implementation and application scope based on the ideas of the present invention. As long as these changes do not depart from the concept of the present invention, they all fall within the protection scope of the present invention.

Claims

1. A basic direction end point based artificial torque method, characterized by, The method is applied to path planning of individual robots in unknown and extremely complex environments, and the method comprises the following steps: S1. Initialize the individual robot R, target position T, working environment, and knowledge barrier wall, and set the current time t. k =t0, let vector M D Let t be a set of zeros where all elements in t are 0. -1 The starting point A of the line segment is attracted at all times. st (-1) and endpoint A ed (-1) are all empty, and other parameters are set; S2, updating all knowledge obstacle line segments according to sensor information, and setting N-type connection function line segments; S3, according to the last time of the attraction line segment A st (k-1) A ed (k-1) and its update distance D upd (k-1) determine the current attraction line segment A st A ed , the attraction point P att and T the key blocking line segment BS T , if possible, also set and delete the relevant B type connection function line segment; S4, if P att is empty, S4.1-S4.4 are performed; S4.1, according to T and BS T obtaining a positive direction line segment set KOS P , a negative direction line segment set KOS N and a basic direction endpoint set BEND; S4.2, if KOS P is empty, output "unreachable" alert and stop the algorithm; S4.3, based on the basic direction end point, a new attractive line segment A is obtained st A ed ; S4.4, Let P att =A ed and D upd =min(|P R A st | / c +D S / 2,D V -D S / 2); S5, R moves one step under the guidance of P att to the next sampling time t k+1 ; S6, if R reaches T, stop the algorithm; otherwise, go back to S2 after setting t k = t k+1 .

2. The method of claim 1, wherein, The N-type connection function line segments are set in the following ways: For two points A and B on the knowledge obstacle line segments A1A2 and B1B2, if the following conditions are met, AB can be set as an N-type connection function line segment: (1) |AB| is the shortest distance between A1A2 and B1B2 and 0 < |AB| < D S where D S is an important constant related to R-security; (2) A1A2 or B1B2 is a detectable knowledge obstacle line segment; (3) For any other knowledge obstacle line segment, the distance to A1A2 and B1B2 is not less than |AB| at the same time.

3. The method of claim 1, wherein, The S3 specifically comprises the following steps: S3.1, if A ed (k-1) is not empty and |P R A ed (k-1) > D upd (k-1), then perform S3.1.1-S3.1.2; S3.1.1, If ​​D upd (k-1) D V / +D S / 3, then let P att =A ed (k-1); otherwise let P att It is a point that satisfies the following conditions: (P R P att )= (P R A ed (k-1))and|P R P att |=D V ; S3.1.2, if P R P att and the connection function segment with an end point on the detectable knowledge barrier segment do not intersect, then let A st A ed , D upd and M D all be the same as at time t k-1 , stop the algorithm; S3.2, after letting S1 be the M D4 and S2 be the empty set, for each line segment with an endpoint on S1, if it is a B-type connection function line segment, then add it to S2; otherwise add it to S1; S3.3, if M D3 is a line segment and not 0, then perform S3.3.1-S3.3.2; S3.3.1 If there are detectable knowledge barrier segments in S1, then B1 is the endpoint of these segments that allows |agl( R - (P R B1))| is a point that achieves the maximum value; otherwise, B1 is the midpoint of the last knowledge obstacle line segment in S1. S3.3.2, let B2 be a point such that |agl R - (P R , B2))| / 2, B2 is one end point of a knowledge barrier line segment and is located on M D3 or can be reached from M D3 by a knowledge barrier line segment and an N-type connection function line segment. S3.3.3, if there exists a point B2 that satisfies the condition, and M D2 = 1 or |B1B M | > |P R B M | + D S (B M is the midpoint of M D4 ), then set B1B2 as a B-type connection function line segment and add it to S2; S3.4, for each line segment E1E2 in S2 but not in S1, steps S3.4.1-S3.4.2 are performed until S2 is empty or all line segments in S2 are in S1; S3.4.1, if E1, E2 are both endpoints of a line segment in S1, or |E1E2| + (1 - 2M D2 ) |P R A ed (k-1)|>|E1P R |+|E2A ed (k-1)|, then delete E1E2; S3.4.2, otherwise, E1E2 is added to S1, and for each line segment that is not in S1 but can reach E1E2 through knowledge obstacle line segments and N-type connection function line segments, if it is a B-type connection function line segment, it is added to S2; otherwise, it is added to S1; S3.5, after having set A st A end , P att and D upd to empty, S3.5.1-S3.5.3 are executed; S3.5.1, if in S1 there is a line segment which not only has intersection with P R T and is in the line segment of S1, but also its intersection with P R T is the nearest to P R , then stop the algorithm after letting BS T be the line segment and deleting all B-type connection function line segments which are not in S1; S3.5.2, let M D all elements of M are 0, while deleting all B-type connection function line segments; S3.5.3, if there is a knowledge barrier segment or a connection function segment that intersects with P R T, then let BS T = BS att T and BS T = 0.

4. The method of claim 1, wherein, The S4.1 specifically comprises the following steps: S4.1.1, let KOS P , KOS P2 , KOS N , and KOS N2 be the set of BSs T , let END P , END N , and BEND be the empty set; S4.1.2, for each line segment E in KOS P2 E0, with b = 1 and E p E0, with b = 1 and E p E0, with b = 1 and E P2 E0, with b = 1 and E (P R , E p )- (E p , E0)) > 0; S4.1.2a, for each line segment E1E2 intersecting E0 (let the intersection point be E p E1E2, perform S4.1.2a.1-S4.1.2a.4; i E1E2, perform S4.1.2a.1-S4.1.2a.4; S4.1.2a.1, if E1E2 is in KOS N but not in KOS P and E p E0 is disjoint from P R T, then let KOS P be empty and stop the algorithm. S4.1.2a.2, if E1E2 and E p E0 all intersect P R T and E1E2 are not in KOS P then add E1E2 to KOS P and KOS P2 ; S4.1.2a.3, In E1E2 not in KOS P Or E1E2 and P R T intersects but is not at KOS N In the case of agl( (P R E i )- (P R T))>0 and E1E2 and P R If T has an intersection point, then add E1 and E2 to KOS simultaneously. P KOS P2 KOS N and KOS N2 Otherwise if agl( (P R ,Ei)- (P R If T))>0, then add E1E2 to KOS P and KOS P2 middle; S4.1.2a.4, if E p on E1E2 and agl( (P R , E p )- (Ep, (E1+E2) / 2) < 0, then let b = 0 and add E1E2 to KOS P ; S4.1.2b, if b = 1, E p E0 is the knowledge barrier segment and E p not in END P then add E p to END P in order of turn angle (from large to small) S4.1.3, for each line segment E of KOS N2 E0, with b = 1 and E n E0, with b = 1 and E n E0, with b = 1 and E N2 E0, with b = 1 and E (P R , E n )- (E n , E0)) < 0; S4.1.3a, for each line segment E1E2 intersecting E0 (let the intersection point be E n E1E2, perform S4.1.3a.1-S4.1.3a.4; i E1E2, perform S4.1.3a.1-S4.1.3a.4; S4.1.3a.1, if E1E2 is in KOS P but not in KOS N and E p E0 is disjoint from P R T, then let KOS P be empty and stop the algorithm. S4.1.3a.2, if E1E2 and E n E0 are all disjoint from P R T and E1E2 are not in KOS N then add E1E2 to KOS N and KOS N2 ; S4.1.3a.3, in case E1 E2 does not intersect KOS N or E1 E2 intersects P R T but not KOS P , if agl( (P R , E i )- (P R , T)) < 0 and E1 E2 intersects P R T, then add E1 E2 to KOS N , KOS N2 , KOS P and KOS P2 ; otherwise if agl( (P R , E i )- (P R , T)) < 0, then add E1 E2 to KOS N and KOS N2 ; S4.1.3a.4, if E n on E1E2 and agl( (P R , E n )- (E n , (E1+E2) / 2)) > 0, then let b = 0 and add E1E2 to KOS N ; S4.1.3b, if b = 1, E n E0 is the knowledge barrier segment and E n is not in END N , then E n is added to END N in order of turn angle (from large to small) S4.1.4, if KOS P2 is not empty, go to S4.1.2; S4.1.5, if the distance from the first point in END N to P R is closer than the distance from the first point in END P , the first point in END N , the point in END P and the remaining points in END N are added to BEND in order; otherwise, the first point in END P , the point in END N and the remaining points in END P are added to BEND in order.

5. The method of claim 1, wherein, The S4.3 specifically comprises the following steps: S4.3.1, obtain the basic direction end point B Ej and the quantity S related thereto Ej , q Ej and N Ej (j = 1 and 2); let b = N E1 +N E2 and M D3 = 0; where S E1 , q E1 and N E1 are related to B E1 , S E2 , q E2 and N E2 are related to B E2 . S4.3.2, if b < 3, then M D2 = -1 / (D ); otherwise after letting D E1 = |P R B E1 |, D E2 = |P R B E2 |, perform S4.3.2a-S4.3.2b; S4.3.2a, if S E1 or S E2 is not 0, or (B E1 )+ (B E1 )>3 / 2, then let M D1 > 0, let M D2 = -1 / (2 c ) and D Ej = D Ej + (D E1 + D E2 ) min( (B E1 ) / , (B E2 ) / ), where j = 3 - M D1 ; S4.3.2b, else if (B E1 )+ (B E1 )< S4.3.2b.1-S4.3.2b.2 are performed; S4.3.2b.1, in M D2 >2 c +1 and (|B E2 T|-|B E1 T|)(2M D1 -3)>D S , let b = 3 - M D1 and M D2 = -M D2 ; S4.3.2b.2, in other cases, if |M D2 = M D1 and M D2 = M D2 + 1 ; otherwise M D2 = M D2 + 1 ; S4.3.3, if b < 3, then K E = |P R B Eb ; else after letting TR = (T, P R ), S4.3.3a-S4.3.3c are performed; S4.3.3a, for each line segment P1P2, if it is in KOS P but not in KOS N and |agl (T, (P1+P2) / 2)- TR | > |agl (T, B E1 )- TR |, then D E1 = D E1 + |P1P2| / c ; else if it is in KOS N but not in KOS P and |agl (T, (P1+P2) / 2)- TR | > |agl (T, B E2 )- TR |, then D E2 = D E2 + |P1P2| / c ; S4.3.3b, if D E1 +|B E1 T|<D E2 +|B E2 T|, then b = 1; otherwise b = 2; S4.3.3c, let K E = |P R B Eb | / 2; if S Ej 0 (j = 3-b), then M D3 = S Ej ; S4.3.4, Let A st = B Eb ; Let A ed be a point satisfying the following conditions: |P R A ed |= |P R A st | + D S / 2 and (P R , A ed ) = q Eb ; Let M D1 = b; Let M D4 be the knowledge barrier segment containing B Eb ; S4.3.5, as in S Eb 0, then let BS T =S Eb Then execute S4.3.5a-S4.3.5c; repeat this process until S... Eb =0; S4.3.5a, after using A ed KOS is recalculated P , KOS N and BEND; then B is recalculated Ej , S Ej , q Ej and N Ej (j = 1 and 2); let b = N E1 + N E2 ; S4.3.5b.1, in case of b < 3, if M D3 = 0, then let M D2 = 1 and M D3 = BS T ; otherwise, perform S4.3.5b.2-S4.3.5b.3; S4.3.5b.1, if (B E1 )+|agl( - )|+|P R B E1 | / K E < (B E2 )+|agl( E2 - R )|+|P R B E2 | / K E then b = 1 ; else b = 2; S4.3.5b.2, if M D3 = 0 and (B E1 )+ (B E1 )> / 2, then M D3 = BS T ; S4.3.5b.3, if |agl (P R , A ed )- Eb )|>max(|agl (P R , A ed )- R )|(1+|P R B Eb | / D V ), and / 2), then M D2 =1; S4.3.5c, A st = B Eb ; A ed is a point satisfying the condition: |P R A ed |= |P R A st | + D S / 2 and (P R , A ed )= Eb .

6. The method of claim 1, wherein, The basic direction end point B in S4.3.1 is calculated by Ej and the quantity S associated therewith Ej , Ej and N Ej : S7.1, for i = 1 and 2, let B Ei be T, while letting S Ei , Ei and N Ei all be 0; S7.2, for m = 1, 2, 3,..., if the mth point B m of BEND is a positive direction endpoint and N E1 = 0, or B m is a negative direction endpoint and N E2 = 0, then perform S7.2.1-S7.2.4; S7.2.1, let m = min(atan(D S / (|P R B m |+D V )), 2 (B m )); S7.2.2, if B m is a positive direction endpoint, then let b = 1 and E1 = agl( (P R , B m ) + 1 m ); otherwise let b = 2 and E2 = agl( (P R , B m ) - 1 m ); S7.2.3, for a ray q starting at P R with a direction angle Eb S7.2.3a-S7.2.3b are performed; S7.2.3a, if q intersects the line segment with KOS P and KOS N then let P c be the closest intersection point to P R ; Otherwise P c is an infinite point; S7.2.3b, if there are line segments not in KOS P and KOS N that intersect q and the intersection point is closer to P m than B R , then among these line segments, the one whose intersection point with q is closest to P R is S Eb ; otherwise, S Eb = 0; S7.2.4, if P c is at infinity, then N Eb = b, B Eb = B m ; otherwise if B m is on the path to T, then after setting N Eb = b, N Ej = 0 (j = 3 - b) and B Eb = B m , stop the algorithm.

Images