Method for path planning of mobile robot in high-density obstacle scene

Abstract

The application relates to a mobile robot high-density obstacle scene path planning method and belongs to the field of robots. The method comprises the following steps: constructing a robot object and a map object, selecting a path starting point and an ending point; the robot object and the map object are subjected to angle full convolution to obtain an angle full convolution map; a blind lane in which the robot cannot pass through in the angle full convolution map is shielded to reduce the complexity of the map; a surrounding ring in which the robot cannot pass through in the angle full convolution map is shielded to eliminate the surrounding ring and reduce the complexity of the map; a target path is initialized as a straight-line shortest path; a first element of a path matrix is the path starting point, and a last element of the path matrix is the path ending point; a loop is executed to scan an obstacle section in the path and to shorten the obstacle section in a combination mode of forward obstacle avoidance at the starting point and backward obstacle avoidance at the target point; when the length of the obstacle section in the path is zero, obstacle avoidance is ended, and the path matrix is fine-tuned. The application has the advantages of short path length, strong obstacle avoidance capability in a complex environment and short passing time.

Description

Technical Field

[0001] This invention belongs to the field of robotics and relates to a path planning method for mobile robots in high-density obstacle scenarios. Background Technology

[0002] Due to factors such as cost and system complexity, users need mobile robots to traverse high-density obstacle scenarios in the shortest path and time.

[0003] To address the path planning problem of mobile robots, existing technologies typically begin by planning the global shortest path. Then, when points on the path cannot be reached during actual operation, the robot's pose is adjusted in real time based on the distance between the robot and obstacles obtained from sensors. However, in environments with high-density obstacles, the mobile robot may get stuck on the map, leading to mission failure or timeout.

[0004] The specific problems include:

[0005] Uncertainty in environmental perception: In unknown or partially known environments, robots need to perceive the environment through sensors and find the optimal path from the starting point to the target point through path planning algorithms. However, due to the presence of various unknown or partially known obstacles in the environment, robots may not be able to accurately perceive all obstacles, which may lead to deviations in the path planning results.

[0006] The difficulty of real-time pose adjustment: In actual operation, due to the complexity and uncertainty of the environment, the robot may encounter insurmountable obstacles while performing forward path planning. In this case, the robot needs to obtain the distance to the obstacle through sensors and adjust its pose based on this information. However, due to the performance limitations of the sensors, or the complexity and uncertainty of the environment, the robot may not be able to obtain accurate information about the obstacle in real time, which may lead to difficulties in pose adjustment.

[0007] Risk of task failure or timeout: In high-density obstacle environments, due to the complexity and uncertainty of the environment, the robot may not be able to obtain accurate information about the obstacles in real time, or may not be able to make pose adjustments in real time. This may cause the robot to fail to reach the target point, resulting in task failure or timeout.

[0008] Path planning algorithm selection: Although global path planning and local path planning each have their own advantages, in practical applications, due to the complexity and uncertainty of the environment, it may be necessary to combine global and local path planning, or use more complex path planning algorithms to cope with the challenges of the environment. Summary of the Invention

[0009] In view of this, the purpose of the present invention is to provide a path planning method for mobile robots in high-density obstacle scenarios.

[0010] To achieve the above objectives, the present invention provides the following technical solution:

[0011] A path planning method for mobile robots in high-density obstacle scenarios, comprising the following steps:

[0012] Construct robot and map objects, and select the start and end points of the path;

[0013] An angle-completely convolutional map is obtained by performing angular full convolution on the robot object and the map object.

[0014] In a fully convolutional map with shielded angles, blind alleys that robots cannot pass through are identified to reduce map complexity.

[0015] In a fully convolutional map with shielded angles, the robot cannot pass through the enclosing circle to reduce map complexity;

[0016] Initialize the target path as the shortest straight path;

[0017] The first element of the path matrix RPath is the path start point PBg, and the last element is the path end point PEnd.

[0018] Perform a loop scan of the obstacle segments in the path, and shorten the obstacle segments to avoid obstacles by combining forward obstacle avoidance from the starting point and backward obstacle avoidance from the target point;

[0019] Obstacle avoidance ends when the length of the obstacle segment in the path is zero.

[0020] Adjust the path matrix RPath;

[0021] The output path algorithm has ended.

[0022] Optionally, the construction of the robot object and map object, and the selection of the path start and end point, specifically includes:

[0023] The map object is a Width×Higth dimensional integer matrix, with the top left corner of the map as the origin. The positive x-axis extends horizontally to the right from the origin, and the positive y-axis extends vertically downwards from the origin.

[0024] Robot object R(P) i R r W r Angle i ) is 2R r ×2R r An integer matrix, where the robot's in-situ rotation center P i Maximum rotation radius R r Width is W rThe angle between the direction of movement and the positive x-axis of the map (Angle) i ;R(P i R r Width r Angle i The elements in the array take the values ​​0 and 1; elements greater than zero take the value 1.

[0025] When selecting the starting and ending points of a path, the x-coordinate of the starting point is less than the x-coordinate of the ending point.

[0026] Optionally, the maximum rotation radius R r Width W r As the value increases, the corresponding fault tolerance improves.

[0027] Optionally, the robot object and the map object are subjected to angular full convolution to obtain an angular full convolution map, specifically including:

[0028] Robot R(P) i R r W r Angle i ) and Map in P i Perform angular full convolution on the points; the formula for angular full convolution is:

[0029] AglCon(R(P i R r W r Angle i Map(P) i ·x,P i ·y))=SUM(R(P i R r W r Angle i )&Map(P i ·x,P i ·y), ALL)

[0030] Where & is the matrix AND operator, and SUM(:, ALL) is the matrix summation operator;

[0031] The map path matrix PMap is a 16-bit integer matrix of dimension Width×Higth×Depth, where Width is the map width, Height is the map height, and Depth is the number of angle groups of the midpoint of the map.

[0032] Robot R(P) i R r W r Angle i ) in P iThe result of the full convolution of the points with the map (Map) is stored in the map path matrix (PMap).

[0033] Robot R(P) i R r Width r Angle i ) in P i When the angle of the point and the map (Map) is fully convolved and the result is zero, the robot does not collide with the map, and this is represented by the PMap matrix element PMap(P i ·x, P i Assign values ​​to y and nDeepth;

[0034] PMap(P i ·x, P i The formula for assigning values ​​to ·y, nDeepth) is:

[0035] nDeepth = floor(Angle) i / 16);

[0036] Bitmap(Angle i ) = 1 << (15 - mod (Angle) i ,16))

[0037] PMap(P i ·x, P i ·y,nDeepth)&=Bitmap(Angle i ),

[0038] Where nDeepth is Angle i The number of angle groups is determined by grouping angles into sets of 16. `floor(·)` is the floor function; `mod(·)` is the modulo function. `Bitmap(Angle)`... i Angle i The bitwise operation template, where & is the "bitwise AND" operator;

[0039] The robot's mobility matrix RMap is a width × height 3D integer matrix representing the robot's R(P) capability. i R r Width r Angle i ) in P i The passability of a point; the initial values ​​of all elements in the RMap matrix are 0;

[0040] Robot R(P) i R r W r Angle i) and map Angle i When the convolution result of the angle is zero, there is no collision between the robot and the map. The formula for assigning values ​​to the RMap matrix elements is:

[0041] RMap(P i ·x, P i ·y)=RMap(P i ·x, P i ·y)+1;

[0042] The robot accessibility mask matrix RMapMask is a Width×Higth dimensional integer matrix with initial values ​​of 0.

[0043] Optionally, in the fully convolutional map of the shielding angle, the blind alleys that the robot cannot pass through specifically include:

[0044] In fully convolutional maps with eliminated angles, the width is less than 2R. r Narrow alleys where you can't turn around;

[0045] According to the direction angle Angle i Query the points in RMap that have a value of 1, and set the last point on the line with a value of 2 in RMap to 1 in RMapMask.

[0046] Optionally, the bounding circle in the fully convolutional map at the shielding angle that the robot cannot pass through specifically includes:

[0047] For each point in RMap with a value greater than 180, find the arc segment formed by the set of points in its neighborhood with an RMap value less than or equal to 2, and set the first and last points of the arc segment to 1 in RMapMask.

[0048] Optionally, initializing the target path as the shortest straight path specifically involves:

[0049] Initialize the path RPath as the shortest straight path, regardless of whether the shortest straight path passes through the obstacle area.

[0050] Optionally, the step of performing a cyclic scan of obstacle segments in the path and shortening the obstacle segments for obstacle avoidance by combining forward obstacle avoidance from the starting point and backward obstacle avoidance from the target point specifically includes:

[0051] S111: The starting point of the obstacle segment detected in the nPStp=1th scan is PObF1, and the ending point of the obstacle segment is PObB1;

