Structured environment even super-elliptic limit loop path planning method

Abstract

The application discloses a kind of even super-elliptic limit ring path planning methods under structured environment, first obtain structured two-dimensional prior map, rectangular fitting is carried out to the obstacle in map and each rectangular parameter is extracted;Second, determine the current most influential obstacle to the target point, according to the set porosity, determine the rectangular circumscribed super-elliptic frequency, build rectangular obstacle envelope even super-ellipse;Then establish the local coordinate system associated with the terminal point and obstacle, determine the solving direction of limit ring and solve the even super-elliptic limit ring trajectory, solve the coordinates of the current limit ring escape point;Finally, judge whether there is an obstacle between escape point and terminal point, if there is, continue to solve even super-elliptic limit ring for the next obstacle, otherwise, solve the trajectory to reach the terminal point;The method is assisted by the advantage of even super-ellipse, while maintaining flexibility and smoothness, has higher map space utilization, shorter path planning length.

Description

Technical Field

[0001] This invention belongs to the field of robot path planning, specifically relating to a method for even-order hyperelliptic limit cycle path planning in structured environments. Background Technology

[0002] With the development of robotics technology, unmanned vehicles are being used more and more widely in various fields, such as logistics and distribution, warehouse management, and smart cities. In these application scenarios, unmanned vehicles need to achieve efficient, accurate, and safe path planning in order to better complete their tasks. Robot path planning technology refers to the process of planning within a limited space, based on information about the robot's surrounding environment, to enable the robot to quickly and accurately reach the target point in a complex environment while avoiding collisions with obstacles. It is an important component of autonomous robot navigation.

[0003] Common local path planning methods include: the simulation-based Dynamic Window Approach (DWA), which treats the robot as a disk and determines the acceptable speed range by simulating the disk's movement at different speeds to avoid obstacles; and the Elastic Band Approach, which treats the robot's trajectory as an elastic band and adjusts the band's shape to allow the robot to avoid obstacles. Traditional limit cycle methods based on circles and ellipses offer advantages such as simple environmental models, short runtime, and smooth paths. However, in compact, structured environments, using circles or elliptical envelope rectangles can lead to high porosity. When obstacles are close together, the envelope ellipse of the current obstacle may intersect with other obstacles, obscuring previously passable areas. On the other hand, using hyperelliptical envelope rectangles has lower porosity, allowing the corresponding hyperelliptical limit cycle trajectory to fit the rectangular obstacle more closely within a safety margin, thus reducing the length of the local path planning.

[0004] Therefore, given the limitations of traditional local path planning methods for elliptical limit cycles, it is essential to propose a path planning method based on even-order hyperelliptical limit cycles, taking advantage of the smoothness, low fitting porosity, and high space utilization of even-order hyperellipses. Summary of the Invention

[0005] To address the aforementioned issues, this invention discloses a path planning method for even-order hyperelliptic limit cycles in structured environments. Leveraging the advantages of even-order hyperellipses, such as their brevity, smoothness, low fitting porosity, and high space utilization, this method rapidly plans a safe and smooth path from the starting point to the endpoint.

[0006] To achieve the above objectives, the technical solution of the present invention is as follows:

[0007] A method for even-order hyperelliptic limit cycle path planning in structured environments includes the following steps:

[0008] S1: Obtain a structured two-dimensional prior map, dilate the safe distance of obstacles in the map, rectangularize it, and extract the rectangle parameters;

[0009] S2: For the environmental obstacles input in step S1, determine the obstacle that has the greatest impact on the path to the target point, determine the number of the circumscribed hyperellipse of the rectangle based on the set porosity, and construct the even-order hyperellipse of the rectangular obstacle envelope.

[0010] S3: Establish a local coordinate system OT between the even-order hyperellipse of the current obstacle envelope generated in step S2 and the target point;

[0011] S4: Based on the local coordinate system OT calculated in step S3, calculate the coordinates of the current point in this coordinate system and determine the solution direction r of the limit cycle;

