Unmanned vehicle path planning method based on real-time sampling

Abstract

The invention discloses an unmanned vehicle path planning method based on real-time sampling, and relates to the field of intelligent systems and automatic control. The method comprises the following steps: dividing a regional map into a plurality of concentric sampling regions by taking an unmanned vehicle starting point and an unmanned vehicle target point as symmetry axes, and performing partition search in the plurality of concentric sampling regions through a dynamic density adjustment algorithm to obtain an initial global path; performing path trimming on the initial global path, and performing path smoothing processing on the initial global path after path trimming to obtain a global path; in the improved DWA algorithm, path selection is carried out based on the global path, the weight of the trajectory evaluation function, the unmanned vehicle initial state and unmanned vehicle model parameters, and a lowest-cost path is obtained; the weight of a trajectory evaluation function in the improved DWA algorithm is dynamically generated by adopting a fuzzy controller; and determining the path with the lowest cost as the path of the unmanned vehicle. The method can improve the reliability of unmanned vehicle path planning.

Description

TECHNICAL FIELD

[0001] The present application relates to the field of intelligent systems and automation control, in particular to a path planning method for unmanned vehicle based on real-time sampling. BACKGROUND

[0002] Unmanned vehicles have become a research hotspot in many fields such as autonomous detection, medical rescue, and logistics transportation. In these applications, unmanned vehicles need to navigate along a global reference trajectory to complete the task. In order to ensure safety, the feasibility and reliability of trajectory planning are crucial. RRT* is widely used in three-dimensional space such as motion control of unmanned vehicles and robotic arms due to its global random sampling characteristics. Unlike Rapidly-exploring Random Tree (RRT), RRT* will reselect and connect the surrounding node with the lowest cost when adding a new node. Therefore, RRT* will reduce the total cost from the starting point to the new node. This mechanism allows RRT* to gradually plan a path close to the optimal one.

[0003] However, RRT* also suffers from expensive search costs due to its global random sampling problem. RRT* lacks distance guidance and kinematic constraints when searching for a path, resulting in poor quality of the generated unmanned vehicle path. SUMMARY

[0004] Therefore, it is necessary to provide a path planning method for unmanned vehicle based on real-time sampling to solve the above technical problems. The method improves the reliability of the path planning of the unmanned vehicle.

[0005] The present application adopts the following technical solutions: The present application provides a path planning method for unmanned vehicle based on real-time sampling, comprising: obtaining a regional map including a starting point and a target point of the unmanned vehicle, initial control parameters of the unmanned vehicle, and model parameters of the unmanned vehicle; the initial control parameters of the unmanned vehicle include linear velocity and angular velocity; the model parameters of the unmanned vehicle include sensor parameters, positioning parameters, and safety parameters; dividing the regional map into a plurality of concentric sampling regions with the starting point of the unmanned vehicle and the target point of the unmanned vehicle as the symmetry axis, and performing zoned search in the plurality of concentric sampling regions through a dynamic density adjustment algorithm to obtain an initial global path; performing path pruning on the initial global path, and performing path smoothing processing on the initial global path after path pruning to obtain a global path; In the improved DWA algorithm, path selection is performed based on the global path, the weight of the trajectory evaluation function, the initial state of the unmanned vehicle, and the model parameters of the unmanned vehicle to obtain a lowest cost path; the weight of the trajectory evaluation function in the improved DWA algorithm is dynamically generated by a fuzzy controller; determining the lowest cost path as the path of the unmanned vehicle.

[0006] Preferably, the regional map is divided into a plurality of concentric sampling regions with the unmanned vehicle starting point and the unmanned vehicle target point as the symmetry axis, and the dynamic density adjustment algorithm is used for partition search in the plurality of concentric sampling regions to obtain an initial global path, specifically including: The regional map is diffused to both sides of the straight line connecting the unmanned vehicle starting point and the unmanned vehicle target point as the symmetry axis, and is divided into a plurality of concentric sampling regions; The path search is performed in the first layer sampling region by the random tree algorithm, when the sampling density of the path search is greater than the sampling density value set in the first layer region, the second layer sampling region is expanded, and the sampling density is dynamically adjusted according to the region level; the first layer sampling region is the two sampling regions closest to the symmetry axis; the higher the region level, the smaller the sampling density; The path search is performed in the second layer sampling region by the random tree, until the search stopping condition is reached; the search stopping condition is that a feasible path is searched or the last layer sampling region is searched completely; The searched feasible path is determined as the initial global path.

[0007] Preferably, the initial global path is path pruned, specifically including: The initial global path is uniformly discretized into a discrete point set; The starting point of the initial global path is selected as the last point of the pruned path; The discrete point set is traversed to judge the visibility between the discrete point and the last point of the pruned path; If the judgment result is invisible, the obstacle voxel center shielding the current discrete point is obtained; The position of the obstacle voxel center is adjusted along the direction perpendicular to the line connecting the discrete point and the last point of the pruned path, and the coplanar direction formed by the line and the obstacle voxel center, to obtain an adjusted point; The adjusted point is added to the point set corresponding to the pruned path, and the last point of the pruned path is updated to the adjusted point; After traversing the discrete point set, the initial global path after path pruning is generated.

[0008] Preferably, the initial global path after path pruning is path smoothed to obtain a global path, specifically including: The initial global path after path pruning is discretized into a plurality of nodes, and every three adjacent nodes form a group; For any group, the three adjacent nodes are taken as the control of the quadratic B-spline curve to construct a quadratic B-spline curve, and the constructed quadratic B-spline curve is taken as a smoothed path; The smoothed paths generated by all groups are connected as a global path.

[0009] Preferably, the method further comprises: if the constructed quadratic B-spline curve collides with the obstacle, changing the curvature of the quadratic B-spline curve by moving the intermediate control points so that the quadratic B-spline curve with the changed curvature bypasses the obstacle.

[0010] Preferably, the path selection is based on the global path, the weight of the trajectory evaluation function, the initial state of the unmanned vehicle and the model parameters of the unmanned vehicle to obtain the lowest cost path, specifically comprising: obtaining a node set corresponding to the global path and a path node set to be expanded; For each node in the node set corresponding to the global path, detecting whether there is a moving obstacle between the node and a candidate node in the path node set to be expanded; If there is a moving obstacle, initializing the improved DWA algorithm and adjusting the initial control parameters of the unmanned vehicle in real time based on the weight of the trajectory evaluation function dynamically generated by the fuzzy controller; According to the adjusted initial control parameters of the unmanned vehicle and the model parameters of the unmanned vehicle, simulating trajectories are predicted, and the cost of the simulated trajectories is calculated by the trajectory evaluation function; When it is detected that the distance between the simulated trajectory and the obstacle is less than a preset distance, it is detected whether there is a collision risk, and if there is a collision risk, a temporary obstacle avoidance path from the current position of the unmanned vehicle to the target point of the unmanned vehicle is generated; The cost of the temporary obstacle avoidance path is calculated, the cost of the temporary obstacle avoidance path is compared with the cost of the simulated trajectory, and the path with smaller cost is selected as the updated trajectory; Each node in the node set corresponding to the global path is traversed to generate the lowest cost path.

[0011] Preferably, the trajectory evaluation function is: ; Wherein, is the total score of all scoring items, represents the score of the angular deviation of the unmanned vehicle attitude at the end of the simulated trajectory and the heading of the target point, represents the distance score between the end of the simulated trajectory and the nearest obstacle, represents the score of the corresponding linear velocity of the simulated trajectory, is the proportion of the yaw angle sub-function, is the proportion of the distance sub-function, is the proportion of the velocity sub-function, is the proportion of the total score, v represents the linear velocity of the unmanned vehicle, represents the angular velocity of the unmanned vehicle.

[0012] Preferably, the weight of the trajectory evaluation function dynamically generated based on the fuzzy controller comprises: The distance between the unmanned vehicle and the target point of the unmanned vehicle and the distance between the unmanned vehicle and the nearest obstacle of the unmanned vehicle are taken as inputs of the fuzzy controller; The inputs of the fuzzy controller are processed through fuzzy rules to obtain weights of the trajectory evaluation function.

[0013] Preferably, the fuzzy rules comprise: when the distance between the unmanned vehicle and the target point of the unmanned vehicle and the distance between the unmanned vehicle and the nearest obstacle are both in a first distance range, using a second , a second and a first ; when the distance between the unmanned vehicle and the target point of the unmanned vehicle is in the first distance and the distance between the unmanned vehicle and the nearest obstacle is in a second distance, using a third , a first and a second ; when the distance between the unmanned vehicle and the target point of the unmanned vehicle is in the second distance but the distance between the unmanned vehicle and the nearest obstacle is in the first distance, using a first , a third and a second ; when the distance between the unmanned vehicle and the target point of the unmanned vehicle is in the first distance and the distance between the unmanned vehicle and the nearest obstacle is in the second distance, using a third and a third and a second .

