Unmanned inspection vehicle autonomous path planning method based on dynamic obstacle avoidance

Abstract

The invention discloses an unmanned inspection vehicle autonomous path planning method based on dynamic obstacle avoidance. The method comprises the following steps: constructing a two-dimensional search network of a local planning area; generating a plurality of motion primitives based on a vehicle dynamics model for connecting the sampling nodes, and determining a feasible path segment; adopting a structured sampling tree to expand layer by layer, and dynamically constructing a candidate trajectory tree in combination with obstacle information, a behavior strategy and a path cost function; and selecting a path with the optimal total cost from the candidate trajectory tree, splicing a motion primitive trajectory corresponding to the optimal path, and outputting an expected trajectory. The method is high in planning speed, high in trajectory quality and high in behavior consistency, and can meet the requirements of real-time performance, feasibility and trajectory optimality under dynamic obstacle collision avoidance.

Description

Technical Field

[0001] This invention relates to the fields of autonomous driving and intelligent vehicle control technology, and in particular to an autonomous path planning method for unmanned inspection vehicles based on dynamic obstacle avoidance. Background Technology

[0002] Autonomous driving technology has developed rapidly in recent years, but its motion planning in complex dynamic scenarios still faces challenges. Traditional optimization methods based on model predictive control (MPC) perform well in static obstacle environments, but often struggle to achieve real-time response when facing dynamic obstacles due to model complexity and sensitivity to initial conditions.

[0003] Sampling-based methods (such as RRT*) have certain advantages in exploring high-dimensional spaces, but their path feasibility and computational stability are difficult to guarantee in dynamic obstacle scenarios. Graph-based methods, on the other hand, are difficult to extend to dynamic obstacle scenarios due to the curse of dimensionality.

[0004] Therefore, there is an urgent need for a structured planning framework that can combine sampling strategies with rapid evaluation of motion primitives to solve the real-time dynamic obstacle avoidance problem. Summary of the Invention

[0005] The purpose of this invention is to provide an autonomous path planning method for unmanned inspection vehicles based on dynamic obstacle avoidance, so as to meet the requirements of real-time performance, feasibility and trajectory optimization under dynamic obstacle collision avoidance. By constructing a sampling tree through rapidly assessable motion primitives and combining semantic behavior strategies and optimal path cost functions, the computational efficiency is greatly improved while maintaining feasible trajectories.

[0006] To solve the above technical problems, the technical solution of the present invention is: an autonomous path planning method for unmanned inspection vehicles based on dynamic obstacle avoidance, comprising the following steps:

[0007] Step S1: Based on the vehicle position and local planning area, combined with lane information and obstacle information in the high-precision map, determine a set of path reference lines arranged along the longitudinal direction of the lane, and generate a two-dimensional search grid;

[0008] Step S2: Based on the vehicle dynamics model and combined with the two-dimensional search mesh, generate multiple motion primitives, then connect adjacent sampling nodes through motion primitives to obtain motion primitive trajectories and perform feasibility judgment to determine valid motion primitive trajectories;

[0009] Step S3: Based on the two-dimensional search grid and the effective motion primitive trajectories, construct a sampling tree, then expand the sampling tree layer by layer according to the path reference line direction, and dynamically construct a candidate trajectory tree by combining obstacle information, behavior strategy and path cost function;

[0010] Step S4: Based on the path cost function, the optimal path with the best total cost is selected from the candidate trajectory tree through recursive backtracking of the sampling nodes. The corresponding motion primitive trajectories of the optimal path are spliced ​​together, and the desired trajectory is output to realize the autonomous path planning of the unmanned inspection vehicle.

[0011] Preferably, the implementation process of step S1 is as follows:

[0012] (1) The current position and orientation of the vehicle are represented by a state vector. Represented as:

[0013]

[0014] in, The vehicle's current location. This is the vehicle's current heading angle;

[0015] (2) Suppose that the reference path centerline provided by the high-precision map is a directed spatial curve, and its expression is as follows:

[0016]

[0017] in, For space curves, The coordinates of the points on the spatial curve. For arc length parameters, The reference path is the length within the current planning range;