[0012] S5: Based on the solution direction determined in step S4 and the even-order hyperellipse parameters determined in step S2, establish the limit cycle equation of the even-order hyperellipse starting from the current point and solve for the trajectory.

[0013] S6: Based on the even-order hyperelliptical limit cycle trajectory obtained in step S5, transform it to the local coordinate system OT determined in step S3, and then determine the coordinates of the current even-order hyperelliptical limit cycle trajectory departure point.

[0014] S7: For the coordinates of the limit loop break-off point obtained in step S6, determine whether the point and the endpoint are other obstacles. If so, go to step S2 and repeat steps S2-S7.

[0015] S8: Based on the coordinates of the limit cycle break-off point obtained in step S7, plan the path between this point and the endpoint to complete the entire path planning process.

[0016] Furthermore, step S1 specifically includes the following process:

[0017] (1-1) Obtain prior grid maps in a structured environment through real-time positioning and mapping technology;

[0018] (1-2) Based on the grid map obtained in step (1-1), the binary image connected component analysis method is adopted. First, the image is converted into a black and white binary image. Then, the connected component analysis algorithm is used to mark each connected component in the image. Finally, the connected components of each obstacle are selected.

[0019] (1-3) For each connected region of obstacles on the map determined in step (1-2), and after dilation according to the set safety distance, construct an obstacle list, and use a rectangle fitting method to determine the center coordinates Coor of each rectangular obstacle.rect 2g in length, 2h in width, and Ω in rotation angle.

[0020] Furthermore, step S2 specifically includes the following process:

[0021] (2-1) For the rectangular obstacle list determined in step S1, determine the number of the obstacle that is currently the most obstructive to passage based on the line connecting the current point and the endpoint.

[0022] (2-2) Determine the number n of the circumscribed hyperellipse of the obstacle based on the set porosity λ, and determine the major semi-axis a and minor semi-axis b of the hyperellipse;

[0023] (2-3) Solve the hyperellipse equation to obtain the sequence of points P of the even-order hyperellipse trajectory circumscribed by the obstacle. SEOH .

[0024] Furthermore, step S3 specifically includes the following process:

[0025] (3-1) Connect the center of the rectangular obstacle determined in step S2 with the target point, and use the X-axis of the local coordinate system OT as the x-axis. OT ;

[0026] (3-2) The even-order circumscribed hyperellipse P of the obstacle obtained in step S2 SEOH Find the two tangent points Q1 and Q2 between the target point and the hyperellipse. The line connecting Q1 and Q2 is used as the y-axis of the local coordinate system OT. OT ;

[0027] (3-3) Based on the local coordinate system x-axis calculated in step (3-1) and the local coordinate system y-axis calculated in step (3-2), determine the origin (x-axis) of the local coordinate system OT. OT ,y OT ), determine X OT The angle α between the X-axis and the global coordinate system determines the X-axis. OT With Y OT The included angle

[0028] (3-4) Based on the angle α and angle obtained in step (3-3) Obtain the transformation relationship between the global coordinate system and the local coordinate system.

[0029] Furthermore, step S4 specifically includes the following process:

[0030] (4-1) Based on the transformation relationship between the global coordinate system and the local coordinate system obtained in step S3 Set the current starting point (x) s_w ,y s_w Transform to the local coordinate system (x) s_OT ,ys_OT );

[0031] (4-2) Based on the y obtained in step (4-1) s_OT Determine the solution direction r of the even-order hyperelliptic limit cycle. If y s_OT >0, r=1, if y s_OT ≤0, r=-1;

[0032] Furthermore, step S5 specifically includes the following process:

[0033] (5-1) Based on the rectangle center Coor determined in step S2 rect Establish the limit cycle equation of an even-order hyperellipse based on the major semi-axis a, minor semi-axis b, and order n of the hyperellipse.

[0034] (5-2) Based on the current starting point (x) s_w ,y s_w Solve the equation of the even-order hyperelliptic limit cycle to obtain the hyperelliptic limit cycle trajectory P starting from the current point. EOHLC_w ;

[0035] Furthermore, step S6 specifically includes the following process:

[0036] (6-1) Based on the transformation matrix obtained in step S3 The hyperelliptic limit cycle trajectory P obtained in step S5 EOHLC_w Transform to the local coordinate system OT to obtain P EOHLC_OT ;

[0037] (6-2) The trajectory P in the local coordinate system obtained based on step (6-1) EOHLC_OT The breakaway point p of the limit cycle trajectory is determined based on the sign of the x-value of the trajectory point. escape .

[0038] Furthermore, step S7 specifically includes the following process:

[0039] (7-1) Based on the current even-order hyperelliptic limit cycle trajectory decoupling point p obtained in step S6 escape Determine whether the point crosses other obstacles between itself and the target point. If it does, proceed to step S2.

[0040] (7-2) If no other obstacles are passed, proceed to step S8.

[0041] Furthermore, step S8 specifically includes the following processes:

[0042] (8-1) The breakaway point p of the limit cycle trajectory solved by S6 escape An adaptive linear interpolation method is used to solve the path from the point to the target point.

[0043] The beneficial effects of this invention are:

[0044] (1) The path planning method for even-order hyperelliptic limit cycles in a structured environment proposed in this invention is an upgrade and improvement of the traditional method based on circular or elliptical limit cycles, which retains the flexibility, simplicity and stability of using limit cycle methods to solve navigation planning problems.

[0045] (2) In a compact structured environment, using a superelliptical envelope rectangle will result in a smaller porosity, which can fully improve the space utilization rate. Even when the distance between obstacles is close, the planning will not fail because the originally passable narrow area will be submerged by the envelope.

[0046] (3) The hyperelliptical limit loop trajectory will fit the rectangular obstacle better if the safety margin allows. Compared with the circular or elliptical limit loop trajectory, it has a shorter path planning length while ensuring smoothness. Attached Figure Description

[0047] Figure 1 A schematic diagram of the method flow of this invention;

[0048] Figure 2 Schematic diagram of a rectangular obstacle;

[0049] Figure 3 Schematic diagram of the even-order hyperellipse trajectory points circumscribed by the obstacle;

[0050] Figure 4 Schematic diagram of local coordinate system OT;

[0051] Figure 5 A schematic diagram of the hyperelliptical limit cycle trajectory starting from the current point;

[0052] Figure 6 A complete schematic diagram of the path planning process for even-order hyperelliptic limit cycles;

[0053] Figure 7 A comparative diagram of methods in a narrow structured environment;

[0054] Figure 8 A schematic diagram illustrating the advantages of even-order hyperelliptic limit cycle path planning. Detailed Implementation

[0055] The present invention will be further illustrated below with reference to the accompanying drawings and specific embodiments. It should be understood that the following specific embodiments are for illustrative purposes only and are not intended to limit the scope of the invention.

[0056] Example 1: As Figure 1 As shown, a path planning method for even-order hyperelliptic limit cycles in a structured environment is proposed, which includes the following steps:

[0057] Step S1: Obtain a structured two-dimensional prior map, dilate the safe distance of obstacles in the map, rectangularize it, and extract the rectangle parameters;

[0058] (1-1) Obtain prior grid maps in a structured environment through real-time positioning and mapping technology;

[0059] (1-2) Based on the grid map obtained in step (1-1), the binary image connected component analysis method is adopted. First, the image is converted into a black and white binary image. Then, the connected component analysis algorithm is used to mark each connected component in the image. Finally, the connected components of each obstacle are selected.

[0060] (1-3) such as Figure 2 As shown, for each connected region of obstacles on the map determined in steps (1-2), and after dilation according to the set safety distance, an obstacle list is constructed. A rectangle fitting method is used to determine the center coordinates (Coor) of each rectangular obstacle. rect 2g in length, 2h in width, and Ω in rotation angle.

[0061] Step S2: For the environmental obstacles input in Step S1, determine the obstacle that currently has the greatest impact on the path to the target point, determine the degree of the circumscribed hyperellipse of the rectangle based on the set porosity, and construct the even-order hyperellipse of the envelope of the rectangular obstacle.

