An unknown region path planning method and system

Abstract

The application provides a path planning method and system in an unknown area, which utilizes a refinement method, a design method and a depth-first search algorithm (DFS) to realize automatic generation of a road distribution map of the unknown area and a path planning method in the area, and provides strong data support for user navigation and application. The generation of a backbone map of the area is obtained through the refinement method, the road distribution map of the area is automatically generated based on the backbone map through the design method, the automatic extraction of road key points is realized, the planning of all possible paths between any two points in the area is realized based on the road key points and given start and end points through the depth-first search algorithm (DFS), and effective support is provided for user navigation in the area. The method not only realizes the rapid construction of the road distribution map of the unknown area in a short time, but also provides effective support for the user to select the best route between the starting point and the target point.

Description

Technical Field

[0001] This invention proposes a method and system for automatically generating road distribution maps and planning routes in unknown areas, belonging to the fields of computer science and navigation. Background Technology

[0002] Path planning is of great significance in autonomous driving, drone navigation, game development, logistics management, traffic control, and earthquake relief. Depth-First Search (DFS) is one of the mainstream methods for path planning. Its basic principle is to use graph traversal to plan all possible paths from the starting point to the destination, based on given start and end points and key points on the map (road intersections, road turns, etc.). However, this method heavily relies on the local map and requires manually determined key points (road intersections, road turns, etc.) during the planning process. This leads to limitations such as incomplete path planning and an inability to plan routes in unknown areas, severely impacting navigation convenience and the selection of the optimal route. Summary of the Invention

[0003] This invention proposes a method for automatically generating road distribution maps of unknown areas and providing all possible paths for user navigation in those areas, utilizing a refinement method and a depth-first search algorithm (DFS).

[0004] The technical solution of this invention is a path planning method for unknown areas, comprising the following steps:

[0005] Step 1: Use sensors to collect 3D point cloud data of the area.

[0006] Step 2: Conversion of 3D point cloud data to 2D map.

[0007] Step 3: Automatic generation of 2D map road map and automatic extraction of key points on the road. Key points are located at road intersections or road turning points.

[0008] Step 4: Randomly select two points A and B on the 2D map, and complete the shortest path planning from point A to point B on the 2D map.

[0009] Furthermore, in step 1, cameras and radar are used to collect 3D point cloud data of the area.

[0010] Furthermore, the specific implementation method of step 2 is as follows;

[0011] Step 2.1, define a 3x3 orthogonal projection matrix P, as follows:

[0012]