[0018] (3) Within the current planning range, the centerline of the reference path is set according to the longitudinal interval. Equidistant sampling generates a set of points for the longitudinal reference line, as shown in the following formula:

[0019]

[0020]

[0021]

[0022] in, For the set of points along the vertical reference line, For the first One vertical reference line point, This represents the number of points on the vertical reference line.

[0023] (4) For each longitudinal reference line point Calculate the coordinates and unit normal vector of the corresponding lane center point;

[0024] The coordinates of the lane center point are:

[0025]

[0026] in, The center point of the lane;

[0027] The normal direction vector of the lane center point is:

[0028]

[0029] in, The normal direction vector of the lane center point. For the first The heading angle of each longitudinal reference line point;

[0030] (5) Based on the center point of each lane Within the current planning scope, horizontally at intervals Expand to the left and right respectively These nodes constitute discrete search points along the reference path;

[0031] The node coordinates are:

[0032]

[0033] in, For the first The first layer of the reference path One horizontal sampling node;

[0034] Therefore, all nodes can be represented as a two-dimensional mesh structure. :

[0035]

[0036] (6) Traverse all nodes and check whether the nodes are within the safe area using the obstacle grid or boundary. The constraints are as follows:

[0037]

[0038] in, A collection of obstacles. Minimum safe distance; For nodes Euclidean distance to the nearest obstacle;

[0039] Nodes that meet the constraints are marked as available and participate in subsequent expansion; nodes that do not meet the constraints are marked as unavailable and do not participate in subsequent expansion.

[0040] (7) The output structured two-dimensional search grid is:

[0041]

[0042] in, It is a two-dimensional search grid. For the first The first layer of the reference path One horizontal sampling node.

[0043] Preferably, in step S2, the vehicle dynamics model is a nonlinear monorail vehicle kinematic model, expressed as follows:

[0044]

[0045]

[0046]

[0047]

[0048] in, For vehicle location, For the vehicle's heading angle, For the longitudinal speed of the vehicle, For the longitudinal acceleration of the vehicle, This refers to the vehicle's wheelbase.

[0049] Preferably, the implementation process of step S2 is as follows:

[0050] (1) The current status of the vehicle is With a set of control inputs Maintain for 1 second to obtain the vehicle's terminated state. This process is taken as a basic motion element and denoted as:

[0051]

[0052] in, As the basic unit of motion, for Real-time vehicle status. For the duration of the basic movement element, The steering angle of the front wheels;

[0053] (2) Generate a motion primitive library:

[0054] Discretize the control input space: ,in, The maximum front wheel steering angle and ;

[0055] For each group of control inputs Through numerical integration on the duration of motion primitives The vehicle state equation is integrated internally to generate corresponding trajectory segments. Each trajectory segment includes a continuous sequence of vehicle state points. The continuous sequence of state points is stored as a set of trajectory points to obtain a motion primitive library.

[0056] (3) To make the node With the target node Connections require feasibility assessment through motion element connections:

[0057] a. Assuming the current state of the vehicle Can the generated motion primitive library make the vehicle's endpoint approach the target node? ;

[0058] b. Calculate the distance between the vehicle's destination and the target node. Error:

[0059]

[0060] in, This is the error value; if This indicates that the motion primitives have been successfully matched. Conversely, This is the error threshold; This is the vehicle's final destination. The target node location;

[0061] c. Determine whether the successfully matched trajectory of the motion primitives satisfies the constraints of obstacle-free collision and smoothness:

[0062] Obstacle collision detection: Whether the trajectory point crosses the obstacle area;

[0063] Curvature constraint: ;

[0064] in, Let be the trajectory curvature at time t. Let be the derivative of the heading angle at time t. Let be the vehicle speed at time t. This represents the maximum curvature of the trajectory.

[0065] Trajectories that satisfy the constraints are valid motion primitive trajectories and are stored in the sampling tree structure of step S3; otherwise, the opposite is true.

[0066] Preferably, the implementation process of step S3 is as follows:

[0067] (1) Construct the sampling tree, the expression is: ,in, Let be a set of nodes, representing the discrete states of a vehicle; Let be the set of edges, representing the edges from node . To the node Motion element connections;

[0068] (2) Use the current state of the vehicle as the root node of the sampling tree. The two-dimensional mesh structure generated in step S1 As a search space, a layer-by-layer expansion mechanism is adopted, starting from the first... Layer path extended to the first Layered paths: Each layer of the path attempts to connect all feasible candidate nodes to the next layer of the path, dynamically constructing a candidate trajectory tree, including:

[0069] a. Expanding the candidate set:

[0070] For each current node Find the set of target nodes for the next level path. ;

[0071] b. Trajectory connection attempt:

[0072] Traversal For each target node To attempt a connection, use the motion primitives from the motion primitive library, specifically: input the current node state. and target node position , For each motion primitive Simulate its final state Calculate the error between the endpoint and the target node:

[0073]

[0074] in, This is the error value. This is the error threshold; if This indicates that the trajectory connection was successful and is recorded as a new edge in the sampling tree. and the target node Add to node set In the middle; if The opposite is true;

[0075] c. Cumulative path cost:

[0076] For each path segment where the trajectory is successfully connected, the cost of that path segment is... Total cost of adding parent node In the middle, update the next node connected to the parent node. The total cost is calculated using the following formula:

[0077]

[0078] d. Repeat the expansion to the last layer:

[0079] Repeat the expansion steps of ac until the last layer is searched, and the expansion is complete;

[0080] After expansion, the complete node connection relationship is stored in the sampling tree. Using the reference relationship between child nodes and parent nodes, the trajectory path from the root node to any end node can be traced back, as shown below:

[0081]

[0082] in, This is the end node.

[0083] Preferably, in step S3, the path cost function is constructed with the goal of minimizing the total cost, and its expression is as follows:

[0084]

[0085]

[0086]

[0087]

[0088] in, Path segment The total cost, Cost of trajectory length The cost of obstacle distance, The cost of behavioral consistency, , , These are the weighting coefficients for the corresponding costs. For the first Position of each trajectory point No. The location of each trajectory point The number of discrete points on the trajectory. For the first The Euclidean distance from each trajectory point to the nearest obstacle Minimum safe distance This is the deviation penalty factor.

[0089] Preferably, before constructing the path cost function, the trajectory length cost, obstacle distance cost, and behavior consistency cost are normalized, as shown in the following formula:

[0090]

[0091] in, The cost of normalization For the current cost, Minimum cost For the maximum cost, It is a tiny integer.

[0092] Preferably, the implementation process of step S4 is as follows:

[0093] (1) Optimal path backtracking:

[0094] The final node with the minimum total cost is selected from the feasible candidate nodes, using the following formula:

[0095]

[0096] in, The terminal node with the minimum total cost. For nodes The current total cost;

[0097] By recursively backtracking through parent node references, the optimal path with the total cost is obtained, as expressed below:

[0098]

[0099] in, The root node of the candidate trajectory tree. The terminal node with the minimum total cost;

[0100] (2) The motion element trajectory segments in the optimal path are spliced ​​together in sequence to obtain the desired trajectory, as shown in the following formula:

[0101]

[0102] in, For the desired trajectory, For the first Segment trajectory, This represents the number of trajectory segments.

[0103] Compared with the prior art, the beneficial effects of the present invention are as follows: The present invention proposes a scalable real-time motion planning framework that combines sampling efficiency and trajectory quality; it improves search efficiency by using structured reference trajectory points and motion primitive design; it can adapt to different scenario requirements of unmanned inspection vehicles, generate collision-free trajectories in dynamic obstacle-dense scenarios, and the computation time meets real-time requirements. Attached Figure Description

[0104] To more clearly illustrate the technical solutions of the embodiments of the present invention, the drawings used in the description of the embodiments of the present invention will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0105] Figure 1 This is a flowchart illustrating the autonomous path planning method for unmanned inspection vehicles based on dynamic obstacle avoidance according to the present invention.

