Method and system for generating a passageway for a trackless vehicle in a confined space, and storage medium

Abstract

The application belongs to the technical field of path planning, and specifically relates to a method and system for generating a passing corridor for a trackless vehicle in a restricted space and a storage medium. The method comprises the following steps: S1, obtaining a plurality of preset path points and obstacle information in an environment; S2, performing collision detection for each preset path point to determine the interaction relationship between the path point and the obstacle; S3, based on the collision detection result, calculating an optimized nearest point between the path point and the obstacle for the obstacle that exists collision; S4, dynamically generating a safe passing area of the preset path point according to the optimized nearest point, wherein the shape and size of the safe passing area are adaptive to the spatial distribution of the obstacle; S5, iteratively processing all the preset path points to combine the respective safe passing areas into a continuous safe passing corridor; and S6, outputting the safe passing corridor for constraining path planning of the trackless vehicle. The safe passing area of the application is larger, and can provide a wider search range for path point optimization.

Description

TECHNICAL FIELD

[0001] The application belongs to the technical field of path planning, and particularly relates to a method and system for generating a passing corridor for a trackless vehicle in a restricted space and a storage medium. BACKGROUND

[0002] The path point planning of a trackless vehicle in a restricted space (such as a pipeline or a tunnel, in which the obstacles can usually be enveloped by a convex polygon) usually fails to be optimized. This is caused by the narrow search space of the path point. In addition, the existing method for generating a passing region by expanding a rectangle stops expanding after the collision of the rectangle or the vertex of the obstacle, and the safe passing region generated thereby cannot well fit the surface of the obstacle, so that the area of the safe passing region is smaller than the actual passable space area. This makes the path optimization in the restricted space more difficult. SUMMARY

[0003] The application provides a method and system for generating a passing corridor for a trackless vehicle in a restricted space and a storage medium.

[0004] The application adopts the following technical scheme: a method for generating a passing corridor for a trackless vehicle in a restricted space, comprising the following steps.

[0005] S1: acquiring a plurality of preset path points and obstacle information in an environment;

[0006] S2: performing collision detection for each preset path point to determine the interaction relationship between the path point and the obstacle;

[0007] S3: based on the collision detection result, calculating the optimized nearest point between the path point and the obstacle for the obstacle with collision;

[0008] S4: dynamically generating a safe passing region of the preset path point according to the optimized nearest point, wherein the shape and size of the safe passing region are adaptive to the spatial distribution of the obstacle;

[0009] S5: iteratively processing all the preset path points to combine the respective safe passing regions into a continuous safe passing corridor;

[0010] S6: outputting the safe passing corridor for constraining the path planning of the trackless vehicle.

[0011] In some embodiments, in step S1, the obstacle information comprises the vertex coordinates of the obstacle as a convex polygon, and the obstacle is numbered.

[0012] In some embodiments, step S2 comprises: the collision detection is achieved by generating a collision detection region centered on the preset path point.

[0013] S1: Analyzing whether there is an obstacle in the collision detection area, and configuring n collision flag bits of the obstacles, wherein the collision flag bit of the i-th obstacle is 1 when the i-th obstacle collides with the collision detection area, otherwise the collision flag bit is 0.

[0014] In some embodiments, the collision detection area is a square collision detection area with a side length of L, and the coordinates of the four vertices of the square collision detection area are:

[0015]

[0016] wherein, x i and y i are the coordinates of the i-th preset path point.

[0017] In some embodiments, S3 comprises:

[0018] Setting a target function: wherein, x i and y i are the coordinates of the i-th preset path point, and x and y are the coordinates of the point on the obstacle to be found. Solving the target function to obtain the nearest point of the obstacle in the collision detection area to the preset path point.

[0019]

[0020] In some embodiments, S4 comprises:

[0021] When there is no obstacle in the collision detection area, the safe passing area of the preset path point is the collision detection area of the path point.

[0022] When there is one obstacle in the collision detection area:

[0023] Calculating the line segment vector of the line connecting the nearest point and the preset path point , and calculating the angle between each edge of the obstacle and

[0024] Under the premise that intersects , find the obstacle edge closest to 90 degrees, i.e., the edge vector used to draw the safe passing area.

[0025] The single-edge safe passing area of the preset path point is described by a square:

[0026]

[0027] wherein, the four vertex coordinates of the square are , and the perpendicular line vector from the preset path point to the nearest point is , when the preset path point is on the right side or the upper side of ,​​​​​​​​​ When the path point is on the left side or the lower side of the obstacle, ; ; The foot of the perpendicular line drawn to the straight line on which the path point is located; The foot of the perpendicular line drawn to the straight line on which the path point is located; The foot of the perpendicular line drawn to the straight line on which the path point is located;

[0028] When there are multiple obstacles in the collision detection area:

[0029] For each obstacle, a single-side safe passing area is calculated, and the safe passing area is the intersection of all single-side safe passing areas.

[0030] In some embodiments, in S4: when the number of obstacles in the square collision detection area is less than the total number of obstacles, the safe passing area calculated is used for secondary collision detection with obstacles not in the square collision detection area, and the final safe passing area is obtained again.

[0031] A safe passing corridor generation system, comprising: a processor; a memory storing a computer program; wherein the processor implements the method for generating an optimal safe passing corridor for a trackless vehicle in a restricted space when executing the computer program.

[0032] A computer readable storage medium having a computer program stored thereon, wherein the computer program, when executed by a processor, implements the method for generating an optimal safe passing corridor for a trackless vehicle in a restricted space.

[0033] Compared with the prior art, the present application has the following beneficial effects:

[0034] The present application provides a safe passing corridor for a mobile device by optimization, and the safe passing area of the present application is larger than that of the rectangular expansion method, which can provide a wider search range for path point optimization and has more advantages for path point optimization in a restricted space. BRIEF DESCRIPTION OF DRAWINGS

[0035] Figure 1 A safe passing corridor generation algorithm block diagram;

[0036] Figure 2 A safe passing area (case 1: no obstacle in the square collision detection area)

[0037] Figure 3 A safe passing area (case 2: one obstacle in the square collision detection area);

[0038] Figure 4 A safe passing area (case 3: multiple obstacles in the square collision detection area);

[0039] Figure 5For the secondary collision detection map;

[0040] Figure 6 For the invention and rectangular expansion method safe passage area comparison map;

[0041] Figure 7 For the vehicle equivalent to two points;

[0042] Figure 8 For the side parking preset path point map;

[0043] Figure 9 For the safe passage corridor map generated by the invention;

[0044] Figure 10 For the safe passage corridor map generated by the rectangular expansion method;

[0045] Figure 11 For the safe passage area map of the invention (select 3 preset path points);

[0046] Figure 12 For the safe passage area map of the rectangular expansion method (select 3 preset path points);

[0047] Figure 13 For the trajectory optimization map (the invention safe passage corridor as a constraint);

[0048] Figure 14 For the trajectory optimization map (rectangular expansion method safe passage corridor as a constraint). DETAILED DESCRIPTION

[0049] In order to make the purpose, technical scheme and advantages of the embodiments of the invention clearer, the technical scheme in the embodiments of the invention will be described clearly and completely below. Obviously, the described embodiments are part of the embodiments of the invention, rather than all the embodiments. Based on the embodiments in the invention, all other embodiments obtained by those skilled in the art without creative labor fall within the scope of protection of the invention.

[0050] A limited space trackless vehicle passage corridor generation method, as shown in Figure 1 , comprising:

[0051] S1: obtaining a plurality of preset path points and obstacle information in the environment;

[0052] S2: for each preset path point, performing collision detection to determine its interaction with the obstacle;

[0053] S3: based on the collision detection result, for the obstacle with collision, calculating the optimized nearest point between the path point and the obstacle;

[0054] S4: dynamically generating a safe passing area of the preset path point according to the optimized nearest point, wherein the shape and size of the safe passing area are adaptive to the spatial distribution of the obstacles;

[0055] S5: iteratively processing all preset path points to combine respective safe passing areas into a continuous safe passing corridor;

[0056] S6: outputting the safe passing corridor for constraining path planning of the trackless vehicle.

[0057] In specific embodiments, step S1: t preset path points arranged in sequence are known: and the vertex coordinates of n convex polygon obstacles are known, and the obstacles are numbered. Set the increment k to 1, that is, start from the first preset path point.

[0058] In specific embodiments, step S2: a square collision detection area with a side length of is generated with as the center, and the vertex coordinates of the square collision detection area are:

[0059]

[0060] The separate axis theorem is used to analyze whether the square collision detection area collides with the obstacles, and n obstacle collision flag bits are configured. When the i-th obstacle collides, the collision flag bit of the obstacle is 1, otherwise it is 0.

[0061] In specific embodiments, step S3: the point inside the i-th obstacle (including the obstacle boundary) can be expressed as follows:

[0062]

[0063] where i is the number of the i-th obstacle, and there are n obstacles in total; j is the number of the j-th edge of the obstacle, and the maximum number of edges of the obstacle is m.

[0064] In order to find the nearest point on the i-th obstacle to the preset path point , the following objective function can be written:

[0065] The above problem is solved using the quadratic programming method:

[0066]

[0067] According to the above method, the nearest point of all obstacles in the square collision detection area is found .

[0068] In specific embodiments, step S4: determine the safe passing area of the path point.

[0069] Case 1: no obstacle in the square collision detection area;