[0062] (2-1) For the rectangular obstacle list determined in step S1, determine the number of the obstacle that is currently the most obstructive to passage based on the line connecting the current point and the endpoint.

[0063] (2-2) Determine the degree n of the circumscribed hyperellipse of the obstacle based on the set porosity λ, and determine the major semi-axis a and minor semi-axis b of the hyperellipse.

[0064] Porosity is defined as:

[0065]

[0066] Among them, S rectangle S represents the area of ​​the rectangle. superellipse This represents the area of ​​the even-order hyperellipse circumscribed in the rectangle.

[0067] Even-order hyperellipses are defined as follows:

[0068]

[0069] Where 'a' represents the major axis length of the hyperellipse, 'b' represents the minor axis length, 'n' represents the even-order hyperellipse, and 'mod' represents the modulo function. Since the vertices of the rectangle lie on the hyperellipse, therefore... and

[0070] The area of ​​a hyperellipse is defined as:

[0071]

[0072] Substituting ab into the equation, we get:

[0073]

[0074] Among them, the Gamma function

[0075] therefore, At this point, the equation holds if and only if

[0076]

[0077] The order n of the even-order hyperellipse can be obtained from the set porosity, and then the major semi-axis a and minor semi-axis b of the hyperellipse can be determined based on formula (5).

[0078] (2-3) such as Figure 3 As shown, solving the hyperellipse equation yields the trajectory point P of the even-order hyperellipse circumscribed by the obstacle. SEOH .

[0079] Step S3: As Figure 4 As shown, a local coordinate system OT is established between the even-order hyperellipse of the current obstacle envelope generated in step S2 and the target point.

[0080] (3-1) Connect the center of the rectangular obstacle determined in step S2 with the target point, and use the X-axis of the local coordinate system OT as the x-axis. OT ;

[0081] (3-2) The obstacle circumscribed even-order hyperellipse P obtained based on step S2 SEOH Find the two tangent points Q1 and Q2 between the target point and the hyperellipse. The line connecting Q1 and Q2 is used as the y-axis of the local coordinate system OT. OT .

[0082] (3-3) Based on the local coordinate system x-axis calculated in step (3-1) and the local coordinate system y-axis calculated in step (3-2), determine the origin (x-axis) of the local coordinate system OT. OT ,y OT ), determine X OT The angle α between the X-axis and the global coordinate system determines the X-axis. OT With Y OT The included angle

[0083] (3-4) Based on the angle α and angle obtained in step (3-3) Obtain the transformation relationship between the global coordinate system and the local coordinate system.

[0084] in,

[0085]

[0086] Step S4: Based on the local coordinate system OT calculated in step S3, calculate the coordinates of the current point in this coordinate system and determine the solution direction r of the limit cycle.

[0087] (4-1) Based on the transformation relationship between the global coordinate system and the local coordinate system obtained in step S3 Set the current starting point (x) s_w ,y s_w Transform to the local coordinate system (x) s_OT ,y s_OT ).

[0088] (4-2) Based on the y obtained in step (4-1) s_OT Determine the solution direction r of the even-order hyperelliptic limit cycle. If y s_OT >0, r=1 indicates solving the trajectory clockwise, if y s_OT ≤0, r=-1, indicates solving the trajectory counterclockwise.

[0089] Step S5: Based on the solution direction r determined in step S4 and the even-order hyperellipse parameters determined in step S2, establish the limit cycle equation of the even-order hyperellipse starting from the current point and solve for the trajectory.

[0090] (5-1) Based on the rectangle center Coor determined in step S2 rect Establish the limit cycle equation of an even-order hyperellipse using the major semi-axis a, minor semi-axis b, and order n of the hyperellipse.

[0091] The differential equation for the even-order hyperelliptic limit ring is:

[0092]

[0093] in, {n≥2,mod(n,2)=0}.

[0094] (5-2) such as Figure 5 As shown, based on the current starting point (x) s_w ,y s_w Solve the equation of the even-order hyperelliptic limit cycle to obtain the hyperelliptic limit cycle trajectory P starting from the current point. EOHLC_w .