[0052] S112: Add any reachable neighbor point of PObF1 to the position immediately adjacent to the start of the path in the robot path matrix RPath;

[0053] S113: Add any reachable neighbor position of PObB1 to the position in the robot path matrix RPath that is adjacent to the end of the path.

[0054] S114: The starting point of the obstacle segment scanned in the nPStp>1-th cycle is PObF nPStp , and the end point of the obstacle segment is PObB nPStp ; The obstacle segment is represented as Obs(PObF nPStp ,PObB nPStp ).

[0055] S115: Select the point in the robot path matrix RPath that is closest to PObF nPStp as the starting point of the forward exploration point PNbr(PObF nFStp ), and perform exploration to obtain PFwd(PObF nFStp );

[0056] S116: Select the point in the robot path matrix RPath that is closest to PObB nPStp as the starting point of the backward exploration point PNbr(PObB nFStp ), and perform exploration to obtain PBwd(PObB nFStp );

[0057] S117: Select the point with the smallest subsequent path obstacle ratio between the two points PFwd(PObF nFStp ) and PBwd(PObB nFStp ) and add it to the path matrix RPath;

[0058] S118: When PFwd(PObF nFStp ) is selected, PFwd(PObF nFStp ) is added to the path matrix RPath behind the previous path point, that is, behind the starting point of the path exploration;

[0059] S119: When PBwd(PObB nFStp ) is selected, PBwd(PObB nFStp ) is added to the path matrix RPath in front of the previous path point, that is, in front of the starting point of the path exploration;

[0060] S120: If the length of the obstacle segment Obs(PObF nPStp ,PObB nPStp ) is non-zero at the end of the exploration in the nPStp-th step, nPStp = nPStp + 1, and continue to loop and execute S114;

[0061] S121: If nPStp < cPStp, that is, the number of loops is less than the upper limit constant cPStp, continue to execute S114;

[0062] S122: If the obstacle segment Obs(PObF) ends at the end of the nPStp step of exploration, nPStp ,PObB nPStp If the length is zero, the loop ends;

[0063] S123: At the end of the exploration step nPStp>cPStp, if the obstacle segment Obs(PObF) is... nPStp ,PObB nPStp If the length is still not zero, then the loop ends.

[0064] Optionally, the adjustment of the path matrix RPath specifically involves adjusting the barrier-free path RPath according to the principle of minimum tension.

[0065] The algorithm ends, and the resulting RPath matrix is ​​output.

[0066] The beneficial effects of the present invention are as follows: the present invention has the advantages of short path length, strong obstacle avoidance ability in complex environments, and short passage time.

[0067] Other advantages, objectives, and features of the invention will be set forth in part in the description which follows, and in part will be apparent to those skilled in the art from the following examination, or may be learned from practice of the invention. The objectives and other advantages of the invention can be realized and obtained through the following description. Attached Figure Description

[0068] To make the objectives, technical solutions, and advantages of the present invention clearer, the preferred embodiments of the present invention will be described in detail below with reference to the accompanying drawings, wherein:

[0069] Figure 1 A flowchart of a path planning method for a mobile robot in a high-density obstacle scene is provided in an embodiment of the present invention;

[0070] Figure 2 This is a schematic diagram of map scene angle convolution provided in an embodiment of the present invention;

[0071] Figure 3 A schematic diagram of forward obstacle avoidance and backward obstacle avoidance provided in the embodiments of the present invention;

[0072] Figure 4 This is the shortest path diagram planned in the embodiments of the present invention. Detailed Implementation

[0073] The following specific examples illustrate the implementation of the present invention. Those skilled in the art can easily understand other advantages and effects of the present invention from the content disclosed in this specification. The present invention can also be implemented or applied through other different specific embodiments, and various details in this specification can be modified or changed based on different viewpoints and applications without departing from the spirit of the present invention. It should be noted that the illustrations provided in the following embodiments are only schematic representations of the basic concept of the present invention. Unless otherwise specified, the following embodiments and features can be combined with each other.

[0074] The accompanying drawings are for illustrative purposes only and are schematic diagrams, not actual pictures. They should not be construed as limiting the invention. To better illustrate the embodiments of the invention, some parts in the drawings may be omitted, enlarged, or reduced, and do not represent the actual product dimensions. It is understandable to those skilled in the art that some well-known structures and their descriptions may be omitted in the drawings.