[0014] Preferably, the method further comprises: In the improved DWA algorithm, the nodes in the global path are taken as local target points of the dynamic window algorithm; In the process of tracking the path, the angle between the line connecting the current position of the unmanned vehicle and the current local target point and the global path is calculated in real time; After completing dynamic obstacle avoidance, if the angle is greater than or equal to a threshold angle, the current local target point is removed, and whether the local target points meet the navigation condition is sequentially judged until a local target point meeting the navigation condition is selected as a new guide point.

[0015] The application provides an unmanned vehicle path planning device based on real-time sampling, comprising: An acquisition module is configured to acquire a regional map including a starting point and a target point of an unmanned vehicle, initial control parameters of the unmanned vehicle and model parameters of the unmanned vehicle; the initial control parameters of the unmanned vehicle include linear velocity and angular velocity; the model parameters of the unmanned vehicle include sensor parameters, positioning parameters and safety parameters; A search module is configured to divide the regional map into a plurality of concentric sampling regions with the starting point of the unmanned vehicle and the target point of the unmanned vehicle as the symmetry axes, and perform zoned search in the plurality of concentric sampling regions through a dynamic density adjustment algorithm to obtain an initial global path. The pruning and smoothing module is configured to perform path pruning on the initial global path, and perform path smoothing on the initial global path after path pruning, to obtain a global path. The selection module is configured to select a path based on the global path, the weight of the trajectory evaluation function, the initial state of the unmanned vehicle and the model parameters of the unmanned vehicle in the improved DWA algorithm, to obtain a lowest-cost path. The determination module is configured to determine the lowest-cost path as the path of the unmanned vehicle.

[0016] The application provides a computer readable storage medium, which stores a computer program, and the computer program is executed by a processor to implement the unmanned vehicle path planning method based on real-time sampling.

[0017] The application provides a computer device, which comprises a memory, a processor and a computer program stored in the memory and executable on the processor, and the processor implements the unmanned vehicle path planning method based on real-time sampling when executing the program.

[0018] The above at least one technical solution adopted by the application can achieve the following beneficial effects: The method comprises the following steps: dividing a regional map into a plurality of concentric sampling regions with the starting point of the unmanned vehicle and the target point of the unmanned vehicle as the symmetry axis, and performing partition search in the plurality of concentric sampling regions through a dynamic density adjustment algorithm to obtain an initial global path; the sampling density is dynamically adjusted to improve the resolution in the obstacle dense area, so as to avoid missing detection; the initial global path is pruned, and the initial global path after path pruning is smoothed to obtain a global path; the weight of the trajectory evaluation function in the improved DWA algorithm is dynamically generated by a fuzzy controller, the fuzzy controller dynamically adjusts the weight of the trajectory evaluation function according to the real-time environment, improves the adaptability of the algorithm, and further improves the reliability of path planning; and the lowest-cost path is determined as the path of the unmanned vehicle. The method improves the reliability of the path planning of the unmanned vehicle. BRIEF DESCRIPTION OF DRAWINGS

[0019] The accompanying drawings, which are included to provide a further understanding of the application and constitute a part of this application, illustrate certain illustrative embodiments of the application and together with the description serve to explain the application. In the drawings:

[0020] Figure 1 A flowchart of the unmanned vehicle path planning method based on real-time sampling is provided. Figure 2 A schematic diagram of the RRT* iteration process is provided. Figure 3 A schematic diagram of random sampling is provided. Figure 4 A partition sampling schematic provided by the present application; Figure 5 A path pruning schematic provided by the present application; Figure 6 A path smoothing schematic provided by the present application; Figure 7 A dynamics model schematic of an unmanned vehicle provided by the present application; Figure 8 An input membership function schematic provided by the present application; Figure 9 An output membership function schematic provided by the present application; Figure 10 A return path rule schematic provided by the present application; Figure 11 A real-time path planning method RSP-UV framework schematic provided by the present application; Figure 12 RRT* and improved RRT* experimental result schematics provided by the present application; Figure 13 DWA and improved DWA experimental result schematics provided by the present application; Figure 14 A path comparison result schematic in a static environment provided by the present application; Figure 15 A path comparison result schematic in a dynamic environment provided by the present application; Figure 16 An RSP-UV dynamic obstacle avoidance process schematic provided by the present application; Figure 17 A literature

[46] method dynamic obstacle avoidance process schematic provided by the present application; Figure 18 An ORRT*-DWA dynamic obstacle avoidance process schematic provided by the present application; Figure 19 An MTAR dynamic obstacle avoidance process schematic provided by the present application; Figure 20 A hardware platform schematic provided by the present application; Figure 21 A real experiment environment schematic provided by the present application; Figure 22 A comparative experiment schematic in a corridor environment provided by the present application; Figure 23 A comparative experiment schematic in a basement environment provided by the present application; Figure 24 A comparative experiment schematic in an unstructured urban road environment provided by the present application; Figure 25 A comprehensive comparison result schematic diagram provided by the present application; Figure 26 An unmanned vehicle path planning device based on real-time sampling provided by the present application; Figure 27 A computer device for implementing an unmanned vehicle path planning method based on real-time sampling provided by the present application. DETAILED DESCRIPTION

[0021] To make the objectives, technical solutions and advantages of the present application clearer, the technical solutions of the present application will be described below in conjunction with specific embodiments of the present application and corresponding drawings. Obviously, the described embodiments are only some of the embodiments of the present application, but not all the embodiments. Based on the embodiments in the present application, all other embodiments obtained by those of ordinary skill in the art without creative work fall within the scope of protection of the present application.

[0022] Devices such as desktop computers, servers and notebook computers that can execute the solutions of the present application. For the convenience of description, only servers are taken as the execution subjects for description below.

[0023] Path planning based on space sampling can be divided into two categories: state space sampling and motion space sampling, according to different sampling spaces. Algorithms based on state space sampling can quickly generate paths in large and high-latitude spaces, such as RRT and Probabilistic Roadmap Method (PRM). Algorithms based on state space sampling have probabilistic completeness, and the main steps include random sampling, metric connectivity, collision detection and path query. Algorithms based on motion space sampling are equidistantly sampled in the velocity space. Algorithms based on motion space sampling select the best control instruction to drive the robot motion through an evaluation function. Main algorithms include Curvature Velocity Method (CVM) and Dynamic Window Approach (DWA).

[0024] The first type of algorithm is the Probabilistic Roadmap (PRM). The core idea of this algorithm is to build a connectivity graph. Find the best path. First, build a connectivity graph. Randomly select samples in the configuration space (C-space) and check whether they are in free space (white area). Use a local planner to try to connect these samples to each other. Only keep the connections that do not intersect with obstacles, and obtain the connectivity graph by combining the start and goal points. Then, find the best path. Use a graph search algorithm to search for a collision-free path based on the above connectivity graph. Researchers have proposed traditional PRM, which builds a path network graph based on the environment model and queries it to generate the best path. Therefore, the more the number of samples, the better the calculation result. Researchers have further extended this by reducing the number of calls to the local planner to improve planning efficiency. Researchers have proposed SPARS2, which builds a dual graph of sparse and dense to further improve the convergence speed of the algorithm.

[0025] The second type of algorithm is RRT. The core idea of RRT is similar to PRM. RRT also randomly samples in the search space and starts to expand from the start point or the goal point as the root node of the tree. It only connects adjacent sampling nodes that meet the constraint conditions until a node in the tree approaches the goal point or the start point. Finally, the shortest path is searched based on the generated tree. Unlike the graph structure of PRM, RRT uses a tree structure to improve query efficiency. The traditional RRT is similar to the traditional A* in many ways, and many researchers have also made similar improvements to the traditional RRT algorithm. From the work of these researchers, we can see the shortcomings of RRT, such as high sampling randomness, winding path, and low robustness. By analyzing the causes of these problems, researchers have proposed appropriate improvements. For example, RRT* finds the relatively optimal path by calculating new nodes. The informed RRT* is designed based on the use of an elliptical range search strategy to reduce the winding path segment. RRTX develops a strategy suitable for dynamic environments to enhance the robustness of the algorithm. Researchers have proposed a two or more tree search method (RRT-connect) to improve search efficiency. Optimal B-RRT* adds a heuristic function on top of RRT-connect to further improve convergence speed. In addition, researchers have applied topological ideas to the expansion tree and theoretically proved the feasibility of this method. The bidirectional expansion tree strategy significantly improves search efficiency. In addition to this method, researchers have developed rules for the way sampling points are generated, which makes sampling points more characteristic. For example, ADD-RRT uses a sphere as a range to limit sampling. In addition, this method establishes a mapping relationship between the sphere radius and the success rate of boundary point expansion to improve the success rate of sampling. OB-RRT is similar to ADD-RRT, which collects data about obstacles to determine the direction of node growth. CL-RRT makes closed-loop predictions on the RRT framework by using physical or logical environments to adjust the bias of the Gaussian sampling cloud. Researchers have studied ways to improve the RRT algorithm for robots in the presence of non-holonomic constraints. For example, Kinodynamic-RRT* considers hard constraints on the robot model, but consumes a lot of computational memory. The improved RRT* overcomes this defect and obtains an asymptotically optimal solution, solving the motion planning problem of robots similar to cars in a 5D state space and aircraft in a 10D state space. In addition, RTR+C*CS provides a complete planning scheme, in which RTR generates a global path, and C*Cs uses straight line segments and circular arc fitting to obtain a local path, but requires more iteration times. For high-dimensional planning problems, RABTT* uses homotopy type to generate an initial path, and uses local optimization techniques to achieve global optimization results.