[0095] Step S6: Based on the even-order hyperelliptical limit cycle trajectory obtained in step S5, transform it to the local coordinate system OT determined in step S3, and then determine the coordinates of the current even-order hyperelliptical limit cycle trajectory breakaway point.

[0096] (6-1) Based on the transformation matrix obtained in step S3 The hyperelliptic limit cycle trajectory P obtained in step S5 EOHLC_w Transform to the local coordinate system OT to obtain P EOHLC_OT .

[0097] (6-2) The trajectory P in the local coordinate system obtained based on step (6-1) EOHLC_OT When the x-value in the local coordinate system changes from negative to positive, that point is the breakaway point p of the limit cycle trajectory. escape .

[0098] Step S7: For the coordinates of the limit loop break-off point obtained in step S6, determine whether the point and the endpoint are other obstacles. If so, go to step S2 and repeat steps S2-S7.

[0099] (7-1) Based on the current even-order hyperelliptic limit cycle trajectory decoupling point p obtained in step S6 escape Determine whether the point crosses other obstacles between itself and the target point. If it does, proceed to step S2.

[0100] (7-2) If no other obstacles are overcome, proceed to step S8.

[0101] Step S8: As Figure 6 As shown, based on the coordinates of the limit cycle break-off point obtained in step S6, the path between this point and the endpoint is planned, thus completing the entire path planning process.

[0102] (8-1) The breakaway point p of the limit cycle trajectory solved by S6 escape The adaptive linear interpolation method is used to solve the path from the point to the target point, thus completing the entire path planning process.

[0103] The advantages of this invention over traditional elliptic limit cycle-based path planning are verified through the following experiments:

[0104] (1) Feasibility comparison of path planning in narrow structured environments

[0105] like Figure 7 As shown, in this structured and crowded environment, starting from the origin, the traditional elliptical limit cycle trajectory will collide with other obstacles. However, the trajectory planned by even-order hyperellipse is able to navigate through the narrow environment and successfully plan a path due to its smaller porosity.

[0106] (2) Comparison of path planning length

[0107] like Figure 8As shown, starting from the same point, the path length planned by the traditional elliptical limit cycle method to reach the destination is 41.2m, while the trajectory length planned by the even-order hyperelliptical limit cycle method is 40.1m. It can be seen that while ensuring smoothness and safety, the path planned by the even-order hyperelliptical limit cycle method is shorter.

[0108] It should be noted that the above content merely illustrates the technical concept of the present invention and should not be construed as limiting the scope of protection of the present invention. For those skilled in the art, various improvements and modifications can be made without departing from the principle of the present invention, and all such improvements and modifications fall within the scope of protection of the claims of the present invention.

Claims

1. A method for planning even-order hyperelliptic limit cycles in structured environments, characterized in that, The method includes the following steps: S1: Obtain a structured two-dimensional prior map, dilate the safe distance of obstacles in the map, rectangularize it, and extract the rectangle parameters; S2: For the environmental obstacles input in step S1, determine the obstacle that has the greatest impact on the path to the target point, determine the number of the circumscribed hyperellipse of the rectangle based on the set porosity, and construct the even-order hyperellipse of the rectangular obstacle envelope. (2-1) For the rectangular obstacle list determined in step S1, determine the number of the obstacle that is currently the most obstructive to passage based on the line connecting the current point and the endpoint; (2-2) Based on the set porosity Determine the number of times the circumscribed hyperellipse of the obstacle is determined. Determine the semi-major axis of the hyperellipse With short half shaft ; Porosity is defined as: （1）； in, Represents the area of ​​the rectangle. This represents the area of ​​the even-order hyperellipse circumscribed in front of the rectangle; Even-order hyperellipses are defined as follows: （2）； in, Indicates the length of the major axis of the hyperellipse. Indicates the length of the minor axis. Indicates the even-order hyperellipse. Let represent the modulo function; since the vertices of the rectangle lie on the hyperellipse, we have ,and ; This represents half the length of a rectangular obstacle. This represents half the width of a rectangular obstacle. The area of ​​a hyperellipse is defined as: （3）； Will Substituting, we get: （4）； Among them, the Gamma function ; therefore, At this point, the equation holds if and only if （5）； The order of the even-order hyperellipse can be obtained from the set porosity. Then, based on formula (5), the semi-major axis of the hyperellipse is determined. and short half shaft ; (2-3) Solve the equation of the hyperellipse to obtain the trajectory points of the even-order hyperellipse circumscribed by the obstacle. ; S3: Establish a local coordinate system between the even-order hyperellipse of the current obstacle envelope generated in step S2 and the target point. ; S4: Local coordinate system calculated based on step S3 Calculate the coordinates of the current point in this coordinate system to determine the solution direction of the limit cycle. ; S5: Based on the solution direction determined in step S4 and the even-order hyperellipse parameters determined in step S2, establish the limit cycle equation of the even-order hyperellipse starting from the current point and solve for the trajectory. S6: Based on the even-order hyperelliptic limit cycle trajectory obtained in step S5, transform it to the local coordinate system determined in step S3. Next, the coordinates of the breakaway point of the current even-order hyperelliptical limit cycle trajectory are determined; S7: For the coordinates of the limit loop break-off point obtained in step S6, determine whether the point and the endpoint are other obstacles. If so, go to step S2 and repeat steps S2-S7. S8: Based on the coordinates of the limit cycle break-off point obtained in step S7, plan the path between this point and the endpoint to complete the entire path planning process.