[0106] Figure 2 This is a schematic diagram of the two-dimensional search grid in this invention;

[0107] Figure 3 This is a schematic diagram of the trajectory of the motion element in this invention;

[0108] Figure 4 This is a schematic diagram illustrating the extended motion element search in this invention. Detailed Implementation

[0109] The present invention will now be described in further detail with reference to the accompanying drawings. The terminal technical solutions of the embodiments of the present invention will be clearly and completely described. Obviously, the described embodiments are only some, not all, of the embodiments of the present invention. All other embodiments obtained by those skilled in the art based on the embodiments of the present invention without creative effort are within the scope of protection of the present invention.

[0110] like Figure 1 As shown, this invention provides an autonomous path planning method for unmanned inspection vehicles based on dynamic obstacle avoidance, comprising the following steps:

[0111] Step S1: Based on the vehicle position and local planning area, combined with lane information and obstacle information in the high-precision map, determine a set of path reference lines arranged along the longitudinal direction of the lane, and generate a two-dimensional search grid.

[0112] (1) The current position and orientation of the vehicle are represented by a state vector. Represented as:

[0113]

[0114] in, The vehicle's current location. This is the vehicle's current heading angle;

[0115] (2) Suppose that the reference path centerline provided by the high-precision map is a directed spatial curve, and its expression is as follows:

[0116]

[0117] in, For space curves, The coordinates of the points on the spatial curve. For arc length parameters, The reference path is the length within the current planning range;

[0118] (3) Within the current planning range, the centerline of the reference path is set according to the longitudinal interval. Equidistant sampling generates a set of points for the longitudinal reference line, as shown in the following formula:

[0119]

[0120]

[0121]

[0122] in, For the set of points along the vertical reference line, For the first One vertical reference line point, This represents the number of points on the vertical reference line.

[0123] (4) For each longitudinal reference line point Calculate the coordinates and unit normal vector of the corresponding lane center point.

[0124] The coordinates of the lane center point are:

[0125]

[0126] in, The center point of the lane;

[0127] The normal direction vector of the lane center point is:

[0128]

[0129] in, The normal direction vector of the lane center point. For the first The heading angle of each longitudinal reference line point;

[0130] (5) Based on the center point of each lane Within the current planning scope, horizontally at intervals Expand to the left and right respectively These nodes constitute discrete search points along the reference path.

[0131] The node coordinates are:

[0132]

[0133] in, For the first The first layer of the reference path One horizontal sampling node;

[0134] Therefore, all nodes can be represented as a two-dimensional mesh structure. :

[0135]

[0136] (6) Traverse all nodes and check whether the nodes are within the safe area using the obstacle grid or boundary. The constraints are as follows:

[0137]

[0138] in, A collection of obstacles. Minimum safe distance; For nodes Euclidean distance to the nearest obstacle.

[0139] Nodes that meet the constraints are marked as available and participate in subsequent expansion; nodes that do not meet the constraints are marked as unavailable and do not participate in subsequent expansion.

[0140] (7) The output structured two-dimensional search grid is:

[0141]

[0142] in, It is a two-dimensional search grid. For the first The first layer of the reference path One horizontal sampling node.

[0143] The two-dimensional search grid serves as the search space basis for the structured sampling tree expansion in step S3, supporting rapid path construction, heuristic evaluation, and multi-target trajectory generation.

[0144] Step S2: Based on the vehicle dynamics model and combined with a two-dimensional search mesh, generate multiple motion primitives, then connect adjacent sampling nodes through motion primitives to obtain motion primitive trajectories and perform feasibility assessment to determine valid motion primitive trajectories.

[0145] The vehicle dynamics model is a nonlinear kinematic bicycle model, which describes the vehicle's lateral and longitudinal motion, as follows:

[0146]

[0147]

[0148]

[0149]

[0150] in, For vehicle location, For the vehicle's heading angle, For the longitudinal speed of the vehicle, For the longitudinal acceleration of the vehicle, This refers to the vehicle's wheelbase.