[0026] The third category is CVM. Its core idea is to generate a predicted trajectory based on the small speed space range of non-homogeneous mobile robots combined with the kinematic model. By performing constraint evaluation analysis, the most suitable trajectory is found. Researchers have proposed a differential mobile robot speed space sampling method based on bounded speed space, i.e., the motion trajectory of the robot is a circular arc or a straight line. For example, CVM samples the medium distance in the speed space and selects the lowest overall cost speed to control the robot. LCM combines CVM with the direction space method for corridor scenarios to make the robot avoid obstacles while moving along the corridor. DCVM combines a probabilistic 3D occupancy-velocity grid map with dynamic obstacle avoidance and has been verified in simulation and real environment.

[0027] The fourth category is DWA. DWA is very similar to CVM, both of which are based on speed space sampling, but differ in evaluation. Researchers have proposed an improved algorithm General DWA, which extends DWA to omnidirectional movement and incorporates navigation functions. TDW considers dynamic obstacles and uses FD* combined with DWA to improve the predictability of robot motion. DW4DO uses a double dynamic window and has no dynamic constraints to limit the size of the dynamic window, thus expanding the detection range of DWA.

[0028] Hybrid path planning methods, global path planning is usually combined with local path planning. Global path planning is to plan an optimal and safe path under the condition of known environmental information. Common algorithms include RRT algorithm, Dijkstra algorithm and A* algorithm. Compared with Dijkstra algorithm and A* algorithm, RRT algorithm has more advantages in planning efficiency.

[0029] Local path planning refers to planning a local path for an autonomous vehicle to dynamically avoid obstacles based on the detected local environmental information (when the environmental knowledge is not completely clear) during the driving process of the autonomous vehicle. Commonly used local algorithms include artificial potential field method, time-varying elastic band method, deep reinforcement learning and dynamic window method.

[0030] The artificial potential field method is simple to calculate, but it is prone to local optimal solution and cannot reach the target point in complex environments. The time elasticity method is complex to calculate, sensitive to parameter changes in complex dynamic environments, and unstable in robot trajectory control. Deep reinforcement learning has attracted widespread attention due to its good adaptability to complex environments. Researchers have proposed a deep Q network based on experience classification and used a state space with generalization ability to improve the learning efficiency of unmanned vehicles. Researchers have improved the exploration factor and set a dynamic reward function to speed up the convergence of the method. Through simulation, they have confirmed that the planning efficiency and path quality have improved. Although there have been many improvements in deep reinforcement learning, the long training time, high computational cost, and large amount of data required limit the practical application of the method. In contrast, DWA has a wider applicability in local obstacle avoidance. It takes into account the kinematic constraints of the robot and does not require a large amount of pre-training data.

[0031] Researchers have dynamically adjusted the DWA parameters based on reinforcement learning to improve the adaptive ability of the algorithm and the success rate of path planning. Researchers have designed adaptive rules to flexibly adjust the corresponding weight values in DWA to enable the robot to safely and efficiently traverse dense obstacles. Currently, a single improved RRT can indeed find the optimal path in a known environment. However, it cannot avoid dynamic obstacles. Although simply improving DWA may be effective in avoiding dynamic obstacles, it is difficult to obtain a globally optimal path. Therefore, it is particularly important to improve both algorithms and achieve fusion. Researchers have proposed a fusion method that combines RRT* and DWA. In their method, RRT* generates an initial path. This initial path serves as a reference path to guide DWA, and a bias function is introduced to achieve local obstacle avoidance. The final optimal path is obtained. However, this method has poor robustness. Researchers alternate between using RRT* and DWA in the sampling space. This improves the robustness of the algorithm to some extent. However, the path formed by this method is tortuous, resulting in reduced safety.

[0032] Researchers have improved the fusion method of RRT algorithm and DWA algorithm, which reduces the time required for the original RRT algorithm to search for the target point and shortens the distance moved by the DWA algorithm to find the target point. However, the improved algorithm does not solve the problem that the path found by RRT is not optimal. Researchers have proposed a hybrid algorithm that combines optimized RRT* and DWA (ORRT*-DWA) to solve the path planning problem faced by quadruped robots in urban environments. Researchers have proposed a novel motion trajectory analysis and regulation navigation framework (MTAR). This framework effectively alleviates the path planning failure problem caused by dynamic environmental changes.

[0033] In the application, the RSP-UV algorithm is proposed, the RSP-UV algorithm adopts the improved RRT* algorithm to obtain an initial reference path. Important key nodes are selected as local path reference points to guide the improved DWA algorithm to perform global path tracking. Finally, the fusion algorithm is improved, and finally a smooth path meeting the kinematic constraints of the robot and capable of realizing local obstacle avoidance is obtained.

[0034] The technical solutions provided by the embodiments of the application are described in detail below with reference to the drawings.

[0035] Figure 1 A flowchart of a path planning method for an unmanned vehicle based on real-time sampling is provided in the application, and the method specifically includes the following steps: S101: Obtain a regional map including a starting point and a target point of the unmanned vehicle, initial control parameters of the unmanned vehicle, and model parameters of the unmanned vehicle. The initial control parameters of the unmanned vehicle include linear velocity and angular velocity. The model parameters of the unmanned vehicle include sensor parameters, positioning parameters, and safety parameters.

[0036] S102: Divide the regional map into a plurality of concentric sampling regions with the starting point of the unmanned vehicle and the target point of the unmanned vehicle as the symmetry axes, and perform zoned search in the plurality of concentric sampling regions through a dynamic density adjustment algorithm to obtain an initial global path.

[0037] In an exemplary embodiment, the regional map is divided into a plurality of concentric sampling regions with the starting point of the unmanned vehicle and the target point of the unmanned vehicle as the symmetry axes, and the initial global path is obtained through zoned search in the plurality of concentric sampling regions through a dynamic density adjustment algorithm, specifically including: diffusing the regional map to both sides with a straight line connecting the starting point of the unmanned vehicle and the target point of the unmanned vehicle as the symmetry axis to divide the regional map into a plurality of concentric sampling regions; performing path search in the first layer sampling region through a random tree algorithm, when the sampling density of the path search is greater than the sampling density value set in the first layer region, expanding to the second layer sampling region, and dynamically adjusting the sampling density according to the region level; the first layer sampling region is the two sampling regions closest to the symmetry axis; the higher the region level, the smaller the sampling density; performing path search in the second layer sampling region through a random tree until a search stop condition is reached; the search stop condition is that a feasible path is searched or the last layer sampling region is searched; and determining the searched feasible path as the initial global path.

[0038] In particular, the RRT* algorithm introduces the concept of path cost into the standard RRT algorithm. The quality of the path is improved by minimizing the path cost. The algorithm extends the search tree in a random manner, randomly selecting a node at each iteration and attempting to connect it to the nearest node. If the new node can reduce the total cost of reaching the goal by connecting to other nodes, it optimizes its position in the tree. The RRT* algorithm gradually converges to the global optimal solution by repeatedly repeating this process. A complete random tree is formed when a new node reaches a certain distance from the goal. The core advantage of this algorithm is that it does not require gradient information and can handle complex non-convex problems, making it suitable for autonomous vehicles.

[0039] At each iteration, the RRT* algorithm is as shown in Figure 2 Step one, random sampling. A new point is randomly selected in the search space as a candidate direction for extending the tree. An existing node in the tree is found such that its distance to the newly generated point is minimized.

[0040] Step two, node expansion. Connect the new sampling point and the nearest point . Given a radius centered at , find a set of potential parent nodes already existing in the tree.

[0041] Step three, cost calculation. Starting from the first potential parent node, connect the potential parent node and the sampling point. Without collision detection, calculate the cost from the starting point to the sampling point.

[0042] Step four, parent node selection. Compare the cost of the new path with the cost of the original path . If the cost of the new path is smaller, perform collision detection. If the cost of the new path is higher, replace it with the next potential parent node. If the collision detection fails, the potential parent node is not used as the new parent node. Then continue to judge the next potential parent node, i.e. compare the cost of the new path with the cost of the original path . If the collision detection is passed, delete the previous edge in the tree. Add a new edge in the tree and change the potential parent node to the new parent node.

[0043] Step five, update the tree structure. Traverse all possible parent nodes to obtain the updated tree structure.

[0044] While RRT* can solve path optimization problems, it performs global sampling on the map, and the sampling points are too random, such as... Figure 3 As shown, the diagonal of the simulated map is used as the starting and ending points. Figure 3 Figure (a) shows that RRT* initially spreads these sampling points globally (blue dots). Eventually, it will cover the entire map, as shown in Figure (a). Figure 2 As shown in Figure (b), the final path (red line) is derived. Therefore, the RRT* method is time-consuming and costly. This inevitably leads to inefficiency and impacts the real-time performance of path planning.