[0013] Step 2.2: Based on the projection matrix P constructed in Step 2.1, iterate through and process the coordinates D of each point in the 3D point cloud data. i =(x i y i , z i This transforms each three-dimensional coordinate D into a projected coordinate I on a two-dimensional plane. i =(u i v i ), as shown in the following formula:

[0014] I i =(u i ,v i ) = PD i =P(x i y i , z i )

[0015] Among them, u i v i The x and y coordinates of the two-dimensional plane;

[0016] Step 2.3: Summarize all the two-dimensional plane projection coordinates u and v obtained in Step 2.2. Select the leftmost, rightmost, topmost, and bottommost coordinate values ​​of the projection coordinates. Extend each of the four coordinate values ​​outward by a length n and draw parallel lines to determine the rectangular area where the two-dimensional plane projection is located. Use this area as the size of the 2D map. The length C of the rectangular area is the width W of the 2D map, and the width K of the rectangular area is the height H of the 2D map.

[0017] Step 2.4: Construct an image data with a width of W and a height of H, and draw the two-dimensional planar projection coordinate data obtained in Step 2.2 into the image data;

[0018] Step 2.5: Determine the RGB values ​​of the 3D point cloud data, and based on the projection relationship determined in Step 2.1, assign the color information of the 3D point cloud to the image data drawn in Step 2.4 to complete the generation of the final 2D map.

[0019] Furthermore, the specific implementation method of step 3 is as follows;

[0020] Step 3.1: Process the 2D map using an edge detection algorithm to obtain objects with boundaries, such as buildings and walls, in the 2D map, and define them as obstacles;

[0021] Step 3.2: Based on the obstacles identified in Step 3.1, the 2D map is processed using a refinement method. The resulting image is the skeleton map corresponding to the 2D map.

[0022] Step 3.3: Use Euclidean distance to calculate the distance between each node in the backbone graph generated in Step 3.2. If the distance between nodes is less than D1, then mark both nodes simultaneously.

[0023] Step 3.4: Link each node separately, and use the point-to-line distance formula to calculate the distance between other nodes and the link. If the distance between other nodes and the link is less than D2, then mark the node.

[0024] Step 3.5: Based on the links constructed in Step 3.4, calculate the distance between each link. If the distance between the links is less than D3, mark the corresponding nodes of the two links.

[0025] Step 3.6: Process the nodes marked in steps 3.3-3.5 in sequence. Draw a circle with the node as the center and M as the radius. If other nodes fall into the circle, calculate the coordinates of all nodes in the circle on average, keep the calculated nodes, and delete the other nodes. If there are no other nodes, continue to process the other nodes.

[0026] Step 3.7: After processing, a walkable road route map, i.e. a road distribution map, can be obtained for the area. The connecting lines of each node are the walkable roads in the 2D map, and each node is the intersection or bend between roads.

[0027] Furthermore, the specific implementation method of step 4 is as follows;

[0028] Step 4.1: Number each intersection or bend in the road distribution map;

[0029] Step 4.2: Use Euclidean distance to calculate the distance between point A and each intersection or bend on the road map, select the intersection or bend closest to point A, and name it point C;

[0030] Step 4.3: Use Euclidean distance to calculate the distance between point B and each intersection or bend on the road map, select the intersection or bend closest to point B, and name it point D;

[0031] Step 4.4: Use the depth-first search algorithm to generate all possible trajectories from point C to point D in the road distribution map;

[0032] Step 4.5: Add point A before all the trajectories generated in step 4.4, that is, connect A and C, and add point B at the end, that is, connect B and D, to complete the generation of all possible motion trajectories from point A to point B, that is, to complete the planning of the entire path from A to C and from B to D.

[0033] Step 4.6: Use Euclidean distance to calculate the geometric distance between each intersection or turn of the trajectory, excluding the starting point, and the previous intersection or turn.

[0034] Step 4.7: Use summation to calculate the total length of the trajectory geometry;

[0035] Step 4.8: Use steps 4.6-4.7 to process all possible motion trajectories and obtain the lengths d1...di...dn of each trajectory;

[0036] Step 4.9: Using the total length of the trajectory set as the criterion, obtain the geometric coefficient Kg of the nearest motion trajectory, as shown in the following formula:

[0037]

[0038] Where di is the total geometric length of the i-th trajectory, and min(d1...di...dn) is the trajectory with the smallest geometric distance among all trajectories;

[0039] Step 4.10: Count the number of nodes S for each trajectory and use it as the criterion to obtain the nearest trajectory convenience coefficient Kc, as shown in the following formula:

[0040]

[0041] Where si is the number of nodes in the i-th trajectory, and min(s1...si...sn) is the minimum number of nodes among all trajectories;

[0042] Step 4.11: Based on the motion trajectory geometric coefficient Kg and the motion trajectory convenience coefficient Kc obtained in Step 4.9, the nearest motion trajectory coefficient K for each trajectory is obtained, as shown in the following formula:

[0043] K = Kg * Kc

[0044] Where Kg is the geometric coefficient of the closest motion of the trajectory, and Kc is the convenience coefficient of the closest motion of the trajectory. The closer K is to 1, the more likely the trajectory is the closest motion trajectory.

[0045] Step 4.12: Select the trajectory whose motion trajectory coefficient K is closest to 1, that is, the shortest motion route from point A to point B.

[0046] This invention also provides a path planning system for unknown areas, comprising the following modules:

[0047] The data acquisition module is used to collect 3D point cloud data of the area using sensors;

[0048] The data conversion module is used to convert 3D point cloud data to 2D maps.

[0049] The key point extraction module is used for the automatic generation of 2D map road maps and the automatic extraction of key points on the roads. Key points are located at road intersections or road turning points.

[0050] The path planning module is used to randomly select two points A and B in a 2D map and complete the shortest path planning from point A to point B in the 2D map.

[0051] Furthermore, the data acquisition module uses cameras and radar to collect 3D point cloud data of the area.

[0052] Furthermore, the specific implementation method of the data conversion module is as follows;

[0053] Step 2.1, define a 3x3 orthogonal projection matrix P, as follows:

[0054]

[0055] Step 2.2: Based on the projection matrix P constructed in Step 2.1, iterate through and process the coordinates D of each point in the 3D point cloud data. i =(x i y i , z i This transforms each three-dimensional coordinate D into a projected coordinate I on a two-dimensional plane. i =(u i v i ), as shown in the following formula:

[0056] I i =(u i ,v i ) = PD i =P(x i y i , z i )

[0057] Among them, u i v i The x and y coordinates of the two-dimensional plane;

[0058] Step 2.3: Summarize all the two-dimensional plane projection coordinates u and v obtained in Step 2.2. Select the leftmost, rightmost, topmost, and bottommost coordinate values ​​of the projection coordinates. Extend each of the four coordinate values ​​outward by a length n and draw parallel lines to determine the rectangular area where the two-dimensional plane projection is located. Use this area as the size of the 2D map. The length C of the rectangular area is the width W of the 2D map, and the width K of the rectangular area is the height H of the 2D map.

[0059] Step 2.4: Construct an image data with a width of W and a height of H, and draw the two-dimensional planar projection coordinate data obtained in Step 2.2 into the image data;

[0060] Step 2.5: Determine the RGB values ​​of the 3D point cloud data, and based on the projection relationship determined in Step 2.1, assign the color information of the 3D point cloud to the image data drawn in Step 2.4 to complete the generation of the final 2D map.

[0061] Furthermore, the specific implementation method of the key point extraction module is as follows;

[0062] Step 3.1: Process the 2D map using an edge detection algorithm to obtain objects with boundaries, such as buildings and walls, in the 2D map, and define them as obstacles;

[0063] Step 3.2: Based on the obstacles identified in Step 3.1, the 2D map is processed using a refinement method. The resulting image is the skeleton map corresponding to the 2D map.

[0064] Step 3.3: Use Euclidean distance to calculate the distance between each node in the backbone graph generated in Step 3.2. If the distance between nodes is less than D1, then mark both nodes simultaneously.

[0065] Step 3.4: Link each node separately, and use the point-to-line distance formula to calculate the distance between other nodes and the link. If the distance between other nodes and the link is less than D2, then mark the node.

[0066] Step 3.5: Based on the links constructed in Step 3.4, calculate the distance between each link. If the distance between the links is less than D3, mark the corresponding nodes of the two links.

[0067] Step 3.6: Process the nodes marked in steps 3.3-3.5 in sequence. Draw a circle with the node as the center and M as the radius. If other nodes fall into the circle, calculate the coordinates of all nodes in the circle on average, keep the calculated nodes, and delete the other nodes. If there are no other nodes, continue to process the other nodes.

[0068] Step 3.7: After processing, a walkable road route map, i.e. a road distribution map, can be obtained for the area. The connecting lines of each node are the walkable roads in the 2D map, and each node is the intersection or bend between roads.

[0069] Furthermore, the specific implementation method of the path planning module is as follows;

[0070] Step 4.1: Number each intersection or bend in the road distribution map in Step 3.7;

[0071] Step 4.2: Use Euclidean distance to calculate the distance between point A and each intersection or bend on the road map, select the intersection or bend closest to point A, and name it point C;

[0072] Step 4.3: Use Euclidean distance to calculate the distance between point B and each intersection or bend on the road map, select the intersection or bend closest to point B, and name it point D;

[0073] Step 4.4: Use the depth-first search algorithm to generate all possible trajectories from point C to point D in the road distribution map;

[0074] Step 4.5: Add point A before all the trajectories generated in step 4.4, that is, connect A and C, and add point B at the end, that is, connect B and D, to complete the generation of all possible motion trajectories from point A to point B, that is, to complete the planning of the entire path from A to C and from B to D.

[0075] Step 4.6: Use Euclidean distance to calculate the geometric distance between each intersection or turn of the trajectory, excluding the starting point, and the previous intersection or turn.

[0076] Step 4.7: Use summation to calculate the total length of the trajectory geometry;

[0077] Step 4.8: Use steps 4.6-4.7 to process all possible motion trajectories and obtain the lengths d1...di...dn of each trajectory;

[0078] Step 4.9: Using the total length of the trajectory set as the criterion, obtain the geometric coefficient Kg of the nearest motion trajectory, as shown in the following formula:

[0079]

[0080] Where di is the total geometric length of the i-th trajectory, and min(d1...di...dn) is the trajectory with the smallest geometric distance among all trajectories;

[0081] Step 4.10: Count the number of nodes S for each trajectory and use it as the criterion to obtain the nearest trajectory convenience coefficient Kc, as shown in the following formula:

[0082]

[0083] Where si is the number of nodes in the i-th trajectory, and min(s1...si...sn) is the minimum number of nodes among all trajectories;

[0084] Step 4.11: Based on the motion trajectory geometric coefficient Kg and the motion trajectory convenience coefficient Kc obtained in Step 4.9, the nearest motion trajectory coefficient K for each trajectory is obtained, as shown in the following formula:

[0085] K = Kg * Kc

[0086] Where Kg is the geometric coefficient of the closest motion of the trajectory, and Kc is the convenience coefficient of the closest motion of the trajectory. The closer K is to 1, the more likely the trajectory is the closest motion trajectory.

[0087] Step 4.12: Select the trajectory whose motion trajectory coefficient K is closest to 1, that is, the shortest motion route from point A to point B.

[0088] This invention utilizes refinement methods, design methods, and a depth-first search (DFS) algorithm to achieve automatic generation of road distribution maps for unknown areas and route planning within those areas, providing strong data support for user navigation and applications. The refinement method generates a backbone map of the region; the design method automatically generates a road distribution map based on the backbone map and automatically extracts key road points; and the DFS algorithm plans all possible paths between any two points within the region based on the key road points and given start and end points, providing effective support for user navigation in the area. This method not only enables rapid construction of road distribution maps for unknown areas in a short time but also effectively supports users in selecting the optimal route between their starting and destination points. Attached Figure Description

[0089] Figure 1 This is a flowchart of the regional path planning method according to an embodiment of the present invention.

[0090] Figure 2 This is a rendering of the 3D point cloud data of the area where the example of this invention is located.

[0091] Figure 3 This is a rendering of the 3D point cloud data of the area where the example of this invention is located projected onto a 2D plane.

[0092] Figure 4 This is a map of walkable roads, i.e., a road distribution map, in the area where the present invention is located.

[0093] Figure 5 This is a road distribution map numbered according to an example of the present invention.

[0094] Figure 6 This is a map showing all possible motion routes from 3 o'clock to 11 o'clock in this invention.

[0095] Figure 7 This is an example of the present invention showing the shortest movement route from 3 o'clock to 11 o'clock. Detailed Implementation

[0096] The technical solution of the present invention will be further described below with reference to the accompanying drawings.

[0097] The key to this invention lies in using refinement methods, design methods, and a depth-first search (DFS) algorithm to provide users with all possible navigation paths. This invention utilizes point cloud data to obtain a 2D map, uses refinement methods to generate a backbone map of the region, uses design methods to automatically generate a road distribution map of the region based on the backbone map, and automatically extracts key road points. Finally, it uses the depth-first search (DFS) algorithm to plan all possible paths within the given starting and ending points, providing effective support for user navigation in the region.

[0098] This embodiment provides a method for path planning in unknown areas; for details, please refer to [link / reference]. Figure 1 The present invention will be specifically illustrated using three-dimensional point cloud data of a certain region. The specific process is as follows:

[0099] Step 1: Use sensor data such as cameras and radar to collect 3D point cloud data for a specific area, such as... Figure 1 As shown.

[0100] Step 2, complete the conversion from 3D point cloud data to 2D map, including the following sub-steps:

[0101] Step 2.1, define a 3*3 orthogonal projection matrix P, as shown in Equation 1.

[0102]

[0103] Step 2.2: Based on the projection matrix P constructed in Step 2.1, iterate through and process the coordinates D of each point in the 3D point cloud data. i =(x i y i , z i This transforms each three-dimensional coordinate D into a projected coordinate I on a two-dimensional plane. i =(u i v i ), as shown in equation two below

[0104]

[0105] Among them, u i v i These are the horizontal and vertical coordinates of a two-dimensional plane.

[0106] Step 2.3: Summarize all the two-dimensional plane projection coordinates u and v obtained in Step 2.2. Select the leftmost, rightmost, topmost, and bottommost coordinate values ​​of the projection coordinates. Extend each of the four coordinate values ​​outward by 1m and draw parallel lines to determine the rectangular area where the two-dimensional plane projection is located. Use this area as the size of the 2D map. The length of the rectangular area is 320m, which is the width of the 2D map. The width of the rectangular area is 160m, which is the height of the 2D map.

[0107] Step 2.4: Construct an image data with a width of 320 and a height of 160, and draw the two-dimensional planar projection coordinate data obtained in Step 2.2 into the image data.

[0108] Step 2.5: Determine the RGB values ​​of the 3D point cloud data, and based on the projection relationship determined in Step 2.1, assign the color information of the 3D point cloud to the image data drawn in Step 2.4, thus completing the generation of the final 2D map. Figure 3 As shown.

[0109] Step 3, complete the automatic generation of the 2D map road map and the automatic extraction of key points on the roads (road intersections, road turning points, etc.), including the following sub-steps,

[0110] Step 3.1: Process the 2D map using an edge detection algorithm to obtain objects with boundaries such as buildings and walls in the 2D map, and define them as obstacles.

[0111] Step 3.2: Based on the obstacles identified in Step 3.1, the 2D map is processed using a refinement method. The resulting image is the skeleton map corresponding to the 2D map.

[0112] Step 3.3: Use Euclidean distance to calculate the distance between each node in the backbone graph generated in Step 3.2. If the distance between nodes is less than 5, then mark both nodes simultaneously.

[0113] Step 3.4: Link each node separately, and use the point-to-line distance formula to calculate the distance between other nodes and the link. If the distance between other nodes and the link is less than 5, then mark the node.

[0114] Step 3.5: Based on the links constructed in Step 3.4, calculate the distance between each link. If the distance between the links is less than 5, mark the corresponding nodes of the two links.

[0115] Step 3.6: Process the nodes marked in steps 3.3-3.5 sequentially. Draw a circle with the node as the center and a radius of 6. If other nodes fall within this circle, calculate the average coordinates of all nodes within the circle, keep the calculated nodes, and delete the other nodes. If no other nodes fall within this circle, continue processing the remaining nodes.

[0116] Step 3.7: After processing, a walkable road map, or road distribution map, is obtained for the area. The connecting lines of each node represent walkable roads in this 2D map, and each node represents an intersection or bend between roads. For example... Figure 4 As shown. Step 4, complete the shortest path planning from point 3 to point 11 on the 2D map, including the following sub-steps:

[0117] Step 4.1: Number each intersection or bend in the road distribution map from Step 3.7, such as... Figure 5 As shown.

[0118] Step 4.2: Use Euclidean distance to calculate the distance between point 3 (i.e., point A) and each intersection or bend on the road map. Select the intersection or bend closest to point 3 and name it point 3 (i.e., point C).

[0119] Step 4.3: Use Euclidean distance to calculate the distance between point 11 (i.e., point B) and each intersection or bend on the road map. Select the intersection or bend closest to point 11 and name it point 11 (i.e., point D).

[0120] Step 4.4: Use the Depth-First Search (DFS) algorithm to generate all possible trajectories from point 3 to point 11 in the road distribution map, such as... Figure 6 As shown.

[0121] For convenience, this embodiment of the invention directly uses the intersection nodes as the start and end points, with point 3 as the start point and point 11 as the end point. Then, path planning is performed from point 3 to point 11. Since points A and B directly use nodes (intersection points), the operations of connecting A and C and connecting B and D can be ignored. If points A and B are not intersection nodes, then it is necessary to connect A and C and connect B and D, and then complete the generation of all possible movement trajectories from point A to point B. This step is not set for simplicity.

[0122] Step 4.5: Use Euclidean distance to calculate the geometric distance between each intersection or turn of the trajectory, excluding the starting point, and the previous intersection or turn.

[0123] Step 4.6: Use summation to calculate the total length of the trajectory geometry.

[0124] Step 4.7: Use steps 4.5-4.6 to process all possible motion trajectories and obtain the lengths of each motion trajectory: 110, 70, 70, 70, 80, 40, 70, 70, 80, 40, 70, 70, 80, 40, 40.

[0125] Step 4.8: Using the total length of the trajectory set as the criterion, obtain the geometric coefficient Kg of the nearest motion trajectory, as shown in the following formula:

[0126]

[0127]

[0128]

[0129]

[0130]

[0131]

[0132]

[0133]

[0134]

[0135]

[0136]

[0137]

[0138]

[0139]

[0140]

[0141]

[0142]

[0143] Step 4.9: Count the number of nodes S for each trajectory and use it as the criterion to obtain the nearest trajectory convenience coefficient Kc, as shown in the following formula:

[0144]

[0145]

[0146]

[0147]

[0148]

[0149]

[0150]

[0151]

[0152]

[0153]

[0154]

[0155]

[0156]

[0157]

[0158]

[0159]

[0160]

[0161] Step 4.10: Based on the motion trajectory geometric coefficient Kg from Step 4.8 and the motion trajectory convenience coefficient Kc from Step 4.9, obtain the nearest motion trajectory coefficient K for each trajectory. As shown in the following formula.

[0162] K = Kg * Kc

[0163] K1=Kg1*Kc1=2.75*1.8333=5.04157

[0164] K2=Kg2*Kc2=1.75*1.5=2.625

[0165] K3=Kg3*Kc3=1.75*1.3333=2.33328

[0166] K4=Kg4*Kc4=1.75*1.3333=2.33328

[0167] K5 = Kg5 * Kc5 = 2 * 1.5 = 3

[0168] K6 = Kg6 * Kc6 = 1 * 1 = 1

[0169] K7=Kg7*Kc7=1.75*1.3333=2.33328

[0170] K8=Kg8*Kc8=1.75*1.3333=2.33328

[0171] K9 = Kg9 * Kc9 = 2 * 1.5 = 3

[0172] K10=Kg10*Kc10=1*1.1667=1.1667

[0173] K11=Kg11*Kc11=1.75*1.3333=2.33328

[0174] K12=Kg12*Kc12=1.75*1.3333=2.33328

[0175] K13 = Kg13 * Kc13 = 2 * 1.5 = 3

[0176] K14=Kg14*Kc14=1*1.1667=1.1667

[0177] K15 = Kg15 * Kc15 = 1 * 1 = 1

[0178] K16 = Kg16 * Kc16 = 1 * 1 = 1

[0179] Step 4.11: Select the trajectory whose motion trajectory coefficient K is closest to 1, which is the shortest path from point A to point B that we want to find, such as... Figure 7 As shown.

[0180] Step 4.8: If you want to find the shortest route from 3 o'clock to 11 o'clock with the fewest turns, you can select P16.

[0181] The method provided by this invention can be implemented using computer software technology. Embodiments of this invention also provide a path planning system for unknown areas, comprising the following modules:

[0182] The data acquisition module is used to collect 3D point cloud data of the area using sensors;

[0183] The data conversion module is used to convert 3D point cloud data to 2D maps.

[0184] The key point extraction module is used for the automatic generation of 2D map road maps and the automatic extraction of key points on the roads. Key points are located at road intersections or road turning points.

[0185] The path planning module is used to randomly select two points A and B in a 2D map and complete the shortest path planning from point A to point B in the 2D map.

[0186] The specific implementation methods of each module are the same as those of each step, and will not be described in this invention.

[0187] The 2D map used in the path planning of this invention is converted from 3D point cloud data; however, if a 2D map exists in the area, the method is equally applicable. Although the distance calculation method used in this invention is the Euclidean method, other distance calculation methods can also be used. The specific embodiments described herein are merely illustrative of the spirit of the invention. Those skilled in the art can make various modifications or additions to the described specific embodiments or use similar methods to replace them, without departing from the spirit of the invention or exceeding the scope defined by the appended claims.

Claims

1. A path planning method for unknown areas, characterized in that, Includes the following steps: Step 1: Use sensors to collect 3D point cloud data of the area. Step 2: Conversion of 3D point cloud data to 2D map. Step 3: Automatic generation of 2D map road map and automatic extraction of key points on the road. Key points are located at road intersections or road turning points. Step 4: Randomly select two points A and B on the 2D map, and complete the shortest path planning from point A to point B on the 2D map. The specific implementation method of step 4 is as follows; Step 4.1: Number each intersection or bend in the road distribution map; Step 4.2: Use Euclidean distance to calculate the distance between point A and each intersection or bend on the road map, select the intersection or bend closest to point A, and name it point C; Step 4.3: Use Euclidean distance to calculate the distance between point B and each intersection or bend on the road map, select the intersection or bend closest to point B, and name it point D; Step 4.4: Use the depth-first search algorithm to generate all possible trajectories from point C to point D in the road distribution map; Step 4.5: Add point A before all the trajectories generated in step 4.4, that is, connect A and C, and add point B at the end, that is, connect B and D, to complete the generation of all possible motion trajectories from point A to point B, that is, to complete the planning of the entire path from A to C and from B to D. Step 4.6: Use Euclidean distance to calculate the geometric distance between each intersection or turn of the trajectory, excluding the starting point, and the previous intersection or turn. Step 4.7: Use summation to calculate the total length of the trajectory geometry; Step 4.8: Use steps 4.6-4.7 to process all possible motion trajectories and obtain the lengths d1...di...dn of each trajectory; Step 4.9: Using the total length of the trajectory set as the criterion, obtain the geometric coefficient Kg of the nearest motion trajectory, as shown in the following formula: Where di is the total geometric length of the i-th trajectory, and min(d1...di...dn) is the trajectory with the smallest geometric distance among all trajectories; Step 4.10: Count the number of nodes S for each trajectory and use it as the criterion to obtain the nearest trajectory convenience coefficient Kc, as shown in the following formula: Where si is the number of nodes in the i-th trajectory, and min(s1...si...sn) is the minimum number of nodes among all trajectories; Step 4.11: Based on the motion trajectory geometric coefficient Kg and the motion trajectory convenience coefficient Kc obtained in Step 4.9, the nearest motion trajectory coefficient K for each trajectory is obtained, as shown in the following formula: Where Kg is the geometric coefficient of the closest motion of the trajectory, and Kc is the convenience coefficient of the closest motion of the trajectory. The closer K is to 1, the more likely the trajectory is the closest motion trajectory. Step 4.12: Select the trajectory whose motion trajectory coefficient K is closest to 1, that is, the shortest motion route from point A to point B.

2. The method for path planning in an unknown area as described in claim 1, characterized in that: Step 1 involves using a camera and radar to collect 3D point cloud data of the area.

3. The method for path planning in an unknown area as described in claim 1, characterized in that: The specific implementation method of step 2 is as follows; Step 2.1, define a 3x3 orthogonal projection matrix P, as follows: Step 2.2: Based on the projection matrix P constructed in Step 2.1, iterate through and process the coordinates of each point in the 3D point cloud data. Transform each three-dimensional coordinate D into a projected coordinate on a two-dimensional plane. As shown in the following formula: in, The x and y coordinates of the two-dimensional plane; Step 2.3: Summarize all the two-dimensional plane projection coordinates u and v obtained in Step 2.

2. Select the leftmost, rightmost, topmost, and bottommost coordinate values ​​of the projection coordinates. Extend each of the four coordinate values ​​outward by a length n and draw parallel lines to determine the rectangular area where the two-dimensional plane projection is located. Use this area as the size of the 2D map. The length C of the rectangular area is the width W of the 2D map, and the width K of the rectangular area is the height H of the 2D map. Step 2.4: Construct an image data with a width of W and a height of H, and draw the two-dimensional planar projection coordinate data obtained in Step 2.2 into the image data; Step 2.5: Determine the RGB values ​​of the 3D point cloud data, and based on the projection relationship determined in Step 2.1, assign the color information of the 3D point cloud to the image data drawn in Step 2.4 to complete the generation of the final 2D map.

4. The method for path planning in an unknown area as described in claim 1, characterized in that: The specific implementation method of step 3 is as follows; Step 3.1: Process the 2D map using an edge detection algorithm to obtain objects with boundaries, such as buildings and walls, in the 2D map, and define them as obstacles; Step 3.2: Based on the obstacles identified in Step 3.1, the 2D map is processed using a refinement method. The resulting image is the skeleton map corresponding to the 2D map. Step 3.3: Use Euclidean distance to calculate the distance between each node in the backbone graph generated in Step 3.

2. If the distance between nodes is less than D1, then mark both nodes simultaneously. Step 3.4: Link each node separately, and use the point-to-line distance formula to calculate the distance between other nodes and the link. If the distance between other nodes and the link is less than D2, then mark the node. Step 3.5: Based on the links constructed in Step 3.4, calculate the distance between each link. If the distance between the links is less than D3, mark the corresponding nodes of the two links. Step 3.6: Process the nodes marked in steps 3.3-3.5 in sequence. Draw a circle with the node as the center and M as the radius. If other nodes fall into the circle, calculate the coordinates of all nodes in the circle on average, keep the calculated nodes, and delete the other nodes. If there are no other nodes, continue to process the other nodes. Step 3.7: After processing, a walkable road route map, i.e. a road distribution map, can be obtained for the area. The connecting lines of each node are the walkable roads in the 2D map, and each node is the intersection or bend between roads.

5. A path planning system for unknown areas, characterized in that, Includes the following modules: The data acquisition module is used to collect 3D point cloud data of the area using sensors; The data conversion module is used to convert 3D point cloud data to 2D maps. The key point extraction module is used for the automatic generation of 2D map road maps and the automatic extraction of key points on the roads. Key points are located at road intersections or road turning points. The path planning module is used to randomly select two points A and B in a 2D map and complete the shortest path planning from point A to point B in the 2D map. The specific implementation method of the path planning module is as follows; Step 4.1: Number each intersection or bend in the road distribution map in Step 3.7; Step 4.2: Use Euclidean distance to calculate the distance between point A and each intersection or bend on the road map, select the intersection or bend closest to point A, and name it point C; Step 4.3: Use Euclidean distance to calculate the distance between point B and each intersection or bend on the road map, select the intersection or bend closest to point B, and name it point D; Step 4.4: Use the depth-first search algorithm to generate all possible trajectories from point C to point D in the road distribution map; Step 4.5: Add point A before all the trajectories generated in step 4.4, that is, connect A and C, and add point B at the end, that is, connect B and D, to complete the generation of all possible motion trajectories from point A to point B, that is, to complete the planning of the entire path from A to C and from B to D. Step 4.6: Use Euclidean distance to calculate the geometric distance between each intersection or turn of the trajectory, excluding the starting point, and the previous intersection or turn. Step 4.7: Use summation to calculate the total length of the trajectory geometry; Step 4.8: Use steps 4.6-4.7 to process all possible motion trajectories and obtain the lengths d1...di...dn of each trajectory; Step 4.9: Using the total length of the trajectory set as the criterion, obtain the geometric coefficient Kg of the nearest motion trajectory, as shown in the following formula: Where di is the total geometric length of the i-th trajectory, and min(d1...di...dn) is the trajectory with the smallest geometric distance among all trajectories; Step 4.10: Count the number of nodes S for each trajectory and use it as the criterion to obtain the nearest trajectory convenience coefficient Kc, as shown in the following formula: Where si is the number of nodes in the i-th trajectory, and min(s1...si...sn) is the minimum number of nodes among all trajectories; Step 4.11: Based on the motion trajectory geometric coefficient Kg and the motion trajectory convenience coefficient Kc obtained in Step 4.9, the nearest motion trajectory coefficient K for each trajectory is obtained, as shown in the following formula: Where Kg is the geometric coefficient of the closest motion of the trajectory, and Kc is the convenience coefficient of the closest motion of the trajectory. The closer K is to 1, the more likely the trajectory is the closest motion trajectory. Step 4.12: Select the trajectory whose motion trajectory coefficient K is closest to 1, that is, the shortest motion route from point A to point B.

6. The unknown area path planning system as described in claim 5, characterized in that: The data acquisition module uses cameras and radar to collect 3D point cloud data of the area.

7. The unknown area path planning system as described in claim 5, characterized in that: The specific implementation method of the data conversion module is as follows; Step 2.1, define a 3x3 orthogonal projection matrix P, as follows: Step 2.2: Based on the projection matrix P constructed in Step 2.1, iterate through and process the coordinates of each point in the 3D point cloud data. Transform each three-dimensional coordinate D into a projected coordinate on a two-dimensional plane. As shown in the following formula: in, The x and y coordinates of the two-dimensional plane; Step 2.3: Summarize all the two-dimensional plane projection coordinates u and v obtained in Step 2.

2. Select the leftmost, rightmost, topmost, and bottommost coordinate values ​​of the projection coordinates. Extend each of the four coordinate values ​​outward by a length n and draw parallel lines to determine the rectangular area where the two-dimensional plane projection is located. Use this area as the size of the 2D map. The length C of the rectangular area is the width W of the 2D map, and the width K of the rectangular area is the height H of the 2D map. Step 2.4: Construct an image data with a width of W and a height of H, and draw the two-dimensional planar projection coordinate data obtained in Step 2.2 into the image data; Step 2.5: Determine the RGB values ​​of the 3D point cloud data, and based on the projection relationship determined in Step 2.1, assign the color information of the 3D point cloud to the image data drawn in Step 2.4 to complete the generation of the final 2D map.

8. The unknown area path planning system as described in claim 6, characterized in that: The key point extraction module is implemented as follows; Step 3.1: Process the 2D map using an edge detection algorithm to obtain objects with boundaries, such as buildings and walls, in the 2D map, and define them as obstacles; Step 3.2: Based on the obstacles identified in Step 3.1, the 2D map is processed using a refinement method. The resulting image is the skeleton map corresponding to the 2D map. Step 3.3: Use Euclidean distance to calculate the distance between each node in the backbone graph generated in Step 3.

2. If the distance between nodes is less than D1, then mark both nodes simultaneously. Step 3.4: Link each node separately, and use the point-to-line distance formula to calculate the distance between other nodes and the link. If the distance between other nodes and the link is less than D2, then mark the node. Step 3.5: Based on the links constructed in Step 3.4, calculate the distance between each link. If the distance between the links is less than D3, mark the corresponding nodes of the two links. Step 3.6: Process the nodes marked in steps 3.3-3.5 in sequence. Draw a circle with the node as the center and M as the radius. If other nodes fall into the circle, calculate the coordinates of all nodes in the circle on average, keep the calculated nodes, and delete the other nodes. If there are no other nodes, continue to process the other nodes. Step 3.7: After processing, a walkable road route map, i.e. a road distribution map, can be obtained for the area. The connecting lines of each node are the walkable roads in the 2D map, and each node is the intersection or bend between roads.

Images