[0075] In the accompanying drawings of the embodiments of the present invention, the same or similar reference numerals correspond to the same or similar components. In the description of the present invention, it should be understood that if terms such as "upper," "lower," "left," "right," "front," and "rear" indicate the orientation or positional relationship based on the orientation or positional relationship shown in the drawings, they are only for the convenience of describing the present invention and simplifying the description, and do not indicate or imply that the device or element referred to must have a specific orientation, or be constructed and operated in a specific orientation. Therefore, the terms used to describe positional relationships in the drawings are only for illustrative purposes and should not be construed as limiting the present invention. For those skilled in the art, the specific meaning of the above terms can be understood according to the specific circumstances.

[0076] The terms "comprising" and "having," and any variations thereof, in the specification, embodiments, claims, and drawings of this invention are intended to cover non-exclusive inclusion, such as including a series of steps or units.

[0077] Figure 1 A flowchart of a path planning method for a mobile robot in a high-density obstacle scene is provided in an embodiment of the present invention.

[0078] In this implementation, the environment map is 200*120 pixels with a resolution of 0.1m. A 200×120 dimensional map object matrix Map(200, 120) is initialized, with obstacle coordinates set to 0 and non-obstacle coordinates set to 1. The algorithm also initializes a 200×120×23 dimensional 16-bit integer map path matrix PMap.

[0079] S0060: Robot angular resolution Aresolve = 1 degree, robot position resolution Presolve = 0.1 meters.

[0080] S0061: Robot object R(P) with a radius of 7 and a width of 6. i ,7,6,Angle r Perform an angular convolution of [0, 360) on the map object Map(200, 120) to find the shortest path between the starting point PBg(54, 18) and the ending point PEnd(152, 98).

[0081] S0062: The robot at point P(60, 40) does not interfere with the map within the angular range of [75 degrees, 105 degrees) and [255 degrees, 285 degrees). Figure 2 As shown. The values ​​of PMap(60, 40, :) obtained by angular full convolution are as follows:

[0082] PMap(60,40,4)=0B0000,0000,0001,1111=0X001F,

[0083] PMap(60, 40, 5) = 0xFFFF

[0084] PMap (60, 40, 6) = 0B1111, 1111,1000,0000 = 0XFF80,

[0085] PMap(60, 40, 15) = 0B0000,0000,0000, 0001 = 0X0001

[0086] PMap(60, 40, 16) = 0XFFFF

[0087] PMap (60, 40, 17) = 0B1111, 1111, 1111, 1000 = 0XFFF8

[0088] All other elements of PMap(60, 40, :) are equal to 0X0000.

[0089] RMap(60, 40) = 60.

[0090] S0063: The robot at point P(75, 64) does not interfere with the map within the angular ranges of [31 degrees, 45 degrees) and [216 degrees, 225 degrees). The values ​​of MPath(75, 64, :) obtained by full angular convolution are as follows:

[0091] RMap(75,64,1)=0B0000,0000,0000,0001=0X0001

[0092] RMap(75,64,2)=0B1111,1111,1111,1000=0XFFF8

[0093] RMap(75,64,13)=0B0000,0000,1111,1111=0X00FF

[0094] RMap(75,64,14)=0B1000,0000,0000,0000=0X8000

[0095] In RMap(75, 64, :), all other elements are equal to 0X0000.

[0096] RMap(75, 64) = 23.

[0097] S0064: Let RMapMask = RMap = 0; define a narrow alley as a passage with a width less than 2R in which the robot cannot turn around.

[0098] S0065: Determining Straight and Narrow Alleys:

[0099] Query RMap(P(x) j ,y j ))≤2, from P(x j ,y j Angle j Starting from point ) and following Angle j Directional query map matrix MPath satisfies condition |P(x aj y aj ,Angle j )-P(x pipej y pipej Angle j ±90°)|≤2R-W r And RMap(P(x) pipej ,y pipej Points whose x ≤ 2, all of which form the set Pipe(P(x)). j ,y j Angle j )).

[0100] S0066: If the tunnel contains First(Pipe(P(x) j ,y j Angle j And MPath(First(Pipe(P(x)) j ,y j Angle j ))).x,First(Pipe(P(x j ,y j Angle j ))).y,Angle jFor points where ) = 1, then block the exit, and let RMapMask(Last(Pipe(P(x) = 1). j ,y j Angle j ))))=1.