[0151] (1) The current status of the vehicle is With a set of control inputs Maintain for 1 second to obtain the vehicle's terminated state. This process is taken as a basic motion element and denoted as:

[0152]

[0153] in, As the basic unit of motion, for Real-time vehicle status. For the duration of the basic movement element, The steering angle of the front wheels;

[0154] (2) Generate a motion primitive library:

[0155] Discretize the control input space: ,in, The maximum front wheel steering angle and .

[0156] For each group of control inputs Through numerical integration on the duration of motion primitives Integrating the vehicle state equations internally generates the corresponding trajectory segments. Each trajectory segment includes a continuous sequence of vehicle state points, wherein the state points are... The continuous sequence of state points is stored as a set of trajectory points to obtain a motion primitive library, which is used for subsequent motion primitive connection and obstacle collision detection.

[0157] (3) To make the node With the target node Connections require both motion element connection and feasibility assessment.

[0158] a. Assuming the current state of the vehicle Can the generated motion primitive library make the vehicle's endpoint approach the target node? .

[0159] b. Calculate the distance between the vehicle's destination and the target node. Error:

[0160]

[0161] in, This is the error value; if This indicates that the motion primitives have been successfully matched. Conversely, This is the error threshold; This is the vehicle's final destination. The target node location.

[0162] c. Determine whether the successfully matched trajectory of the motion primitives satisfies the constraints of obstacle-free collision and smoothness:

[0163] Obstacle collision detection: Whether the trajectory point crosses the obstacle area;

[0164] Curvature constraint: ;

[0165] in, Let be the trajectory curvature at time t. Let be the derivative of the heading angle at time t. Let be the vehicle speed at time t. This represents the maximum curvature of the trajectory.

[0166] Trajectories that satisfy the constraints are valid motion element trajectories, which can be regarded as valid connection methods between two nodes and stored in the sampling tree structure of step S3; otherwise, the opposite applies.

[0167] Step S3: Based on the two-dimensional search grid and the effective motion primitive trajectories, construct a sampling tree, then expand the sampling tree layer by layer according to the path reference line direction, and dynamically construct a candidate trajectory tree by combining obstacle information, behavior strategy and path cost function.

[0168] (1) Construct the sampling tree, the expression is: ,in, Let be a set of nodes, representing the discrete states of a vehicle; Let be the set of edges, representing the edges from node . To the node Motion element connections;

[0169] (2) Take the current state of the vehicle as the root node of the sampling tree. The two-dimensional mesh structure generated in step S1 As a search space, a layer-by-layer expansion mechanism is adopted, starting from the first... Layer path extended to the first Layered paths: Each layer of the path attempts to connect all feasible candidate nodes to the next layer's path, dynamically constructing a candidate trajectory tree, including:

[0170] a. Expanding the candidate set:

[0171] For each current node Find the set of target nodes for the next level path. .

[0172] b. Trajectory connection attempt:

[0173] Traversal For each target node To attempt a connection, use the motion primitives from the motion primitive library, specifically: input the current node state. and target node position , For each motion primitive Simulate its final state Calculate the error between the endpoint and the target node:

[0174]

[0175] in, This is the error value. This is the error threshold; if This indicates that the trajectory connection was successful and is recorded as a new edge in the sampling tree. and the target node Add to node set In the middle; if Conversely, if the opposite is true.

[0176] c. Cumulative path cost:

[0177] Based on trajectory length cost, obstacle distance cost, and behavior consistency cost, and with the goal of minimizing the total cost, the path cost function is constructed as follows:

[0178]

[0179]

[0180]

[0181]

[0182] in, Path segment The total cost, Cost of trajectory length The cost of obstacle distance, The cost of behavioral consistency, , , These are the weighting coefficients for the corresponding costs. For the first The location of each trajectory point No. The location of each trajectory point The number of discrete points on the trajectory. For the first The Euclidean distance from each trajectory point to the nearest obstacle Minimum safe distance This is the deviation penalty factor.

[0183] Since the three cost dimensions are different, they need to be normalized separately first, as shown in the following formula:

[0184]