[0070] The safe passing area of the path point is the square collision detection area, as shown in Figure 2 .

[0071] Case 2: one obstacle in the square collision detection area;

[0072] First, find the nearest point on the obstacle to the path point according to step S3 , as shown in Figure 3 . Then calculate the line segment vector of the line connecting the nearest point and the path point , and calculate the angle between each edge of the obstacle and . Under the premise that and intersect, find the obstacle edge closest to 90 degrees (the right edge of the triangular obstacle), that is, the edge vector used to draw the safe passing area:

[0073]

[0074] where and intersect, ; when and do not intersect, .

[0075] When the nearest point is on the edge of the obstacle (not the vertex of the obstacle), the foot of the perpendicular line drawn from the nearest point to the line where is located is , which is the nearest point in this case. When a certain nearest point is at the vertex of the obstacle, as shown in Figure 4 , the nearest point is the right vertex of the triangular obstacle. In this case, the foot of the perpendicular line drawn from the nearest point to the line where is located is . The single-edge safe passing area of the path point is described by a square:

[0076]

[0077] where P1, P2, P3, and P4 are the coordinates of the four vertices of the square. is the perpendicular line vector from to . When the path point is in When on the right or upper side, When path point exist When on the left or lower side, ,like Figure 3 As shown exist On the left side.

[0078] Scenario 3: There are multiple obstacles within the square collision detection area;

[0079] When there are q obstacles within the square collision detection area, calculate the safe passage area on one side for each obstacle, such as... Figure 4 As shown. To ensure the mobile device does not collide with any obstacles, the safe passage zone is the intersection of all one-sided safe passage zones:

[0080]

[0081] Based on scenario 2 or 3, if the number of obstacles within the square collision detection area is less than the total number of obstacles, a secondary collision detection is needed using the safe passage area obtained from scenario 2 or 3 against obstacles not within the square collision detection area. The method from scenario 2 or 3 is then used again to obtain the final safe passage area. For example... Figure 5 As shown, the square collision detection area only has the triangular obstacle on the left. First, the method in case 2 is used to obtain the one-sided safe passage area. This safe passage area collides with the rectangle on the right. We also need to obtain the one-sided safe passage area of ​​the rectangular obstacle. Finally, we find the intersection of these two one-sided safe passage areas to obtain the final safe passage area.

[0082] In a specific implementation, step S5: increment the increment k by 1, and repeat steps S2 to S4 in sequence to obtain the safe passage area of ​​the next preset path point until k equals t, then jump to step 6.

[0083] In a specific implementation, step S6: The safe passage areas of each preset path point are summarized into a safe passage corridor. Since the safe passage area is a convex polygon, the path points to be further optimized can be constrained within the safe passage corridor. Therefore, the safe passage corridor can be described by multiple linear inequalities (the description of the points inside the convex polygon is the same as the representation of the points inside the obstacles mentioned above).

[0084] Simulation verification

[0085] First, the formula and process of the method for generating trackless vehicle passage corridors in confined spaces according to this invention were written into MATLAB code, where the quadratic programming method for obtaining the nearest point was implemented using the MATLAB optimization library function `quadprog`. Then, the algorithm was compared and verified in a MATLAB simulation environment. Figure 6Three preset path points are set, which are (-4, 0), (-2, -2) and (0, -2). The safe passing areas obtained by the method and the rectangular expansion method are shown in Table 1. The safe passing area obtained by the method is more in line with the surface of the obstacle, while in most cases, the rectangular expansion method stops expanding after the rectangular vertex collides with the obstacle. The safe passing area obtained by the method is larger than that obtained by the rectangular expansion method. Therefore, the method provides a larger search area for subsequent path point optimization, and has more advantages in narrow space path point optimization.

[0086] Table 1 Comparison of safe passing area

[0087]