[0101] S0067: If Last(Pipe(P(x) exists... j ,y j Angle j And MPath(Last(Pipe(P(x)) j ,y j Angle j ))).x,Last(Pipe(P(x j ,y j Angle j ))).y,Angle j For points where ) = 1, the entrance is blocked, and RMapMask(First(Pipe(P(x) = 1) is set to 1. j ,y j Angle j ))))=1.

[0102] S0068: Determine the turning lane.

[0103] From P j (x j ,y j Angle j ) Starting from point, query the narrow lanes (P(x)) in RMap that meet the conditions. j ,y j Angle j );

[0104] From P k (x k ,y k Angle k ) Starting from point, query the narrow lanes (P(x)) in RMap that meet the conditions. k ,y k Angle k ).

[0105] When P j (x j ,y j Angle j The starting line and P k (x k ,y k Angle k The lines originating from this point intersect at point P. jk (x jk,y jk ), and P jk (x jk ,y jk ) angular range [Angle j ,Angle k If it is reachable within [the specified area], then the set Pipe(Angle) is accessible. j ) and Pipe (Angle k ) merged into Pipe (Angle) j +Angle k If Pipe(Angle) j +Angle k If the number of openings at the endpoints of a narrow alley is 1, then all values ​​at the points corresponding to the narrow alley openings in the RMapMask matrix are set to 1.

[0106] S0069: Define a passage with a width greater than 2R, in which a robot can turn around, as an enclosing circle.

[0107] S0070: From PMap, Angle ec Point P ∈ [0, 360) ec (x ec ,y ec Angle ec Angle at any angle ec Starting with an incremental step size, we find the condition RMap(P(x) satisfies the following criteria. es y es ))=1 point.

[0108] From P(x) es y es ,Angle es ) Starting from the clockwise direction, query RMap(P(x) efwd y efwd If ))≤2, then the set Bag is formed. fwd ={P(x efwd y efwd ,Angle efwd )}.

[0109] From P(x) es y es ,Angle es ) Starting from the counter-clockwise direction, query RMap(P(x) ebwd y ebwd If ))≤2, then the set Bag is formed. bwd ={P(x ebwd y ebwd ,Angle ebwd )}.