2. The method for planning even-order hyperelliptic limit cycles in structured environments according to claim 1, characterized in that, In step S3, the specific details are as follows: Includes the following processes: (3-1) Connect the center of the rectangular obstacle determined in step S2 with the target point to form a local coordinate system. of axis ; (3-2) Obstacle circumscribed even-order hyperellipse based on step S2 Find the two tangent points between the target point and the hyperellipse. ,connect and The line connecting them serves as a local coordinate system. of axis ; (3-3) Local coordinate system calculated based on step (3-1) The axes and the local coordinate system calculated in step (3-2) Axis, determining the local coordinate system The origin ,Sure With global coordinate system Angle between axes ,Sure and The included angle ; (3-4) Angles obtained based on step (3-3) with horns This yields the transformation relationship between the global coordinate system and the local coordinate system. .

3. The method for planning even-order hyperelliptic limit cycles in structured environments according to claim 1, characterized in that, In step S4, the specific details are as follows: Includes the following processes: (4-1) Based on the transformation relationship between the global coordinate system and the local coordinate system obtained in step S3 , will the current starting point Transform to local coordinate system ; (4-2) Based on step (4-1) Determine the solution direction r of the even-order hyperelliptic limit cycle; if , ,if , .

4. The method for planning even-order hyperelliptic limit cycles in structured environments according to claim 1, characterized in that, In step S5, the specific details are as follows: Includes the following processes: (5-1) Based on the center of the rectangle determined in step S2 semi-major axis of the hyperellipse short half shaft Hyperelliptic order Establish the equations for the limit ring of an even-order hyperellipse; (5-2) Based on the current starting point Solve the equation of the even-order hyperelliptic limit cycle to obtain the hyperelliptic limit cycle trajectory starting from the current point. .

5. The method for planning even-order hyperelliptic limit cycles in structured environments according to claim 1, characterized in that, In step S6, the specific details are as follows: Includes the following processes: (6-1) Based on the transformation matrix obtained in step S3 The hyperelliptic limit cycle trajectory obtained in step S5 Transform to local coordinate system Get ; (6-2) Trajectory in the local coordinate system obtained based on step (6-1) The breakaway point of the limit cycle trajectory is determined based on the sign of the x-value of the trajectory point. .

6. The method for planning even-order hyperelliptic limit cycles in structured environments according to claim 1, characterized in that, Step S7 specifically includes the following processes. (7-1) The breakaway point of the current even-order hyperelliptic limit cycle trajectory obtained based on step S6 Determine whether the point crosses other obstacles between itself and the target point. If it does, proceed to step S2. (7-2) If no other obstacles are crossed, the adaptive linear interpolation method is used to solve the path from the point to the target point.

Images