[0045] To improve search efficiency, the map was divided into several regions for sampling. (The starting point is then used for connection.) and target point The line serves as the axis of symmetry for diffusion sampling on both sides. The environment map is divided into multiple sampling regions, such as... Figure 4 As shown. The partitioning expands outwards with a distance d. Within the central regions A1 and A2, if a feasible path exists, its path cost is relatively low compared to feasible paths in other regions. Therefore, the central regions A1 and A2 are searched first as the initial sampling regions. The region sampling density is then set. If the random tree within the sampling density of the constrained region still fails to find a feasible path, the sampling region is expanded to the outer regions A3 and A4. The random tree continues to grow in the outer sampling region, also through sampling density... The constraint is used as a stopping condition for the search. If a feasible path is found, the algorithm terminates; otherwise, it continues until the sampling area has expanded to the entire map. Note that... Figure 4 The blank areas in the search are the last areas to be explored. Partition search avoids over-searching of invalid areas (such as A5 and A6) at the beginning of the search, which effectively improves search efficiency and ensures the probabilistic integrity of the algorithm.

[0046] As can be seen from the regional sampling rules, the size of the sampling region determines the number of sampling points. The sampling density of each region satisfies formula (1):

[0047] (1) in, It is the set sampling density value. It is the first i Sampling density of each sampling area It is the first i The number of sampling points in each sampling region It is the area of ​​the corresponding sampling region. n This represents the number of sampling regions.

[0048] The final area sampling rule is shown in formula (2): (2) The partition sampling pseudo code is shown in Algorithm 1, which selects the sampling region according to the sampling rule shown in formula (2) First, search the first set of sampling regions, is the total sampling density of the sampling region , which is calculated by formula (1). If the sampling density exceeds the set sampling density value (line 1, line 2), search the next set of regions (line 3). This loop continues until the search stops on the path or the last region is sampled (line 5, line 6). Finally, the original path set (line 7) is obtained.

[0049] Algorithm 1 S103: Path pruning is performed on the initial global path, and path smoothing is performed on the initial global path after path pruning to obtain a global path.

[0050] In an exemplary embodiment, the initial global path is path pruned, specifically including: uniformly discretizing the initial global path into a discrete point set; selecting a starting point of the initial global path as a last point of a pruned path; traversing the discrete point set to judge the visibility between the discrete point and the last point of the pruned path; if the judgment result is invisible, an obstacle voxel center that currently occludes the discrete point is obtained; the obstacle voxel center is positionally adjusted in a direction perpendicular to a line segment between the discrete point and the last point of the pruned path, and coplanar with the line segment and the obstacle voxel center to obtain an adjusted point; the adjusted point is added to a point set corresponding to the pruned path, and the last point of the pruned path is updated to the adjusted point; the discrete point set is traversed to generate the initial global path after path pruning.

[0051] Specifically, when the sampling density is limited, it cannot be guaranteed that the final result obtained is the optimal path. Here, the present application needs to prune the path to obtain a potential optimal path, and the path obtained from exploration is uniformly discretized into a set of discrete points. In each iteration, if the discrete point is invisible to the last point of the pruned path, the voxel center of the occluding object is extracted and pushed away from the obstacle. The pushing direction is perpendicular to the line segment between the discrete point and the last point of the pruned path in this iteration, and coplanar with the Euclidean signed distance field gradient between the line segment and the voxel center. The point is added to the pruned point set, so that it becomes the last point of the new pruned path. This process will continue until the last point. As Figure 5As shown, the green line is the path obtained from the sampling. At the beginning, the red start point is used as the last point of the path obtained after pruning, and discrete points are taken along the green path. When the orange discrete point is reached, since its connecting line with the start point intersects with the obstacle, it is known that the discrete point is not visible to the start point (the last point of the pruned path). At this time, the voxel center of the occluder needs to be moved in a direction away from the obstacle to obtain the blue point. The connection between the start point and the blue point obtains the pruned path. In the next cycle, Figure 5 the blue point in the (a) figure is used as the last point of the pruned path to make the above judgment. By analogy, until the discrete point is the pink end point, the cycle ends and the pruned path (blue path) as shown in the (b) figure of Figure 5 is obtained.

[0052] Algorithm 2 is a shortened path found by dividing the sampling path . The present application regards the sampling points on as a set of key points (line 1). In each iteration, if the point in the key point is not visible to the last point , the voxel center of the occluder is taken (line 5). This point is pushed away from the direction perpendicular to to obtain the point (line 6). is the line connecting the end point of the shortened path and the current search key point. is added to the shortened path (line 7). This iteration process continues until the last destination.

[0053] Algorithm 2 In an exemplary embodiment, the initial global path after path pruning is subjected to path smoothing to obtain a global path, specifically comprising: discretizing the initial global path after path pruning into a plurality of nodes, every three adjacent nodes being a group; for any group, taking the three adjacent nodes as the control of a quadratic B-spline curve to construct a quadratic B-spline curve, and taking the constructed quadratic B-spline curve as a smooth path; and connecting the smooth paths generated by all groups to obtain a global path.

[0054] In an exemplary embodiment, the method further comprises: if the constructed quadratic B-spline curve collides with the obstacle, changing the curvature of the quadratic B-spline curve by moving the middle control point to make the quadratic B-spline curve with changed curvature bypass the obstacle.

[0055] In particular, the global path of the improved RRT* planning is smoothed by a quadratic B-spline curve, which improves the smoothness of the path and ensures the feasibility of the path. In this way, not only the smoothness of the path is improved, but also the feasibility of the path is ensured. The B-spline curve is a curve that locally controls the shape by control points. It can be regarded as a spliced curve of several Bezier curves. It inherits some good properties of Bezier curves, while having more advantages. First, the local control property, the value of B-spline can be completed locally, so the control points can be effectively expanded without re-evaluating the entire trajectory; second, it has the convex hull property. It rewrites the dynamic feasibility constraint into a linear form, avoiding the computational overhead related to the feasibility check; finally, the control cost can be evaluated in a closed form without solving the two-point boundary value problem which is computationally expensive. In the present invention, the adjacent path nodes are used as the basis function control points to construct the quadratic B-spline curve (parabola), instead of the original path, as shown in Figure 6 The quadratic B-spline curve equation is shown in equation (3):

[0056] (3) where, is the control point, is the basis function of the quadratic B-spline.

[0057] The basis function is shown in equation (4): (4) where, t is the curve parameter, i.e. the parameter sliding along the curve t ∈[0,1], is the quadratic B-spline basis function of the 0th point, is the quadratic B-spline basis function of the 1st point, is the quadratic B-spline basis function of the 2nd point.

[0058] Substituting equation (4) into equation (3), the matrix expression of the quadratic B-spline curve is shown in equation (5): (5) The definition of the end point position of the quadratic B-spline curve is shown in equation (6): (6) In Figure 6 , the black broken line segment is the initial path. Take , , A quadratic B-spline curve is constructed as control points instead of the initial path to obtain a smooth path. If the smoothed path collides with obstacles, as shown in the red curve, its curvature needs to be changed appropriately. In this case, the curvature of the curve can be changed by moving the control points. The blue curve connects the inflection point and the control point and is extended. A new control point is selected on the extended line and the B-spline curve is reconstructed with , , as control points (blue curve).

[0059] S104: In the improved DWA algorithm, the path is selected based on the global path, the weight of the trajectory evaluation function, the initial state of the unmanned vehicle, and the model parameters of the unmanned vehicle to obtain the lowest cost path; the weight of the trajectory evaluation function in the improved DWA algorithm is dynamically generated by a fuzzy controller.

[0060] The principle of the DWA algorithm is summarized as follows. First, the velocity is sampled in the velocity space, and the predicted trajectory profile of the unmanned vehicle (dynamic window) is calculated. Second, the optimal trajectory is selected according to the evaluation function to update the state and position of the unmanned vehicle.

[0061] Figure 7 The nonholonomic motion model of the unmanned vehicle is shown, and DWA is a commonly used local path planning algorithm based on velocity sampling. The linear velocity and angular velocity of the unmanned vehicle at time t are sampled, and the possible motion trajectories of the unmanned vehicle in several sampling periods are predicted based on the position of the unmanned vehicle at the current time. The motion trajectories are evaluated according to a set of evaluation functions, and the optimal trajectory is selected as the motion trajectory of the unmanned vehicle in this time period. The unmanned vehicle is driven to move at the corresponding speed.

[0062] Since the sampling period is very small, it can be approximately considered that the unmanned vehicle is in a uniform straight line motion state in this time period, so the kinematic model of the unmanned vehicle is shown in equation (7): (7) where , and are the coordinates of the coordinate axis and the x coordinate axis and the heading angle of the unmanned vehicle at time y , , and are the coordinates of the coordinate axis and the x coordinate axis and the heading angle of the robot at time y , and linear and angular velocities of the car at time is the sampling period.

[0063] Since there are infinite sets of velocity combinations , the velocity is constrained by a feasible range based on the car’s own characteristics and surrounding environment information. The velocity sampling space of the DWA algorithm consists of three constraints. The first is the maximum and minimum velocity constraints of the car (motion constraints). The second is the maximum acceleration and deceleration limits (motor torque constraints). The third is the velocity range constraints for safety reasons (safety braking constraints). The three constraints are included in equation (8), which is shown as follows:

[0064] (8) wherein , and represent the motion constraints, the motor torque constraints, and the safety braking constraints, respectively, v represents the driving velocity of the car, represents the angular velocity of the car, and are the minimum driving velocity and the maximum driving velocity of the car, respectively, and are the minimum angular velocity and the maximum angular velocity of the car, respectively, represents t the driving velocity of the car at time is the sampling time of the simulation, and are the maximum braking velocities of the car, and are the maximum accelerations of the car, is the shortest distance between the car and the obstacle at the current velocity.

[0065] Among the car state parameters obtained from the velocity sampling, there will be multiple sets of parameters that satisfy the constraints. Each set of parameters corresponds to a trajectory curve. The trajectory curves formed by multiple sets of parameters form the dynamic window of the robot within that time period. This is used to predict the position (coordinates) that the robot can reach within that time period. In order to obtain the dynamic window of the next time period, the current trajectory profile is evaluated by an evaluation function, and the optimal velocity obtained is used as the basis for the next stage of prediction. The trajectory evaluation function is shown in equation (9):

[0066] (9) wherein is the total score of all scoring items, ​a score representing the angular deviation of the end of the simulated trajectory from the vehicle attitude and the heading of the target point, a score representing the distance between the end of the simulated trajectory and the nearest obstacle, a score representing the linear velocity of the simulated trajectory, , and are the proportion of the yaw angle, distance and velocity sub-functions, respectively, is the proportion of the total score.

[0067] In order to prevent a certain evaluation index from occupying too high a proportion, the evaluation sub-function needs to be normalized before being added, which is to divide each item by the sum and normalize it to the interval, as shown in equation (10): (10) wherein, n is the number of all current simulated trajectories, i is the current predicted trajectory.

[0068] By calculating the value of the trajectory evaluation function of each predicted trajectory, the trajectory with the highest score is selected as the optimal trajectory.

[0069] In an exemplary embodiment, path selection is performed based on the global path, the weight of the trajectory evaluation function, the initial state of the unmanned vehicle and the model parameters of the unmanned vehicle to obtain a lowest-cost path, specifically including: obtaining a node set corresponding to the global path and a path node set to be expanded; for each node in the node set corresponding to the global path, detecting whether there is a moving obstacle between the node and a candidate node in the path node set to be expanded; if there is a moving obstacle, initializing the improved DWA algorithm and dynamically generating the weight of the trajectory evaluation function based on the fuzzy controller to adjust the initial control parameters of the unmanned vehicle in real time; predicting a simulated trajectory based on the adjusted initial control parameters of the unmanned vehicle and the model parameters of the unmanned vehicle, and calculating the cost of the simulated trajectory through the trajectory evaluation function; detecting whether there is a collision risk when the distance between the simulated trajectory and the obstacle is less than a preset distance, and generating a temporary obstacle avoidance path from the current position of the unmanned vehicle to the target point of the unmanned vehicle if there is a collision risk; calculating the cost of the temporary obstacle avoidance path, comparing the cost of the temporary obstacle avoidance path with the cost of the simulated trajectory, and selecting the path with a smaller cost as an updated trajectory; traversing each node in the node set corresponding to the global path to generate a lowest-cost path.

[0070] In an exemplary embodiment, the weight of the trajectory evaluation function dynamically generated by the fuzzy controller specifically includes: taking the distance between the unmanned vehicle and the target point and the distance between the unmanned vehicle and the nearest obstacle as the input of the fuzzy controller; processing the input of the fuzzy controller through fuzzy rules to obtain the weight of the trajectory evaluation function.

[0071] In an exemplary embodiment, the fuzzy rule comprises: when the distance between the unmanned vehicle and the target point and the distance between the unmanned vehicle and the nearest obstacle are both the first distance, using the second , the first and the first speed; when the distance between the unmanned vehicle and the target point is the first distance and the distance between the unmanned vehicle and the nearest obstacle is the second distance, using the third , the first and the second ; when the distance between the unmanned vehicle and the target point is the second distance but the distance between the unmanned vehicle and the nearest obstacle is the first distance, using the first , the third and the second ; when the distance between the unmanned vehicle and the target point is the first distance and the distance between the unmanned vehicle and the nearest obstacle is the second distance, using the third and the third and the second .

[0072] Specifically, the traditional DWA algorithm evaluation function adopts a fixed weight, and the unmanned vehicle cannot adaptively adjust the parameters for dynamic obstacle avoidance in a complex obstacle environment, resulting in low dynamic obstacle avoidance efficiency of the unmanned vehicle. In order to solve this problem, the present application designs a fuzzy controller. The fuzzy controller improves the adaptability of the unmanned vehicle to the environment while enhancing the dynamic obstacle avoidance efficiency.

[0073] Specifically, the first distance, the second distance, the second , the first , the third , the first , the second , the third , the first , the second value is set according to the specific engineering practice, in the embodiment of the present application, the first distance is short, the second distance is far, the first , the first and the first value is small, the second , the second and the second value is moderate, the third and the third value is large.

[0074] The input of the fuzzy controller is two fuzzy sets G-dist and O-dist . G-dist represent the distance between the unmanned vehicle and the target point. O-dist represent the distance between the unmanned vehicle and the nearest obstacle. G-distdefined as , L , M and B represent small, medium, large, respectively, for the distance between the vehicle and the goal point, with threshold values . When , the fuzzy description is L , when , the fuzzy description is M , when , the fuzzy description is B . The membership function of the fuzzy set G-dist is shown in Fig. (a) of Figure 8 . Similarly, the fuzzy set O-dist of is defined as , with threshold values . The fuzzy description when is L , when is M , when is B , and the membership function of the fuzzy set O-dist is shown in Fig. (b) of Figure 8 .

[0075] The output quantities of the fuzzy controller are the three scaling factors of the DWA evaluation function , and . The fuzzy control process is performed on these parameters so that they can change according to the changes in the environment. The output quantities are defined as follows. , and are defined as , i.e. small, medium, large, respectively, for the weight, with threshold values . , and . The corresponding membership functions are shown in Figure 9 .

[0076] The fuzzy rules are the key component of the fuzzy controller, and the DWA fuzzy controller designed in this invention is based on the following four principles.

[0077] (1) When both the distance of the vehicle from the goal point and the distance to the nearest obstacle are small, a medium , a medium and a slightly small speed should be used. This allows the vehicle to move towards the goal point while avoiding the collision with the obstacle. The vehicle reduces its speed as much as possible to avoid overshooting.

[0078] (2) When the distance between the car and the goal is close, and the distance between the car and the nearest obstacle is far, the car should use large , small and medium . The car needs to prioritize planning a trajectory towards the goal. Since the distance to the obstacle is far, the car considers obstacle avoidance less. At the same time, the car should reduce the speed to avoid missing the goal.

[0079] (3) When the distance between the car and the goal is far, but the distance between the car and the nearest obstacle is close, the car should use small , large and medium . The car prioritizes obstacle avoidance while maintaining a certain speed of progress.

[0080] (4) When the distance between the car and the goal and the distance between the car and the nearest obstacle are both far, the car should use large and large and medium . The car will travel as fast as possible towards the goal while considering obstacle avoidance.

[0081] By the above four principles, the fuzzy rules developed by the present application are shown in Table 1.

[0082] Table 1 The global path obtained by the improved RRT* is input to the improved DWA. The pseudo code of the improved DWA algorithm is shown in Algorithm 3. First, define the set of points in the global path and the set of points to be added to the trajectory (line 1). First, check if there is a moving obstacle between the current nearest node and the new node (line 2). If so, initialize DWA (line 3). Then, it adjusts the control parameters (line 4, 5), such as linear and angular velocities, using fuzzy rules. Next, the algorithm predicts a trajectory based on the current control parameters (line 6) and evaluates the cost of this trajectory using the evaluation function (line 7). The evaluation function is formula (9). If the predicted trajectory collides with the obstacle, set the trajectory cost to infinity, indicating infeasibility (line 8, 9). If it is close, check if a collision occurs (line 10, 11). If there is a collision, create a path from the current initial point to the goal and evaluate the cost of this temporary path (line 12, 13). If the cost of the temporary path is lower, update the best trajectory and cost (line 14-16). Finally, return the best trajectory with the lowest cost at the end of the loop (line 23-25).

[0083] The fusion algorithm takes the key nodes in the global path as the local target points of the DWA algorithm to guide the unmanned vehicle to track the initial path and achieve dynamic obstacle avoidance. However, the backtracking phenomenon caused by the unmanned vehicle obstacle avoidance trajectory passing through the local target point is prone to occur in the path tracking process. This causes the zigzag of the local trajectory, and increases the path and time cost. In order to solve this problem, the present application proposes a return path rule. As shown in Figure 10 shown, in order to avoid the deviation of the global path due to the movement of the obstacle (light blue rectangle) during the movement of the unmanned vehicle to the local target point. The angle between the line connecting the unmanned vehicle position (red dot) and the current local target point (orange dot) and the global path is denoted as. After completing the dynamic obstacle avoidance, if, it is considered that the unmanned vehicle has not passed through the current local target point. At this time, the unmanned vehicle still uses the current local target point to guide the movement of the unmanned vehicle. On the contrary, if, it is considered that the trajectory of the unmanned vehicle has passed through the current local target point. In order to avoid the backtracking phenomenon, the local target point is removed and the next local target point (blue dot) is judged whether it meets the requirements. If it meets the requirements, the next local target point guides the movement of the robot. If it does not meet the requirements, the next local target point (green dot) is removed and the judgment is made. In this way, the winding phenomenon in the dynamic obstacle avoidance process of the robot can be effectively avoided.