[0110] If last(Bag) fwd ) and last(Angle bwd ) with P(x eflast y eflast Angle is the angle of the vertex. efwd2ebwd ≥180°, including Angle es ;

[0111] At this time, P(x) eflast y eflast ) and P(x eblast y eblast The values ​​of the points on the connecting lines are all set to 1 in the corresponding points of the RMapMask matrix.

[0112] S0071: The user inputs the starting point PBg and the ending point PEnd of the path, and the conditions 0≤PBg·x≤PEnd·x, PBg·y≥0, and PEnd·y≥0 are satisfied. If the above conditions are not satisfied, the coordinates of PBg and PEnd are swapped.

[0113] S0072: The straight-line path StPath between the starting point PBg and the ending point PEnd is taken as the theoretical shortest path, and its length is... The shortest path angle is the angle between the STPath path vector and the X-axis of the map coordinate system, denoted as Angle(StPath); the upper limit of the path is defined as UpPath. Its length is nLU = Length(UpPath) = πLength(StPath) / 2.

[0114] S0073: The i-th row vector of the RPath matrix represents the i-th point in the path. RPath(i:.) = (P i .x,P i .y,Angle(P i )).

[0115] S0074: Initialize the nLU×3D robot path matrix RPath. Let RPath(0:.)=(PBg.x,PBg.y,Angle(StPath))

[0116] RPath(nLU-1:.)=(PEnd.x,PEnd.y,Angle(StPath))

[0117] S0075: Initialize the pathfinding counter nPStep = 1;

[0118] Initialize the forward pathfinding steps nFStep = 1 and the backward pathfinding steps nBStep = 1;

[0119] S0076: Step 1 scans Path(PBg,PEnd) to find the obstacle-free and obstacle-prone segments of the path. An obstacle-free segment is a path segment that does not pass through any obstacle points from its start to its end. The detected obstacle segment starts at PObF1 and ends at PObB1. Obst(PObF1,PObB1) represents the obstacle-prone segment that starts at obstacle point PObF1 and ends at obstacle point PObB1, such as... Figure 3 As shown.

[0120] S0077: Select obstacle avoidance point PNbr(PObF1) for PObF1 and obstacle avoidance point PNbr(PObB1) for PObB1.

[0121] PNbr(PObF1) is a point among the neighbors of PObF1 along StPath that does not collide with the obstacle; PNbr(PObB1) is a point among the neighbors of PObB1 along StPath that does not collide with the obstacle; Angle(PNbr(PObF1)) = Angle(StPath) + ΔAngleF; Angle(PNbr(PObB1)) = Angle(StPath) + ΔAngleB.

[0122] S0078: Add two points to the path matrix RPath.

[0123] RPath(1:.)=

[0124] (PNbr(PObF1).x,PNbr(PObF1).y,Angle(PNbr(PObF1)))

[0125] RPath(nLU-2:.)=

[0126] (PNbr(PObB1).x,PNbr(PObB1).y,Angle(PNbr(PObB1)))

[0127] S0079: When the path is represented as a segmented path, it is as follows:

[0128] Path(PBg,PEnd)=Path(PBg,PNbr(PObF1))+Obst(PObF1,PObB1)+Path(PNbr(PObB1),PEnd)

[0129] S0080: Update counter

[0130] nPStp=nPStp+1; nFStp=nFStp+1; nBStp=nBStp+1;

[0131] S0081: Initialize the starting point of the nPStp-th exploration step

[0132] PNbr(PObF nPStp = RPath(nFStp-1:.);

[0133] PNbr(PObB nPStp )=RPath(nUL-1-(nBStp-1):.);

[0134] S0082: Determine the initial value of the endpoint of the nPStp-th exploration step.

[0135] PFwd(PObF nPStp )=PNbr(PObF nPStp ),

[0136] PBwd(PObB nPStp )=PNbr(PObB nPStp ),

[0137] S0083: Calculate the current PNbr(PObF) nPStp ) to PNbr(PObB nPStp Shortest path StPath(PNbr(PObF)) nPStp ),PNbr(PObB nPStp The obstacle ratio;

[0138] Length(StPath(PNbr(PObF nPStp ),PNbr(PObB nPStp )) is from PNbr(PObF nPStp ) to PNbr(PObB nPStp The shortest path length of a straight line from point ( ).

[0139] OLength(PNbr(PObF nPStp ),PNbr(PObB nPStp )) is from PNbr(PObF nPStp ) to PNbr(PObB nPStp The length of the line segment formed by the obstacle points on the straight line from point )

[0140] RObs1 = OLength(PNbr(PObF) nPStp ),PEnd) / Length(StPath(PNbr(PObF nPStp ),PEnd)

[0141] S0084: Forward and backward exploration loop begins;

[0142] S0085: Assign forward angle Angle F =Angle(PNbr(PObF) nPStp ));

[0143] Assigning the reverse angle Angle B =Angle(PNbr(PObB) nPStp ))

[0144] S008652: Calculate the exploration point PFwd(PObF) nPStp The coordinates of ).

[0145] PFwd(PObF nPStp )·x=PNbr(PObF nPStp )·x+Dresolv×cos(Angel F )

[0146] PFwd(PObF nPStp )·y=PNbr(PObF nPStp )·y+Dresolv×sin(Angel F )

[0147] Bitmap(Angle F )=1<<{15-mod[Angle F / 16]}

[0148] S0087: If

[0149] MPath(PFwd(PObF nFStp )·x,PFwd(PObF nFStp )·y,nDeepth(Angle F ))&Bitmap(Angle F ) = 0

[0150] This indicates that the robot cannot continue moving in that direction.

[0151] S0088: Calculate the new robot forward exploration direction angle

[0152] Angle F =Angle F +Aresolve

[0153] Proceed to S0086 until the robot can continue moving in that direction.

[0154] S0089: If

[0155] MPath(PFwd(PObF nFStp)·x,PFwd(PObF nFStp )·y,nDeepth(Angle F )), &Bitmap(Angle F ) = 1

[0156] And RMapMask(PFwd(PObF) nFStp )·x,PFwd(PObF nFStp )·y)≠1

[0157] This indicates that the robot can continue moving in that direction.

[0158] S0090: Calculation

[0159] RObsF = OLength(PFwd(PObF) nPStp ),PEnd) / Length(StPath(PFwd(PObF nPStp ),PEnd)

[0160] S0091: Calculate the reverse exploration point PBwd(PObB) nBStp The coordinates of ).

[0161] PBwd(PObB nPStp )·x=PNbr(PObB nPStp )·x-Dresolv×cos(Angel B )

[0162] PBwd(PObB nPStp )·y=PNbr(PObB nPStp )·y-Dresolv×sin(Angel B )