[0185] in, The cost of normalization For the current cost, Minimum cost For the maximum cost, It is a tiny integer.

[0186] For each path segment where the trajectory is successfully connected, the total cost of that path segment will be... Total cost of adding parent node In the middle, update the next node connected to the parent node. The total cost is calculated using the following formula:

[0187]

[0188] d. Repeat the expansion to the last layer:

[0189] Repeat the expansion steps of ac until the last layer is searched, and the expansion is complete;

[0190] After expansion, the complete node connection relationship is stored in the sampling tree. Using the reference relationship between child nodes and parent nodes, the trajectory path from the root node to any end node can be traced back, as shown below:

[0191]

[0192] in, This is the end node.

[0193] Step S4: Based on the path cost function, the optimal path with the best total cost is selected from the candidate trajectory tree through recursive backtracking of the sampling nodes. The corresponding motion primitive trajectories of the optimal path are spliced ​​together, and the desired trajectory is output to realize the autonomous path planning of the unmanned inspection vehicle.

[0194] After the sampling tree completes multi-level expansion and covers the local planning area, starting from the root node of the candidate trajectory tree, the optimal path branch with the minimum total cost is backtracked, and the motion primitive trajectories of each segment are spliced ​​in sequence to form a continuous and trackable temporal reference trajectory point sequence, which serves as the tracking target of the motion controller.

[0195] (1) Optimal path backtracking:

[0196] Each sampling tree node stores its parent node and the current total cost. Once the sampling tree expands to the target layer or near the endpoint region, the terminal node with the minimum total cost is selected from the feasible candidate nodes in step S3, as shown in the following formula:

[0197]

[0198] in, The terminal node with the minimum total cost. For nodes The current total cost, Represents the set of feasible candidate nodes

[0199] By recursively backtracking through parent node references, the optimal path with the total cost is obtained, as expressed below:

[0200]

[0201] in, The root node of the candidate trajectory tree. The terminal node with the minimum total cost;

[0202] (2) The motion element trajectory segments in the optimal path are spliced ​​together in sequence to obtain the desired trajectory, including:

[0203] Each pair of adjacent nodes During the sampling tree expansion phase, a corresponding motion element trajectory segment has already been generated;

[0204]

[0205] in, For the first Segmental motion basic unit, for Time of the first The status of each motion element vehicle. The duration of the motion primitive.

[0206] The trajectory segment contains a continuous sequence of vehicle state points, which can be represented as:

[0207]

[0208] in, For the first A sequence of trajectory points generated by a motion primitive. For the first The first motion element vehicle state A state point, This represents the number of state points.

[0209] To ensure time continuity, a sampling time interval is set. ,in, For the basic duration, To determine the number of longitudinal reference points, all trajectory segments are concatenated sequentially to obtain the trajectory sequence of the optimal path, i.e., the desired trajectory:

[0210]

[0211] in, For the desired trajectory, For the first Segment trajectory, This represents the number of trajectory segments.

[0212] To maintain the uniqueness of trajectory points and the consistency of time sequence, duplicate nodes between segments need to be removed when splicing trajectory segments. For example, the last node of each trajectory segment should be equal to the first node of the next trajectory segment.

[0213] All parts not covered in this invention are the same as or implemented using existing technologies.

[0214] The above description, in conjunction with specific embodiments, provides a further detailed explanation of the present invention. It should not be construed that the specific implementation of the present invention is limited to these descriptions. For those skilled in the art, various simple deductions or substitutions can be made without departing from the concept of the present invention, and all such modifications and substitutions should be considered within the scope of protection of the present invention.

Claims