[0088] In order to better illustrate the advantages of the method, lateral parking in an inclined road is taken as an example. The vehicle steering mechanism is a front wheel Ackerman steering mechanism. The length of the vehicle is 3.95 meters, the width is 1.97 meters, the wheelbase is 2.48 meters, the front suspension length is 0.8 meters, and the rear suspension length is 0.67 meters, as shown in Figure 7 The vehicle is enveloped by two circles, so that the vehicle is abstracted as two points. As long as the two points do not embed in the expanded obstacle (the expansion radius is the same as the vehicle circle envelope radius), the vehicle will not collide with the obstacle. The preset path points are manually set as shown in Figure 8 The safe driving corridors generated by the method and the rectangular expansion method are shown in Figure 9 and 10 In order to compare more intuitively, three path points are extracted, and their safe passing areas are compared, as shown in Figure 11 and 12 It can be seen that the safe passing area generated by the method is much larger than that generated by the rectangular expansion method. Since the preset path may not satisfy the vehicle kinematics, the vehicle trajectory point is optimized by satisfying the vehicle kinematics (the vehicle with a front wheel Ackerman steering mechanism can be equivalent to a bicycle model, and the vehicle motion center is the rear axle center), taking the safe driving corridor as a linear constraint, taking the start point and the target point as an equality constraint, and minimizing the vehicle acceleration and steering angle as an objective function. The vehicle trajectory point refers to the position of the rear axle center. If the position of the rear axle center is determined, the positions of the two envelope circle center points in front and behind the rear axle center are determined. The trajectory point of the vehicle at each point is required to be in the corresponding safe passing area. The safe driving corridors obtained by the method and the rectangular expansion method are taken as linear constraints, and other conditions are consistent. The optimized trajectory points are shown in Figure 13 and 14The trajectory shown. It can be seen that the safe driving corridor of the present method is a collision-free optimization trajectory with linear constraints, and the trajectory is planned from the starting point to the target point; while the optimization of the rectangular expansion method fails to plan the vehicle to the target point, which is due to the small area of each safe passage area and the limited search range.

[0089] Finally, it should be noted that: the above embodiments are only used to illustrate the technical solutions of the present application, and not to limit them; although the present application has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that: it can still modify the technical solutions recorded in the foregoing embodiments, or make equivalent replacement to part or all of the technical features; and these modifications or replacements do not make the essence of the corresponding technical solutions deviate from the scope of the technical solutions of the embodiments of the present application.

Claims

1. A method for generating a trackless vehicle passageway in a confined space, characterized in that, include: S1: Obtain information on multiple preset path points and obstacles in the environment; S2: For each preset path point, perform collision detection to determine its interaction with obstacles; S3: Based on the collision detection results, for obstacles that collide, calculate the optimal closest point between the path point and the obstacle; S4: Based on the optimized nearest point, dynamically generate a safe passage area for preset path points, wherein the shape and size of the safe passage area adapt to the spatial distribution of obstacles; S4 includes: When there are no obstacles in the collision detection area: the safe passage area of ​​the preset path point is the collision detection area of ​​that path point; When there is one obstacle in the collision detection area: Calculate the line segment vector connecting the nearest point and the preset path point. And calculate each edge of the obstacle. and The included angle; exist and Given that the obstacles intersect, find the obstacle edge closest to 90 degrees, which is the edge vector used to draw the safe passage area. ; One-sided safe passage area of ​​preset path points Describe it using a square: in Let be the coordinates of the four vertices of the square. Let the perpendicular vector be the line vector from the preset path point to the nearest point. When the preset path point is at... When on the right or upper side, When the path point is When on the left or lower side, ; When there are multiple obstacles in the collision detection area: For each obstacle, calculate the one-sided safe passage area. The safe passage area is the intersection of all one-sided safe passage areas. S5: Iteratively process all preset path points and combine their respective safe passage areas into a continuous safe passage corridor; S6: Output the safe passage corridor to constrain the path planning of trackless vehicles.

2. The method for generating a trackless vehicle passageway in a confined space according to claim 1, characterized in that, In S1, the obstacle information includes the vertex coordinates of the obstacle as a convex polygon, and the obstacle is numbered.

3. The method for generating a trackless vehicle passageway in a confined space according to claim 1, characterized in that, S2 includes: Collision detection is achieved by generating a collision detection region centered on a preset path point. Analyze whether there are obstacles in the collision detection area and configure the collision flag bits of n obstacles. When colliding with the i-th obstacle, the collision flag bit of that obstacle is 1, otherwise it is 0.

4. The method for generating a trackless vehicle passageway in a confined space according to claim 3, characterized in that, The collision detection area has a side length of [length missing]. The square collision detection region has the following vertex coordinates: in, The coordinates of the preset path points.

5. The method for generating a trackless vehicle passageway in a confined space according to claim 1, characterized in that, S3 includes: Define the objective function: ,in The coordinates of the preset path points, These are points on the obstacles to be found; Solve the objective function to obtain the closest point of all obstacles within the collision detection area. .

6. The method for generating a confined space trackless vehicle passage corridor according to claim 1, characterized in that, In S4: when the number of obstacles in the square collision detection area is less than the total number of obstacles, a second collision detection is performed using the already calculated safe passage area and obstacles not in the square collision detection area to obtain the final safe passage area again.

7. A safe passage corridor generation system, characterized in that, include: processor; Memory, which stores computer programs; When the processor executes the computer program, it implements the method for generating a confined space trackless vehicle passageway as described in any one of claims 1-6.

8. A computer-readable storage medium having a computer program stored thereon, characterized in that, When the computer program is executed by a processor, it implements the method for generating a confined space trackless vehicle passageway as described in any one of claims 1-6.

Images