[0163] Bitmap(Angle B )=1<<{15-mod[Angle B / 16]}

[0164] S0092: Robot object R(PObB) nPStp ,R r W r ,Angle B ) and path map MPath(PObB nPStp ·x, PObB nPStp ·y,nDeepth(Angle B Perform bitwise operations;

[0165] S0093: If

[0166] MPath(PObBnPStp ·x, PObB nPStp ·y,nDeepth(Angle B ))&Bitmap(Angle B ) = 0

[0167] Calculate the new robot backward exploration direction angle

[0168] Angle B =Angle B +Aresolve;

[0169] Execute S0091 until the robot can continue to move in that direction.

[0170] S0094: If

[0171] MPath(PObB nPStp ·x, PObB nPStp ·y,nDeepth(Angle B ))&Bitmap(Angle B ) = 1

[0172] And RMapMask(PFwd(PObF) nFStp )·x,PFwd(PObF nFStp )·y)≠1

[0173] This indicates that the robot can continue to move in the opposite direction.

[0174] S0095: Calculation

[0175] RObsB = OLength(PBwd(PObB) nPStp ),PBg) / Length(StPath(PBwd(PObB nPStp ),PBg))

[0176] S0096: Calculate RObs1, RObs F RObs B Minimum value RObs min ;

[0177] S0097: If RObs min =RObs F Update RPath(nFStp:·)=PFwd(PObF nPStp ); nFStp=nFStp+1; FStep=FStep+1;

[0178] S0098: If RObs min =RObsB Update RPath(nLU-1-nBStp:·)=PBwd(PObB nPStp )nBStp=nBStp+1; FStep=FStep+1;

[0179] S0099: If RObs min =RObs B =RObs F Update RPath(nFStp:·)=PFwd(PObF nFStp+1 ) or RPath(nLU-1-nBStp:·)=PBwd(PObB nBStp ), and its corresponding counter.

[0180] S0100: If RObs min =RObs1, no operation performed;

[0181] S0101: RObs1 ≠ 0 The loop executes S0046-0067, such as... Figure 3 As shown.

[0182] S0102: RObs1 = 0, loop ends.

[0183] S0103: The final path is represented as follows:

[0184] Path(PBg,PEnd)=

[0185] Path(PBg,RPath(1:.),…,RPath(nFStp:.),…,

[0186] RPath(nLU-1-nBStp:.),…,RPath(nLU-2:.),PEd)

[0187] S0104: Fine-tune the path, output the shortest path, such as... Figure 4 As shown.

[0188] S0105: Algorithm ends.

[0189] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention and are not intended to limit it. Although the present invention has been described in detail with reference to preferred embodiments, those skilled in the art should understand that modifications or equivalent substitutions can be made to the technical solutions of the present invention without departing from the spirit and scope of the present invention, and all such modifications or substitutions should be covered within the scope of the claims of the present invention.

Claims

1. A path planning method for mobile robots in high-density obstacle scenarios, characterized by: The method includes the following steps: Construct robot and map objects, and select the start and end points of the path; Performing angular full convolution on the robot object and the map object yields an angular full convolution map; specifically including: robot With map exist Perform angular full convolution on the points; the formula for angular full convolution is: in It is the matrix AND operator. It is the summation operator for all elements of a matrix; The center of rotation for the robot in place. For the maximum rotation radius, For width, For the direction of movement and the map of x The angle between the positive axis and the clockwise axis; Map objects for A 3D integer matrix, with the top-left corner of the map as the origin, and horizontal coordinates extending to the right from the origin. x The positive direction of the axis, starting from the origin and perpendicularly downwards is... y Positive direction of the axis; Map path matrix yes A 16-bit integer matrix. Width For map width, Height This represents the map height. Depth Number of angle groups for the midpoint of the map; robot exist Points and Maps The results of the full convolution of the angles are stored in the map path matrix. middle; robot exist Points and Maps When the result of the fully convolution of the angle is zero, the robot does not collide with the map. Matrix elements Assignment; The assignment formula is: ； ， in for The number of angle groups, grouped into sets of 16. This is the floor function; It is the modulo function. For angle The bitwise operation template, where It is the bitwise AND operator; Robot accessibility matrix yes 3D integer matrix representation of robots exist The traffic capacity of the point; The elements in the matrix are initially set to all zeros. robot With map conduct When the convolution result of the angle is zero, the robot does not collide with the map. The formula for assigning values ​​to matrix elements is: ； The robot accessibility mask matrix RMapMask is a Width×Higth dimensional integer matrix with initial values ​​of 0. In a fully convolutional map with shielded angles, blind alleys that robots cannot pass through are identified to reduce map complexity. In a fully convolutional map with shielded angles, the robot cannot pass through the enclosing circle to reduce map complexity; Initialize the target path as the shortest straight path; Path matrix The first element is the starting point of the path. The last element is the path endpoint. ; Perform a loop scan of the obstacle segments in the path, and shorten the obstacle segments to avoid obstacles by combining forward obstacle avoidance from the starting point and backward obstacle avoidance from the target point; Obstacle avoidance ends when the length of the obstacle segment in the path is zero. Adjust path matrix ; The output path algorithm has ended.