[0084] The general framework of the fusion method is shown in Figure 10 As input are the unmanned vehicle model parameters, initial state parameters and map information. The improved RRT* algorithm is used to plan the optimal path from the starting point to the target point on the known map, and the optimal path node set S is output. S is used as the local key point set of the improved DWA. Through the return path rule, it is determined whether the end point of the local predicted trajectory returns to the current local target point, which avoids unnecessary round trips. Finally, the asymptotically optimal trajectory is formed.

[0085] S105: Determine the lowest cost path as the path of the unmanned vehicle.

[0086] In an exemplary embodiment, the present application provides an implementation path planning method RSP-UV framework diagram as shown in Figure 11

[0087] In an exemplary embodiment, the present application is verified by simulation using Matlab. The simulation experiment is based on a 100m x 100m two-dimensional environment map. The black area is the obstacle area, and the white area is the passable area. The maximum number of iterations is set to 10000 times.

[0088] In order to compare the RRT* algorithm and the improved RRT* algorithm proposed by the present application, 80 groups of controlled experiments on path planning are carried out in the simulation environment. As shown in Figure 12 ​As shown, the orange dots from left to right represent the start and end points, respectively. The red line represents the path result obtained by the improved RRT*. The blue line represents the path result obtained by RRT*. The red dots represent the path critical sampling points of the improved RRT*. On the other hand, the blue dots represent the path critical sampling points of RRT*.

[0089] Table 2 shows the simulation data of the RRT* and improved RRT* provided by this invention. As can be seen from Table 2, the average path length of the improved RRT* is 19.02% shorter than that of the original RRT*, and the average path planning time is also 72.23% less. Therefore, the improved RRT* outperforms the original RRT* in global path planning. Furthermore, the number of critical nodes in the path is significantly reduced, which decreases the computational load of subsequent fusion methods.

[0090] Table 2 To compare the DWA algorithm with the proposed improved DWA algorithm, 80 sets of control experiments were conducted on path planning in a simulation environment. Table 3 shows the simulation experiment parameters provided by this invention.

[0091] Table 3 like Figure 13 As shown in Figure (a), the orange dots represent the starting and ending points from left to right. The curves show that initially, as the autonomous vehicle slowly approaches the obstacle, it is still far from the target. The fuzzy controller output prioritizes factors related to the obstacle. The result is a larger... Smaller and moderate This ensures the vehicle approaches the finish line slowly. When the iteration value approaches 355, the autonomous vehicle continues moving towards the target and leaves the obstacle. This process can also be performed in... Figure 13 The final stage of the path in diagram (b) is shown. At this point, the rule result is larger. moderate and smaller This ensures that the unmanned vehicle slowly approaches the finish line.

[0092] Table 4 is the simulation data of DWA and improved DWA. From Table 4, it can be seen that the improved DWA algorithm is superior to the DWA algorithm in terms of planning path length and planning success rate. The success rate of DWA path planning is 90%, while the success rate of the improved DWA algorithm is 95%. In terms of average path length, the improved DWA algorithm reduces by 3.88% compared with DWA. As shown by the data change in the table, the improved DWA algorithm is more reliable and has a shorter planning path length. The reason for the failure of the improved DWA method is that it lacks the guidance of the global path. Therefore, it is easy to fall into the local minimum problem. In order to effectively solve this problem, the global path generated by the improved RRT* method can be used as a guide path.

[0093] Table 4 In order to highlight the results of the research of the present application, the present application compares RSP-UV with similar methods, the latest method ORRT*-DWA and MTAR. Simulation tests are carried out in static and dynamic environments. A series of path planning simulation experiments are carried out in the simulation environment. Different starting points and endpoints are set for each experimental group. The present application simulates in the same map with four methods and sets the same starting point and endpoint to observe the change of the path.

[0094] Figure 14 An example chart showing a test environment and test comparison results is shown. The blue dot is the starting point. The red dot is the target point. The blue line is the path planning output of the similar method. The red line is the path planning result of RSP-UV. The green line is the path planning result of ORRT*-DWA. The orange line is the path planning result of MTAR. From Figure 14 It can be seen that in the static environment, the paths simulated by RSP-UV and the similar method are very similar. However, as shown in the experimental data in Table 5, the average path length of RSP-UV is shorter than that of other comparison methods. In addition, the average running time of RSP-UV is 45.05% shorter than that of the similar method, 11.65% shorter than that of ORRT*-DWA, and 5.18% shorter than that of MTAR. Obviously, RSP-UV improves the real-time performance and asymptotic optimality of the path planning of unmanned vehicles. Literature

[46] is a path planning algorithm that fuses improved RRT and dynamic window method.

[0095] Table 5 Figure 15 An example of a dynamic environment simulation is shown. The blue and red dots represent the starting point and the endpoint, respectively. The blue squares are dynamic obstacles, from left to right, they are obstacle 1, obstacle 2 and obstacle 3.

[0096] Figure 16 、 Figure 17 ,Figure 18 and Figure 19 Figures 8, 9, 10 and 11 respectively show simulation examples of RSP-UV, the method of literature

[46] , ORRT*-DWA and MTAR in the same dynamic environment. In Figure 16 , the red dashed line represents the global path formed without obstacles. The red solid line represents the path planning process in the dynamic environment. Similarly, in Figure 17 , Figure 18 and Figure 19 , the dashed line represents the global path of the corresponding method. The solid line represents the path formed by the corresponding method in the dynamic environment. Overall, in the same environment, due to the different guide paths generated by each method, ORRT*-DWA and MTAR are only threatened by dynamic obstacle 3. However, RSP-UV and the method of literature

[46] need to avoid three dynamic obstacles. In Figure 16 , Figure (a) and Figure 17 , Figure (a), when the unmanned vehicle encounters obstacle 1, RSP-UV and the method of literature

[46] choose different obstacle avoidance directions. In Figure 16 , Figure (c) and Figure 17 , Figure (c), when the unmanned vehicle avoids obstacle 3, RSP-UV can enter the narrow area for exploration with the same exploration direction. Therefore, RSP-UV can reduce the number of turning points. Overall, RSP-UV has fewer turning points and shorter paths than the method of literature

[46] . From Figure 16 , Figure (d), Figure (d) in Figure 17 , Figure (d), Figure (d) in Figure 18 , and Figure (d) in Figure 19 , the moving positions of the dynamic obstacles can be seen, although RSP-UV needs to avoid three obstacles, it takes the shortest time to complete path planning. This reflects its real-time performance.

[0097] Table 6 Dynamic environment simulation experiment data, from Table 6, it can be seen that the average path length of RSP-UV is reduced by 18.42% compared with the method of literature

[46] . Compared with the methods of literature

[46] , ORRT*-DWA and MTAR, the average running time of RSP-UV is reduced by 22.50%, 11.7% and 5.18% respectively. The average turning angle of RSP-UV is less than the average of the methods of literature

[46] and MTAR. This shows that RSP-UV turns less, reduces the path cost, and the path traveled by the unmanned vehicle is smoother.

[0098] Table 6 Experiments are experimentally verified using an autonomous small unmanned vehicle as a physical platform. The hardware platform is as follows Figure 20The chassis system is composed of a front-mounted Ackerman steering mechanism, two rear-mounted independent motors, and an actuator to provide driving force for the vehicle. The upper computer is equipped with a ROS operating system and executes an embedded improved fusion path planning algorithm. The application connects the laser radar to the upper computer through an Ethernet interface. The application connects the inertial measurement unit to the upper computer through a USB interface. In addition, the application needs to ensure that the upper computer and the laser radar are in the same network to communicate through the ROS system. The upper computer communicates with the vehicle chassis through a CAN bus. After sending operation commands to the control chassis, the driver sends a command to rotate the unmanned vehicle motor. Therefore, the unmanned vehicle completes the path planning task. The parameters of the unmanned vehicle are shown in Table 7. The information of the experimental hardware equipment is shown in Table 8. The maximum linear speed of the unmanned vehicle is 0.5 m / s, and the maximum angular speed is 0.25 rad / s. In order to test the environmental adaptability of RSP-UV, the application is tested in three environments. The application compares the path planned by RSP-UV with the path planned by the method in document

[46] , ORRT*-DWA, and MTAR. The real experimental environment is shown in Figure 21 . Figure 21 Figure (a) in the above is a long straight corridor with simple structure and sufficient light. Figure 21 Figure (b) in the above is a garage scene with relatively dark light. Figure 21 Figure (c) in the above is a rainy environment on an unstructured urban road.

[0099] Table 7 is the vehicle body structure parameters.