1. An autonomous path planning method for unmanned inspection vehicles based on dynamic obstacle avoidance, characterized in that: Includes the following steps: Step S1: Based on the vehicle position and local planning area, combined with lane information and obstacle information in the high-precision map, determine a set of path reference lines arranged along the longitudinal direction of the lane, and generate a two-dimensional search grid; Step S2: Based on the vehicle dynamics model and combined with the two-dimensional search mesh, generate multiple motion primitives, then connect adjacent sampling nodes through motion primitives to obtain motion primitive trajectories and perform feasibility judgment to determine valid motion primitive trajectories; Step S3: Based on the two-dimensional search grid and the effective motion primitive trajectories, construct a sampling tree, then expand the sampling tree layer by layer according to the path reference line direction, and dynamically construct a candidate trajectory tree by combining obstacle information, behavior strategy and path cost function; Step S4: Based on the path cost function, the optimal path with the best total cost is selected from the candidate trajectory tree through recursive backtracking of the sampling nodes. The corresponding motion primitive trajectories of the optimal path are spliced ​​together, and the desired trajectory is output to realize the autonomous path planning of the unmanned inspection vehicle.

2. The autonomous path planning method for unmanned inspection vehicles based on dynamic obstacle avoidance according to claim 1, characterized in that: The implementation process of step S1 is as follows: (1) The current position and orientation of the vehicle are represented by a state vector. Represented as: in, The vehicle's current location. This is the vehicle's current heading angle; (2) Suppose that the reference path centerline provided by the high-precision map is a directed spatial curve, and its expression is as follows: in, For space curves, The coordinates of the points on the spatial curve. For arc length parameters, The reference path is the length within the current planning range; (3) Within the current planning range, the centerline of the reference path is set according to the longitudinal interval. Equidistant sampling generates a set of points for the longitudinal reference line, as shown in the following formula: in, For the set of points along the vertical reference line, For the first One vertical reference line point, This represents the number of points on the vertical reference line. (4) For each longitudinal reference line point Calculate the coordinates and unit normal vector of the corresponding lane center point; The coordinates of the lane center point are: in, The center point of the lane; The normal direction vector of the lane center point is: in, The normal direction vector of the lane center point. For the first The heading angle of each longitudinal reference line point; (5) Based on the center point of each lane Within the current planning scope, horizontally at intervals Expand to the left and right respectively These nodes constitute discrete search points along the reference path; The node coordinates are: in, For the first The first layer of the reference path One horizontal sampling node; Therefore, all nodes can be represented as a two-dimensional mesh structure. : (6) Traverse all nodes and check whether the nodes are within the safe area using the obstacle grid or boundary. The constraints are as follows: in, A collection of obstacles. Minimum safe distance; For nodes Euclidean distance to the nearest obstacle; Nodes that meet the constraints are marked as available and participate in subsequent expansion; nodes that do not meet the constraints are marked as unavailable and do not participate in subsequent expansion. (7) The output structured two-dimensional search grid is: in, It is a two-dimensional search grid. For the first The first layer of the reference path One horizontal sampling node.

3. The autonomous path planning method for unmanned inspection vehicles based on dynamic obstacle avoidance according to claim 1, characterized in that: In step S2, the vehicle dynamics model is a nonlinear monorail vehicle kinematic model, expressed as follows: in, For vehicle location, For the vehicle's heading angle, For the longitudinal speed of the vehicle, For the longitudinal acceleration of the vehicle, This refers to the vehicle's wheelbase.

4. The autonomous path planning method for unmanned inspection vehicles based on dynamic obstacle avoidance according to claim 1, characterized in that: The implementation process of step S2 is as follows: (1) The current status of the vehicle is With a set of control inputs Maintain for 1 second to obtain the vehicle's terminated state. This process is taken as a basic motion element and denoted as: in, As the basic unit of motion, for Real-time vehicle status. For the duration of the basic movement element, The steering angle of the front wheels; (2) Generate a motion primitive library: Discretize the control input space: ,in, The maximum front wheel steering angle and ; For each group of control inputs Through numerical integration on the duration of motion primitives The vehicle state equation is integrated internally to generate corresponding trajectory segments. Each trajectory segment includes a continuous sequence of vehicle state points. The continuous sequence of state points is stored as a set of trajectory points to obtain a motion primitive library. (3) To make the node With the target node Connections require both motion element connection and feasibility assessment. a. Assuming the current state of the vehicle Can the generated motion primitive library make the vehicle's endpoint approach the target node? ; b. Calculate the distance between the vehicle's destination and the target node. Error: in, This is the error value; if This indicates that the motion primitives have been successfully matched. Conversely, This is the error threshold; This is the vehicle's final destination. The target node location; c. Determine whether the successfully matched trajectory of the motion primitives satisfies the constraints of obstacle-free collision and smoothness: Obstacle collision detection: Whether the trajectory point crosses the obstacle area; Curvature constraint: ; in, Let be the trajectory curvature at time t. Let be the derivative of the heading angle at time t. Let be the vehicle speed at time t. This represents the maximum curvature of the trajectory. Trajectories that satisfy the constraints are valid motion primitive trajectories and are stored in the sampling tree structure of step S3; otherwise, the opposite is true.