2. The path planning method for mobile robots in high-density obstacle scenarios according to claim 1, characterized in that: The construction of the robot object and map object, and the selection of the path start and end points, specifically include: Map objects for A 3D integer matrix, with the top-left corner of the map as the origin, and horizontal coordinates extending to the right from the origin. x The positive direction of the axis, starting from the origin and perpendicularly downwards is... y Positive direction of the axis; robot object for Integer matrix, The element value is either 0 or 1; elements greater than zero are assigned the value 1. When selecting the start and end points of a path, the path start point... x The axis coordinate is less than the endpoint. x Axis coordinates.

3. The path planning method for mobile robots in high-density obstacle scenarios according to claim 2, characterized in that: The maximum rotation radius ,width As the value increases, the corresponding fault tolerance improves.

4. The path planning method for mobile robots in high-density obstacle scenarios according to claim 1, characterized in that: In the fully convolutional map of the shielding angle, the blind alleys that the robot cannot pass through specifically include: In fully convolutional maps with eliminated angles, the width is less than Blind alleys where you can't turn around; According to direction angle Query The point with a value of 1 is selected in the middle, and the line is... The last point with a value of 2 is at The middle value is 1.

5. The path planning method for mobile robots in high-density obstacle scenarios according to claim 1, characterized in that: In the fully convolutional map with the shielding angle, the enclosing circle that the robot cannot pass through specifically includes: Query For points with values ​​greater than 180, there exist points in their neighborhood. An arc segment consisting of a set of points with values ​​less than or equal to 2, with the first and last points of the arc segment at... The middle value is 1.

6. The path planning method for mobile robots in high-density obstacle scenarios according to claim 1, characterized in that: The process of initializing the target path as the shortest straight path specifically involves: path Initialize to the shortest straight path, regardless of whether the shortest straight path passes through the obstacle area.

7. The path planning method for mobile robots in high-density obstacle scenarios according to claim 1, characterized in that: The process involves scanning obstacle segments in a loop and shortening these segments using a combination of forward obstacle avoidance from the starting point and backward obstacle avoidance from the target point. Specifically, this includes: S111: The The starting point of the obstacle segment detected in the next scan is The end of the obstacle course is ; S112: Will Add any reachable neighbor point location to the robot path matrix The location immediately adjacent to the starting point of the path; S113: Will Add any reachable neighbor point location to the robot path matrix The location immediately adjacent to the end of the path; S114: The The starting point of the obstacle segment scanned in the next iteration is The end of the obstacle course is Obstacle segments are represented as ; S115: Selection and Closest robot path matrix The points in the middle are used as forward exploration points. From the starting point, we can explore and obtain... ; S116: Selection and Closest robot path matrix The points in the middle are used as backward exploration points. From the starting point, we can explore and obtain... ; S117: Select and The point that minimizes the obstacle ratio of subsequent paths among the two points is added to the path matrix. middle; S118: When When selected, Added to The previous path point, that is, after the starting point of the path exploration; S119: When When selected, Added to The previous path point, that is, before the starting point of the path exploration; S120: The If the obstacle section ends at the end of the exploration step The length is not zero. Continue executing S114 in a loop; S121: If That is, the number of iterations is less than the upper limit constant. Continue executing S114; S122: The If the obstacle section ends at the end of the exploration step If the length is zero, the loop ends; S123: The If the obstacle section ends at the end of the exploration step If the length is still not zero, then the loop ends.

8. The path planning method for mobile robots in high-density obstacle scenarios according to claim 1, characterized in that: The adjustment path matrix Specifically, this means: applying the principle of minimum tension to barrier-free paths. Make adjustments; The algorithm ends, and the output is obtained. matrix.

Images