[0100] Table 7 Table 8 is the experimental equipment.

[0101] Table 8 Firstly, the application performs preliminary testing in a corridor environment to verify the practical feasibility of RSP-UV. As shown in Figure 22 , a cardboard box (red square) is used as a static unknown obstacle in the corridor environment. The yellow dot represents a dynamic obstacle (pedestrian) moving at a constant speed. The dynamic obstacles in the garage and unstructured urban road environments are also represented by yellow dots. Figure 22 Figure (a) in the above, Figure 22 Figure (b) in the above, and Figure 22 Figure (c) in the above show the dynamic obstacle avoidance process of the unmanned vehicle. The starting point A and the target point B are set. As shown in Figure 22 Figure (a) in the above, at this time the unmanned vehicle moves in front of the yellow obstacle. If the unmanned vehicle maintains the current motion, there is a high probability of collision with the dynamic obstacle. Figure 22Fig. 3(b) in the accompanying drawings illustrates the obstacle avoidance process of the unmanned vehicle. After detecting the dynamic obstacle, the unmanned vehicle successfully avoids the dynamic obstacle by adjusting the direction. Figure 22 Fig. 3(c) in the accompanying drawings illustrates the final path generated by RSP-UV. Figure 22 Fig. 3(d) in the accompanying drawings, Figure 22 Fig. 3(e) in the accompanying drawings, and Figure 23 Fig. 3(f) in the accompanying drawings respectively illustrate the path trajectories generated by literature

[46] , ORRT*-DWA and MTAR under the same conditions. By comparing these four figures, it can be seen from the present application that RSP-UV improves the asymptotic optimality of path planning. In addition, the fluctuation of RSP-UV path is relatively small. This indicates that RSP-UV increases the driving stability of the unmanned vehicle.

[0102] Figure 23 Fig. 4(a) in the accompanying drawings, Figure 23 Fig. 4(b) in the accompanying drawings, Figure 23 Fig. 4(c) in the accompanying drawings, and Figure 23 Fig. 4(d) in the accompanying drawings respectively illustrate the example graphs for comparing RSP-UV, literature

[46] , ORRT*-DWA and MTAR in the garage environment. The starting point A and the target point B are set. From Figure 24 It can be seen from Fig. 4 that when avoiding the dynamic obstacle (yellow point), RSP-UV selects the right obstacle avoidance, while the other methods select the left obstacle avoidance. In terms of the moving distance of the dynamic obstacle, it is observed that the time required for RSP-UV to avoid the obstacle is the shortest. This indicates that RSP-UV improves the real-time performance of path planning. In terms of the overall path, it can be seen that RSP-UV plans the shortest path distance, improving the asymptotic optimality of the path.

[0103] Figure 24 Fig. 5(a) in the accompanying drawings, Figure 24 Fig. 5(b) in the accompanying drawings, Figure 24 Fig. 5(c) in the accompanying drawings, and Figure 24 Fig. 5(d) in the accompanying drawings respectively illustrate the example graphs for comparing RSP-UV, literature

[46] , ORRT*-DWA and MTAR in the unstructured urban road. From Figure 24 Fig. 5(a) in the accompanying drawings, and Figure 25It can be seen from Fig. (b) in Fig. 7 that the path formed by RSP-UV is more similar to the path formed by the method in

[46] . When avoiding the dynamic obstacle (yellow dot), the path formed by RSP-UV is relatively smooth. In terms of the distance traveled by the dynamic obstacle, in terms of the time spent avoiding the dynamic obstacle, RSP-UV is close to ORRT*-DWA and MTAR. However, RSP-UV and ORRT*-DWA choose different directions to avoid the obstacle. RSP-UV chooses a narrower side, which shortens the path length. When they avoid the dynamic obstacle, the turning angle of MTAR is larger than that of RSP-UV. This shows that the path formed by RSP-UV is smoother than the path of MTAR.

[0104] Table 9 is the real environment experimental data.

[0105] Table 9 Table 9 shows the average length, average time and average number of corners of the paths generated by the four methods in three different environments with different starting points and ending points. As can be seen from Table 9, in all three environments, RSP-UV finds the shortest average path length and the lowest average path planning time. This shows that RSP-UV outperforms the other four methods in terms of path planning.

[0106] For a comprehensive evaluation, the average path length, average planning time and corners of the four methods in the three environments are re-averaged into statistical charts, respectively, as shown in Figure 25 . Figure 1 vividly depicts the superiority of RSP-UV. The average path length of RSP-UV is 2.26% shorter than

[46] , 3.03% shorter than ORRT*-DWA, and 3.33% shorter than MTAR. The average planning time of RSP-UV is 7.16% less than

[46] , 2.15% less than ORRT*-DWA, and 8.78% less than MTAR. The average corners of RSP-UV are reduced by 22.99% compared with

[46] , and the average corners of RSP-UV are reduced by 2.90% compared with ORRT*-DWA and MTAR, respectively.

[0107] In summary, the path formed by RSP-UV has the lowest cost, the highest safety and the best adaptability. In terms of ensuring the reliability, real-time and asymptotic optimality of path planning, RSP-UV performs best.

[0108] In the present application, the present application proposes a real-time path planning method RSP-UV for unmanned vehicles. By improving the RRT* algorithm, a partition sampling strategy and pruning and smoothing processing of the path set are developed. This to some extent overcomes the problems of low sampling efficiency, high expansion randomness and non-smooth path planning. The present application designs a fuzzy controller to improve the dynamic feasibility of DWA and integrates it into the improved RRT* algorithm. For the path backtracking phenomenon, the present application proposes a return path set rule. In this way, the dynamic obstacle avoidance ability of the unmanned vehicle is improved, and an asymptotically optimal path can be obtained. The reliability, real-time performance and asymptotic optimality of the RSP-UV algorithm are verified through a large number of simulations and actual experiments. However, the path found by RSP-UV is asymptotically optimal.

[0109] In the application of the real-time sampling based path planning method for unmanned vehicles provided by the present application, the execution order of each step shown in the figure can be determined according to the needs, and the present application does not limit the execution order of each step. Figure 26

[0110] The above is the real-time sampling based path planning method for unmanned vehicles provided by one or more embodiments of the present application. Based on the same idea, the present application also provides a corresponding real-time sampling based path planning device for unmanned vehicles, as shown in Figure 26

[0111] Figure 1 A real-time sampling based path planning device for unmanned vehicles provided by the present application is shown in the figure, which comprises: The acquisition module 2601 is configured to acquire a regional map including the starting point and target point of the unmanned vehicle, initial control parameters of the unmanned vehicle and model parameters of the unmanned vehicle. The initial control parameters of the unmanned vehicle include linear velocity and angular velocity. The model parameters of the unmanned vehicle include sensor parameters, positioning parameters and safety parameters.

[0112] The search module 2602 is configured to divide the regional map into a plurality of concentric sampling regions with the starting point of the unmanned vehicle and the target point of the unmanned vehicle as the symmetry axis, and perform partition search in the plurality of concentric sampling regions through a dynamic density adjustment algorithm to obtain an initial global path.

[0113] The pruning and smoothing processing module 2603 is configured to perform path pruning on the initial global path, and perform path smoothing processing on the initial global path after path pruning to obtain a global path.

[0114] The selection module 2604 is configured to perform path selection based on the global path, the weight of the trajectory evaluation function, the initial state of the unmanned vehicle and the model parameters of the unmanned vehicle in the improved DWA algorithm to obtain a lowest cost path. The weight of the trajectory evaluation function in the improved DWA algorithm is dynamically generated by the fuzzy controller. ​​

[0115] The determining module 2605 is configured to determine the lowest cost path as the path of the unmanned vehicle.

[0116] For specific limitations of the unmanned vehicle path planning device based on real-time sampling, refer to the limitations of the unmanned vehicle path planning method based on real-time sampling described above, which will not be repeated here. Each module in the above-described unmanned vehicle path planning device based on real-time sampling can be realized by software, hardware, and a combination thereof, in whole or in part. The above-described each module can be embedded in or independent of the processor in the computer device in hardware form, or can be stored in the memory in the computer device in software form, so as to be called and executed by the processor to perform the operations corresponding to the above each module.

[0117] The application further provides a computer readable storage medium, which stores a computer program, and the computer program can be used to execute the above-mentioned Figure 27 The application provides an unmanned vehicle path planning method based on real-time sampling.

[0118] The application further provides an unmanned vehicle path planning device based on real-time sampling. Figure 27 The structure schematic diagram of the computer device is shown in FIG. 1. Figure 1 As shown in FIG. 1, at the hardware level, the computer device includes a processor, an internal bus, a network interface, a memory, and a non-volatile memory, and of course can also include other hardware required by business. The processor reads the corresponding computer program from the non-volatile memory into the memory and then runs to realize the above-mentioned ​ The application provides an unmanned vehicle path planning method based on real-time sampling.