5. The autonomous path planning method for unmanned inspection vehicles based on dynamic obstacle avoidance according to claim 1, characterized in that: The implementation process of step S3 is as follows: (1) Construct the sampling tree, the expression is: ,in, Let be a set of nodes, representing the discrete states of a vehicle; Let be the set of edges, representing the edges from node . To the node Motion element connections; (2) Take the current state of the vehicle as the root node of the sampling tree. The two-dimensional mesh structure generated in step S1 As a search space, a layer-by-layer expansion mechanism is adopted, starting from the first... Layer path extended to the first Layered paths: Each layer of the path attempts to connect all feasible candidate nodes to the next layer's path, dynamically constructing a candidate trajectory tree, including: a. Expanding the candidate set: For each current node Find the set of target nodes for the next level path. ; b. Trajectory connection attempt: Traversal For each target node To attempt a connection, use the motion primitives from the motion primitive library, specifically: input the current node state. and target node position , For each motion primitive Simulate its final state Calculate the error between the endpoint and the target node: in, This is the error value. This is the error threshold; if This indicates that the trajectory connection was successful and is recorded as a new edge in the sampling tree. and the target node Add to node set In the middle; if The opposite is true; c. Cumulative path cost: For each path segment where the trajectory is successfully connected, the cost of that path segment is... Total cost of adding a parent node In the middle, update the next node connected to the parent node. The total cost is calculated using the following formula: d. Repeat the expansion to the last layer: Repeat the expansion steps of ac until the last layer is searched, and the expansion is completed; After expansion, the complete node connection relationship is stored in the sampling tree. Using the reference relationship between child nodes and parent nodes, the trajectory path from the root node to any end node can be traced back, as shown below: in, This is the end node.

6. The autonomous path planning method for unmanned inspection vehicles based on dynamic obstacle avoidance according to claim 1, characterized in that: In step S3, with the goal of minimizing the total cost, the path cost function is constructed as follows: in, For path segment The total cost, Cost of trajectory length The cost of obstacle distance, The cost of behavioral consistency, , , These are the weighting coefficients for the corresponding costs. For the first The location of each trajectory point No. The location of each trajectory point The number of discrete points on the trajectory. For the first The Euclidean distance from each trajectory point to the nearest obstacle Minimum safe distance This is the deviation penalty factor.

7. The autonomous path planning method for unmanned inspection vehicles based on dynamic obstacle avoidance according to claim 6, characterized in that: Before constructing the path cost function, the trajectory length cost, obstacle distance cost, and behavior consistency cost are normalized, as shown in the following formula: in, The cost of normalization For the current cost, Minimum cost For the maximum cost, It is a tiny integer.

8. The autonomous path planning method for unmanned inspection vehicles based on dynamic obstacle avoidance according to claim 1, characterized in that: The implementation process of step S4 is as follows: (1) Optimal path backtracking: The final node with the minimum total cost is selected from the feasible candidate nodes, using the following formula: in, The terminal node with the minimum total cost. For nodes The current total cost; By recursively backtracking through parent node references, the optimal path with the total cost is obtained, as expressed below: in, The root node of the candidate trajectory tree. The terminal node with the minimum total cost; (2) The motion element trajectory segments in the optimal path are spliced ​​together in sequence to obtain the desired trajectory, as shown in the following formula: in, For the desired trajectory, For the first Segment trajectory, This represents the number of trajectory segments.

Images