[0119] Those skilled in the art can understand that all or part of the processes in the above-mentioned embodiment methods can be completed by a computer program instructing related hardware. The computer program can be stored in a non-volatile computer readable storage medium and can include the processes of the above-mentioned embodiments when executed. Any reference to the memory, storage, database, or other medium used in each embodiment of the application can include at least one of the non-volatile and volatile memories. The non-volatile memory can include a read-only memory (ROM), a tape, a floppy disk, a flash memory, or an optical memory. The volatile memory can include a random access memory (RAM) or an external cache memory. As an illustration but not limitation, the RAM can be in various forms, such as a static random access memory (SRAM) or a dynamic random access memory (DRAM).

[0120] The technical features of the above embodiments can be combined in any manner. For the sake of brevity, not all possible combinations of the technical features in the above embodiments are described, however, as long as the combinations of the technical features do not contradict each other, they should be considered to be within the scope of the present application.

Claims

1. A method for unmanned vehicle path planning based on real-time sampling, characterized in that, include: Acquire regional maps including the starting and target points of the autonomous vehicle, the initial control parameters of the autonomous vehicle, and the parameters of the autonomous vehicle model; The initial control parameters of an autonomous vehicle include linear velocity and angular velocity; the model parameters of an autonomous vehicle include sensor parameters, positioning parameters, and safety parameters. The region map is divided into multiple concentric sampling regions with the starting point and target point of the autonomous vehicle as the axis of symmetry. An initial global path is obtained by performing partition search in multiple concentric sampling regions through a dynamic density adjustment algorithm. The higher the level of the sampling region, the lower the sampling density. The initial global path is pruned, and the pruned initial global path is smoothed to obtain the global path. In the improved DWA algorithm, path selection is performed based on the global path, the weights of the trajectory evaluation function, the initial state of the autonomous vehicle, and the parameters of the autonomous vehicle model to obtain the lowest-cost path. The weights of the trajectory evaluation function in the improved DWA algorithm are dynamically generated using a fuzzy controller. The trajectory evaluation function is: ； in, It is the total score of all scoring items. This represents the score indicating the angular deviation of the unmanned vehicle's attitude at the end of the simulated trajectory relative to the heading at the target point. The score represents the distance between the end of the simulated trajectory and the nearest obstacle. The score represents the linear velocity corresponding to the simulated trajectory. This represents the proportion of the yaw angle sub-function. This represents the proportion of the distance sub-function. This represents the proportion of the velocity sub-function. It is the percentage of the total score. v This represents the linear velocity of the autonomous vehicle. This represents the angular velocity of the driverless car; The weights are based on the trajectory evaluation function dynamically generated by the fuzzy controller; the fuzzy rules of the fuzzy controller include: when the distance between the autonomous vehicle and its target point and the distance between the autonomous vehicle and the nearest obstacle are both within a first distance range, a second distance is used. ,second and first When the distance between the autonomous vehicle and its target point is at the first distance and the distance between the autonomous vehicle and the nearest obstacle is at the second distance, the third distance is used. ,First Second When the distance between the autonomous vehicle and its target point is at the second distance but the distance between the autonomous vehicle and the nearest obstacle is at the first distance, the first distance is used. ,third Second When the distance between the autonomous vehicle and its target point is at the first distance and the distance between the autonomous vehicle and the nearest obstacle is at the second distance, the third distance is used. and the third And the second ; The lowest cost path is determined as the path for the autonomous vehicle.

2. The method as described in claim 1, characterized in that, The process involves dividing the regional map into multiple concentric sampling regions with the autonomous vehicle's starting point and target point as the axis of symmetry, and then performing a partitioned search within these concentric sampling regions using a dynamic density adjustment algorithm to obtain the initial global path. Specifically, this includes: The regional map is expanded to both sides of the straight line connecting the starting point and the target point of the autonomous vehicle as the axis of symmetry, and divided into multiple concentric sampling areas. In the first-level sampling region, a path search is performed using a random tree algorithm. When the sampling density of the path search is greater than the sampling density value set in the first-level region, the search is extended to the second-level sampling region, and the sampling density is dynamically adjusted according to the region level. The first-level sampling region consists of the two sampling regions closest to the axis of symmetry. The higher the region level, the lower the sampling density. In the second-layer sampling region, a path search is performed using a random tree until the search stops; the search stops when a feasible path is found or the last layer of sampling region is searched. The searched feasible paths are determined as the initial global paths.

3. The method as described in claim 2, characterized in that, The path pruning of the initial global path specifically includes: The initial global path is uniformly discretized into a set of discrete points; Select the starting point of the initial global path as the last point of the trimmed path; Traverse the set of discrete points and determine the visibility between the discrete points and the last point of the trimming path; If the judgment result is invisible, then obtain the pixel center of the obstacle object that is occluding the current discrete point; Along a direction perpendicular to the line connecting the discrete point and the last point of the trimming path, and coplanar with the line connecting the obstacle object center, the position of the obstacle object center is adjusted to obtain the adjustment point. Add the adjustment point to the point set corresponding to the trimming path, and update the last point of the trimming path to the adjustment point; After traversing the set of discrete points, an initial global path after path trimming is generated.

4. The method as described in claim 3, characterized in that, The process of smoothing the initial global path after path trimming to obtain the global path specifically includes: The initial global path after path trimming is discretized into multiple nodes, with each group consisting of three adjacent nodes. For any given set, three adjacent nodes are used as controls of a quadratic B-spline curve to construct the quadratic B-spline curve, and the constructed quadratic B-spline curve is used as a smooth path. Connect the smooth paths generated by all groups to form the global path.

5. The method as described in claim 4, characterized in that, The method further includes: if the constructed quadratic B-spline curve collides with an obstacle, the curvature of the quadratic B-spline curve is changed by moving the intermediate control point so that the quadratic B-spline curve with the changed curvature bypasses the obstacle.

6. The method as described in claim 1, characterized in that, The path selection based on the global path, the weights of the trajectory evaluation function, the initial state of the autonomous vehicle, and the parameters of the autonomous vehicle model to obtain the lowest-cost path specifically includes: Get the node set corresponding to the global path and the node set of the path to be expanded; For each node in the node set corresponding to the global path, detect whether there is a moving obstacle between the node and the candidate nodes in the node set of the path to be expanded; If there are moving obstacles, the improved DWA algorithm is initialized, and the initial control parameters of the unmanned vehicle are adjusted in real time based on the weights of the trajectory evaluation function dynamically generated by the fuzzy controller. The simulated trajectory is predicted based on the adjusted initial control parameters and model parameters of the unmanned vehicle, and the cost of the simulated trajectory is calculated through the trajectory evaluation function. When the distance between the simulated trajectory and the obstacle is detected to be less than the preset distance, a collision risk is detected. If a collision risk exists, a temporary obstacle avoidance path is generated from the current position of the driverless vehicle to the target point of the driverless vehicle. Calculate the cost of the temporary obstacle avoidance path, compare the cost of the temporary obstacle avoidance path with the cost of the simulated trajectory, and select the path with the lower cost as the updated trajectory; Iterate through each node in the set of nodes corresponding to the global path to generate the lowest cost path.

7. The method as described in claim 6, characterized in that, The trajectory evaluation function is: ； in, It is the total score of all scoring items. This represents the score indicating the angular deviation of the unmanned vehicle's attitude at the end of the simulated trajectory relative to the heading at the target point. The score represents the distance between the end of the simulated trajectory and the nearest obstacle. The score represents the linear velocity corresponding to the simulated trajectory. This represents the proportion of the yaw angle sub-function. This represents the proportion of the distance sub-function. This represents the proportion of the velocity sub-function. It is the percentage of the total score. v This represents the linear velocity of the autonomous vehicle. This represents the angular velocity of the driverless car.

8. The method as described in claim 6, characterized in that, The weights of the trajectory evaluation function dynamically generated based on the fuzzy controller specifically include: The distance between the autonomous vehicles and their target points, and the distance between the autonomous vehicles and their nearest obstacles, are used as inputs to the fuzzy controller. The input to the fuzzy controller is processed by fuzzy rules to obtain the weights of the trajectory evaluation function.

9. The method as described in claim 8, characterized in that, The fuzzy rules include: when the distance between the autonomous vehicle and its target point, and the distance between the autonomous vehicle and the nearest obstacle, are both within a first distance range, a second fuzzy rule is used. ,second and first When the distance between the autonomous vehicle and its target point is at the first distance and the distance between the autonomous vehicle and the nearest obstacle is at the second distance, the third distance is used. ,First Second When the distance between the autonomous vehicle and its target point is at the second distance but the distance between the autonomous vehicle and the nearest obstacle is at the first distance, the first distance is used. ,third Second When the distance between the autonomous vehicle and its target point is at the first distance and the distance between the autonomous vehicle and the nearest obstacle is at the second distance, the third distance is used. and the third And the second .

10. The method as described in claim 6, characterized in that, The method further includes: In the improved DWA algorithm, nodes in the global path are used as local target points for the dynamic window algorithm; During the path tracking process, the angle between the line connecting the current position of the unmanned vehicle and the current local target point and the global path is calculated in real time. After completing dynamic obstacle avoidance, if the included angle is greater than or equal to the threshold angle, the current local target point is removed, and the local target points are sequentially judged to meet the navigation conditions until a local target point that meets the navigation conditions is selected as a new guide point.

Images