Trajectory planning method and system for narrow unstructured scene with dynamic obstacles

Abstract

The application discloses a kind of trajectory planning method and system of narrow unstructured scene containing dynamic barrier, it includes: step 1, the kinematics model characteristics of four-wheel independent steering vehicle is analyzed, constructs hybrid A* algorithm, obtains unstructured scene relevant parameter;Step 2, according to hybrid A* algorithm, combined with improved collision detection algorithm, solve initial path without collision;Improved collision detection algorithm includes: according to vehicle body contour, construct equal division multiple disc profile and barrier inflation map, calculate equal division multiple disc center coordinates, by traversing each disc center, it is judged whether disc center is interfered with barrier of barrier inflation map;Step 3, the numerical optimization method based on probability safety driving corridor is used to optimize initial trajectory.The application takes four-wheel independent steering vehicle as carrier, so that vehicle can effectively generate a safe and feasible driving trajectory in the unstructured scene of dense barrier, narrow road and dynamic barrier.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of automatic driving, in particular to a trajectory planning method and system for a narrow unstructured scene containing dynamic obstacles. BACKGROUND

[0002] The narrow unstructured scene containing dynamic obstacles can be understood as an unstructured scene including dense obstacles, narrow roads and dynamic obstacles. For trajectory planning of such an unstructured scene, the existing technology mainly generates a driving trajectory by using methods such as search, sampling, curve interpolation, numerical optimization and machine learning, and ensures path safety by means of a collision detection algorithm. The current mainstream collision detection methods include the Axis-Aligned Bounding Box and the Oriented Bounding Box detection methods, but there are technical defects such as large consumption of computing resources, poor adaptability to narrow road scenes, etc., which are difficult to meet the real-time planning requirements in complex environments.

[0003] Existing researches focus on structured roads with clear signs and single environment, such as urban roads and highways, and lack effective solutions for unstructured scenes with irregular obstacle distribution and narrow road sections, such as parking lots and off-road sections. Traditional front-wheel drive steering configurations are difficult to cope with such complex environments due to limited maneuverability, while distributed drive steering configurations have stronger terrain adaptability, but their trajectory planning algorithms still face many technical bottlenecks: (1) The optimal control theory needs to deal with large-scale non-convex optimization problems when constructing obstacle avoidance constraints, and existing methods simplify the constraint conditions by constructing a driving corridor, but the fixed step construction method significantly reduces the computational efficiency; (2) The lack of dynamic obstacle interaction modeling leads to insufficient scene adaptability of the trajectory planning scheme; (3) The driving corridor reconstruction mechanism does not consider the iterative nature of trajectory optimization, which is prone to produce suboptimal or infeasible solutions.

[0004] There is a hierarchical trajectory planning framework in the existing technology: the upper layer generates an initial path by using an improved hybrid algorithm, and the lower layer realizes the dimension reduction processing of obstacle avoidance constraints through driving corridor construction, and introduces an iterative reconstruction mechanism to improve the solving efficiency. Although this technology reduces the dependence on the quality of the initial solution by using a lightweight iterative framework, it still has significant shortcomings: first, the obstacle avoidance constraint modeling does not consider the motion prediction of dynamic obstacles, which limits the actual scene application; second, the driving corridor construction process uses a single processing strategy, which is time-consuming and affects the overall optimization efficiency; third, the corridor construction in narrow road scenes lacks an adaptive adjustment mechanism, which is prone to trajectory feasibility problems.

[0005] Based on the above analysis, the prior art has the following common defects: ① the traditional collision detection method is difficult to balance the calculation efficiency and accuracy in narrow scenes; ② the driving corridor construction strategy lacks dynamic environment adaptability and calculation efficiency optimization; ③ the lack of dynamic obstacle interaction model leads to insufficient trajectory safety; and ④ the fixed parameter setting is difficult to adapt to the diversity needs of complex scenes.

[0006] Therefore, it is urgent to develop a technical scheme capable of realizing efficient and safe trajectory planning in an unstructured scene with dense obstacles, narrow roads and dynamic obstacles. SUMMARY

[0007] The purpose of the present application is to provide a complex unstructured scene-oriented vehicle trajectory planning method and system for complex narrow scenes with dynamic obstacles, which takes a four-wheel independent steering vehicle as a carrier, so that the vehicle can effectively generate a safe and feasible driving trajectory in an unstructured scene with dense obstacles, narrow roads and dynamic obstacles.

[0008] To achieve the above purpose, the present application provides a trajectory planning method for a narrow unstructured scene with dynamic obstacles, which comprises:

[0009] Step 1, analyze the kinematic model characteristics of the four-wheel independent steering vehicle, construct a hybrid algorithm, obtain the parameters related to the unstructured scene, and use the hybrid algorithm to search from the starting point to the target point, utilize the kinematic constraints and cost function of the vehicle to generate a series of path expansion nodes, and interpolate nodes on the expansion node path;

[0010] Step 2, according to the hybrid algorithm of step 1, combined with an improved collision detection algorithm, the initial collision-free path is solved; wherein the improved collision detection algorithm comprises:

[0011] According to the rectangular profile of the vehicle body, an equal division multi-disc profile and an obstacle inflation map based on the equal division multi-disc are constructed, the equal division multi-disc center coordinates are calculated, each disc center is traversed, and it is judged whether the disc center interferes with the obstacles of the obstacle inflation map: if not, it is determined that the vehicle does not collide with the obstacles at the node, and the corresponding planning node is a feasible node; if yes, it is determined that the corresponding planning node is an infeasible node;

[0012] Step 3, the initial trajectory of step 2 is optimized by using a numerical optimization method based on a probabilistic safe driving corridor: a static risk field-based adaptive expansion safe driving corridor method is used to quickly construct a required driving corridor, then the driving corridor is adjusted according to the collision probability of the vehicle and the dynamic obstacle, a target function and constraint condition considering trajectory smoothness, comfort, safety and efficiency are provided, an optimal control problem of trajectory optimization is constructed, and finally the optimized optimal trajectory is output after numerical solution.

[0013] Further, the method of "constructing an equal division of multiple disc profiles according to the rectangular profile of the vehicle body" in step 2 includes:

[0014] The rectangular vehicle body is approximated by multiple discs with the same radius that can cover the vehicle body, the area of the disc that extends beyond the vehicle body is a redundant area, and the number of discs is pre-set to reduce the area of the redundant area , the maximum number of discs is obtained by formula (9):

[0015] (9)

[0016] wherein, is the maximum number of discs required to meet the preset safety margin between the vehicle and the obstacle , which is represented by formula (6), is the maximum number of discs under the time complexity limit, which is obtained by formula (8):

[0017] (6)

[0018] (8)

[0019] wherein, , is the equal division length of the rectangular vehicle along the length and width, respectively , is the length and width of the rectangular vehicle, respectively is the time complexity of the collision detection algorithm is the number of expanded equal division discs.

[0020] Further, the "equal division disc center coordinates" of step 2 are calculated according to the center point of the vehicle, and the calculation method specifically includes:

[0021] The first node of step 1 is represented as the vehicle pose point , , , , is the coordinate of the vehicle center point in the global coordinate system and the vehicle heading angle, respectively, the first The coordinates of the center of each disk in global coordinates It is obtained by calculation using equation (10). , The total number of discs, including the number of discs in the longitudinal and transverse directions of the vehicle. and All ;

[0022] (10)

[0023] in, and These represent the equal division lengths of the vehicle in the longitudinal and lateral directions, respectively.

[0024] Furthermore, step 3, "rapidly constructing a driving corridor that meets the requirements based on the static risk field adaptive extended safe driving corridor method," specifically includes:

[0025] By adaptively adjusting the extension length, in Expansion length in the direction To reach the maximum value required by the static risk field, the expansion is performed sequentially in each direction, point... The driving corridor is at its maximum size. Described as follows (20):

[0026] (20)

[0027] In the formula, These are the coordinates of the nodes on the initial path. , This is the total number of initial path sampling points. and Representing points respectively Vertical and horizontal coordinates in the global coordinate system This indicates the upper, right, lower, and left edges of the driving corridor.

[0028] Furthermore, step 3, "the probability of a collision between the vehicle and a dynamic obstacle," specifically includes:

[0029] The car is Always in position Collision probability Represented as equation (18):

[0030] (18)

[0031] in, In time With the The probability of colliding with a dynamic obstacle is expressed as equation (17):

[0032] (17)

[0033] where, is the projected area of ego vehicle in two-dimensional plane , is the probability density function of the random position distribution of the first dynamic obstacle at time .

[0034] Further, the "objective function" of step 3 is expressed as equation (34):

[0035] (34)

[0036] where, is the objective function, is the total planning time, and denotes the driving comfort, denotes the smoothness of the trajectory, and are normalized weighting coefficients, , is the total number of set path sampling points.

[0037] Further, the "constraint conditions" of step 3 include obstacle avoidance constraints, vehicle kinematics constraints, and two-point boundary constraints, wherein the obstacle avoidance constraints specifically include:

[0038] (1) The positions of the centers of the front and rear discs in the discrete driving corridor constraint in the global coordinates are constrained as follows:

[0039] (27)

[0040] (2) The local rectangular constraint in the discrete driving corridor constraint is as follows:

[0041] (28)

[0042] where, , is the edge of the driving corridor that each disc center expands in accordance with safety requirements, , respectively, denote the upper, lower, left, and right of the driving corridor.

[0043] Further, the vehicle kinematics constraints specifically include:

[0044] (1) The nonlinear kinematics equation constraint in the discrete kinematics constraint is as follows, equation and ​respectively represent the Ackerman steering mode and the crab mode kinematics constraints;

[0045] (29)

[0046] (30)

[0047] The state variable and control variable value range constraints in the discrete kinematics constraints are as follows:

[0048] (31)

[0049] wherein, , , , respectively represent the velocity, acceleration, steering angle and steering angular velocity, and the subscript , respectively represent the minimum value and the maximum value, is the wheelbase of the vehicle.

[0050] Further, the two-point boundary constraints specifically include:

[0051] (1) The starting point constraint in the discrete two-point boundary constraints is as follows:

[0052] (32)

[0053] (2) The end point constraint in the discrete two-point boundary constraints is as follows:

[0054] (33)

[0055] wherein, the subscript , respectively represent the starting point and the end point.

[0056] The application also provides a trajectory planning system for a narrow unstructured scene containing dynamic obstacles, which comprises:

[0057] a path node expansion unit configured to analyze the kinematics model characteristics of the four-wheel independent steering vehicle, construct a hybrid algorithm, acquire the unstructured scene related parameters, search from the starting point to the target point by using the hybrid algorithm, generate a series of path expansion nodes by using the kinematics constraints and the cost function of the vehicle, and interpolate nodes on the expansion node path;

[0058] An initial path generating unit is configured to solve an initial path without collision according to a hybrid A* algorithm of the path node expanding unit in combination with an improved collision detection algorithm, wherein the improved collision detection algorithm comprises:

[0059] According to the rectangular profile of the vehicle body, an equally divided multi-disc profile and an obstacle inflation map based on the equally divided multi-disc are constructed, the equally divided multi-disc center coordinates are calculated, and whether the disc center interferes with the obstacle of the obstacle inflation map is judged by traversing each disc center: if not, it is determined that the vehicle does not collide with the obstacle at the node, and the corresponding planning node is a feasible node; if yes, it is determined that the corresponding planning node is an infeasible node.

[0060] An optimal trajectory generating unit is configured to optimize the initial trajectory of the initial path generating unit by using a numerical optimization method based on a probabilistic safe driving corridor: a safe driving corridor meeting the requirements is quickly constructed based on a static risk field adaptive expansion safe driving corridor method, then the driving corridor is adjusted according to the collision probability of the vehicle and the dynamic obstacle, a target function and a constraint condition considering the trajectory smoothness, comfort, safety and efficiency are provided, an optimal control problem of trajectory optimization is constructed, and finally the optimized optimal trajectory is output after numerical solution.

[0061] The present application has the following advantages due to the above technical solutions:

[0062] The present application aims to take a four-wheel independent steering vehicle as an object, and provides a trajectory planning method and system for an unstructured scene with dynamic obstacles on a narrow road.

[0063] Compared with the method in the prior art, the present application adopts a probabilistic safe driving corridor method based on a risk field, first expands a safe driving corridor based on a static risk field, then quantifies the uncertainty of the dynamic obstacle movement by using a risk distribution, quickly adjusts the boundary of the driving corridor according to the risk value, so that the trajectory planner can output a trajectory that effectively avoids collision with static and dynamic obstacles. BRIEF DESCRIPTION OF DRAWINGS

[0064] Figure 1 is a kinematic model schematic diagram of a four-wheel independent steering vehicle in different modes of the embodiment of the present application, wherein (a) is an Ackerman steering mode, (b) is a diagonal moving mode, and (c) is a turning-in-place mode.

[0065] Figure 2 is a principle schematic diagram of a double-disc coverage obstacle avoidance method of the embodiment of the present application.

[0066] Figure 3is a redundant space diagram of an embodiment of the present invention, wherein: (a) the redundant space is sufficient for the vehicle to pass through, (b) the redundant space is insufficient for the vehicle to pass through.

[0067] Figure 4 is a diagram of an equal division of a multi-disk coverage of an embodiment of the present invention.

[0068] Figure 5 is a diagram of an unstructured scene obstacle inflation of an embodiment of the present invention, wherein: (a) a multi-disk covers the vehicle, (b) a multi-disk center point.

[0069] Figure 6 is a diagram of an equal division of a multi-disk coverage structure of an embodiment of the present invention.

[0070] Figure 7 is a diagram of a different step length safe driving corridor of an embodiment of the present invention, wherein: (a) (k-1)th expansion, (b) kth expansion, (c) (k+1)th expansion, (d) (k+2)th expansion.

[0071] Figure 8 is a diagram of a dynamic contraction of a driving corridor from (a)→(b)→(c) of an embodiment of the present invention.

[0072] FIG. 9 is a diagram of a trajectory in a static obstacle scene of an embodiment of the present invention.

[0073] Figure 10 is a diagram of a trajectory in a dynamic obstacle scene of an embodiment of the present invention. DETAILED DESCRIPTION

[0074] In the drawings, the same or similar notations used in different drawings represent the same or similar elements or elements having the same or similar functions. The embodiments of the present invention are described in detail below with reference to the accompanying drawings.

[0075] In the description of the present invention, the terms “center”, “longitudinal”, “transverse”, “front”, “back”, “left”, “right”, “vertical”, “horizontal”, “top”, “bottom”, “inner”, “outer” and the like indicate the orientation or positional relationship based on the orientation or positional relationship shown in the drawings, and are only for the convenience of describing the present invention and simplifying the description, and do not indicate or imply that the device or element referred to must have a particular orientation, be constructed and operated in a particular orientation, and therefore cannot be understood as limiting the scope of protection of the present invention.

[0076] The upper layer of the embodiment of the present invention utilizes improved hybrid The algorithm solves an initial path, wherein a collision detection method of the algorithm is provided again, thereby solving the problem that the traditional method has no solution in the face of a narrow road.

[0077] The trajectory planning method for a narrow unstructured scene containing a dynamic obstacle provided by the embodiment of the present application comprises:

[0078] Step 1, analyzing the kinematic model characteristics of a four-wheel independent steering vehicle, constructing a hybrid algorithm, acquiring unstructured scene related parameters, and using the hybrid algorithm to search from a starting point to a target point, using the kinematic constraints of the vehicle and a cost function to generate a series of path expansion nodes, and interpolating nodes on the path of the expansion nodes.

[0079] Step 2, according to the hybrid algorithm of step 1, an initial path without collision is solved in combination with an improved collision detection algorithm; wherein the improved collision detection algorithm comprises:

[0080] According to the rectangular profile of the vehicle body, an equal-division multi-disc profile and an obstacle inflation map based on the equal-division multi-disc are constructed, the equal-division multi-disc center coordinates are calculated, each disc center is traversed, and it is judged whether the disc center interferes with the obstacle of the obstacle inflation map: if not, it is determined that the vehicle does not collide with the obstacle at the node, and the corresponding planning node is a feasible node; if yes, it is determined that the corresponding planning node is an infeasible node, thereby solving the problem that the path cannot be solved in the narrow road scene.

[0081] Step 3, a numerical optimization method based on a probabilistic safe driving corridor is used to optimize the initial trajectory of step 2: a safe driving corridor meeting the requirements is quickly constructed based on a static risk field adaptive expansion safe driving corridor method, then the driving corridor is adjusted according to the collision probability of the vehicle and the dynamic obstacle, a target function and constraint condition considering the smoothness, comfort, safety and efficiency of the trajectory are provided, an optimal control problem of trajectory optimization is constructed, and finally the optimal trajectory after optimization is output.

[0082] The specific implementation process of the vehicle trajectory planning method for a complex unstructured scene containing a dynamic obstacle provided by the embodiment of the present application will be described in detail below in combination with the pseudo code of algorithm 1:

[0083]

[0084] In one embodiment, the hybrid The construction method of the algorithm specifically comprises:

[0085] Vehicle kinematic model analysis is the core of trajectory planning, ensuring that the planned trajectory can meet the vehicle motion characteristics. Figure 1 Figure 1 The schematic is a global coordinate system XOY, and the vehicle geometric parameters include front suspension length , rear suspension length , wheelbase , total length , vehicle width , front wheel steering angle , rear wheel steering angle ; the wheelbase center point of the vehicle is , and the heading angle of the vehicle is , both of which constitute the pose of the vehicle .

[0086] The embodiment of the application considers the kinematic model of the four-wheel independent steering vehicle in three motion modes of Ackerman steering, wedge steering and in-place steering. Among them, the Ackerman steering mode is the basic motion mode of the four-wheel independent steering vehicle, the vehicle pose has both translation and rotation, which can provide high maneuverability and flexibility, especially in a small space, showing good steering performance, as shown in (a) of Figure 1 The diagonal movement mode is a special motion mode of the four-wheel independent steering vehicle, and the vehicle pose only has translation without rotation, as shown in (b) of Figure 1 The in-place steering mode is another special motion mode, and the vehicle pose only has rotation without translation, as shown in (c) of Figure 1 .

[0087] In addition, the hybrid algorithm needs to provide node expansion methods and cost functions, which can be obtained from existing literature.

[0088] In one embodiment, the method for obtaining the parameters related to the unstructured scene specifically comprises:

[0089] After analyzing the kinematic model of the four-wheel independent steering vehicle, the embodiment of the application needs to obtain the following geometric and kinematic parameters to ensure that the trajectory planning can accurately reflect the actual motion ability of the vehicle, and obtain the geometric parameters of the vehicle and the kinematic parameters such as minimum speed , maximum speed , minimum acceleration , maximum acceleration , maximum steering angle , maximum steering angular velocity . At the same time, the starting point pose , the terminal point pose ​and unstructured scene obstacle map. Subsequently, these acquired unstructured scene related parameters are input into a subsequent trajectory planner.

[0090] In order to ensure that the vehicle avoids collision with obstacles throughout the trajectory planning process, its trajectory must satisfy the obstacle avoidance constraint. Traditional collision detection methods usually need to traverse the vehicle contour, and convert the "whether the vehicle collides with the obstacle" problem into the "whether the vehicle contour interferes with the obstacle region" problem. However, such methods are relatively cumbersome to calculate. In order to solve these problems, the embodiment of the present application uses the equal division multi-disc method to approximate the rectangular vehicle contour as a plurality of discs, and converts the "polygon and polygon interference" problem into the "point and polygon interference" problem, so as to realize simpler and more efficient collision detection.

[0091] As shown in Figure 2 , the contour of the rectangular vehicle is approximated by a double-disc model, in which: the rectangular body is covered by two discs with the same radius, aiming to simplify the collision detection process. However, this covering method can reflect the shape of the vehicle to some extent, but since the total area of the disc is larger than the area of the actual rectangular body, it results in an extra redundant area (as the gray area in Figure 2 ). This part of the redundant space does not correspond to the actual occupied area of the vehicle, so the area that can originally pass is misjudged as impassable when trajectory planning is performed.

[0092] As shown in Figure 3 , the redundant space will cause the trajectory planning algorithm to fail to find a feasible solution when searching for a feasible path in narrow roads or obstacle-dense areas. Therefore, how to reduce the size of the redundant space becomes a key problem to improve the efficiency and accuracy of trajectory planning.

[0093] As shown in Figure 4 , in order to facilitate modeling, the embodiment of the present application equally divides the rectangular vehicle in the longitudinal and transverse directions, i.e.:

[0094] (1)

[0095] wherein, and respectively represent the number of discs in the longitudinal and transverse directions of the vehicle, represents the number of equal division multi-discs used to cover the vehicle, and are the length and width of the vehicle, respectively, and respectively represent the equal division length in the longitudinal and transverse directions of the vehicle.

[0096] The center of the circle is located at these equidistant points, and the disc can just evenly divide the rectangular vehicle, so the disc radius can be calculated as , expressed as formula (2) as follows:

[0097] (2)

[0098] According to the above formula and the geometric relationship analysis, the area of the redundant space is:

[0099] (3)

[0100] According to the above formula, by increasing the number of discs covering the vehicle , the actual contour area of the vehicle can be more accurately approximated, thereby reducing the redundant space. The equidivision multi-disc model is to more accurately depict the actual contour of the vehicle by optimizing the position and number of discs, avoid the influence of the effectiveness of trajectory planning due to excessive redundant space, and better adapt to the trajectory planning requirements in the environment of narrow roads. This improvement significantly improves the passability of the vehicle and the feasibility of the trajectory planning algorithm, providing a more reliable initial solution for the subsequent optimization process.

[0101] In addition, in the trajectory planning process, in addition to avoiding collision, the safety margin requirement must also be considered. That is, the vehicle maintains a sufficient safety distance from the obstacle during driving, and avoids the risk of collision due to control error and planning error. The present embodiment provides that the minimum distance between the vehicle and the obstacle is greater than a predetermined safety margin . In the present embodiment, since there is a redundant space, the extension length of each redundant space generated by the disc in the transverse and longitudinal directions is required to be greater than the safety margin, thereby ensuring the safety of the entire planning process, i.e.

[0102] (4)

[0103] wherein respectively represent the extension length of the disc in the transverse and longitudinal directions, and the calculation formula is as follows.

[0104] (5)

[0105] Combined with formulas (1), (2), (4) and (4), the maximum value of the number of discs required to meet the safety margin requirement can be obtained , as shown in formula (6):

[0106] (6)

[0107] However, increasing the number of discs can improve the approximation accuracy of the vehicle profile, but it will also significantly increase the solving time. As can be seen from the pseudo code of Algorithm 2, in the equal-division multi-disc method, the increase in the number of discs directly leads to an increase in the number of calculation loops, thereby increasing the time complexity. This means that, in the trajectory planning process, as the number of discs increases, the amount of calculation required for each iteration will also increase, thereby affecting the real-time performance and computational efficiency of the algorithm. Excessive solving time may not meet the real-time requirements of the system. Therefore, when increasing the number of discs, a trade-off must be made between accuracy and computational efficiency to ensure that the algorithm can provide sufficient obstacle avoidance capability while not causing excessive computational overhead.

[0108] To evaluate the time cost of the multi-disc collision detection method, the present embodiment adopts a progressive time complexity analysis method, in which the time complexity is expressed in Big O notation to describe the growth relationship between the execution time of the algorithm and the size of the data. In the equal-division multi-disc collision detection method adopted in the present embodiment, the algorithm flow includes a main loop and a nested secondary loop, so the overall time complexity is the combination of the two loop structures. According to the flow analysis of Algorithm 1, the time complexity can be expressed as formula (7):

[0109] (7)

[0110] wherein, is the number of extended equal-division multi-discs, is the number of nodes.

[0111] This time complexity indicates that as the number of discs increases, the amount of calculation required for collision detection will increase significantly. Therefore, it is necessary to limit the number of discs to avoid excessive calculation, which will affect the real-time performance and overall performance of the algorithm, i.e. formula (8):

[0112] (8)

[0113] wherein, is the maximum number of discs under the time complexity limit, in the present embodiment, ;

[0114] The maximum number of discs is expressed as formula (9):

[0115] (9)

[0116] In view of this, in one embodiment, the method of "constructing an equal-division multi-disc profile according to the rectangular profile of the vehicle body" in step 2 includes:

[0117] ​The rectangular vehicle body is approximated by a plurality of discs with the same radius covering the vehicle body, the area of the disc extending beyond the vehicle body is a redundant area, and the number of discs is preset The area of the redundant area is reduced The maximum number of discs is obtained by formula (9).

[0118] When the disc is used to approximate the rectangular vehicle body, it is necessary to determine whether the disc collides with the obstacle during the traversal process, and the collision problem is still a "polygon-polygon interference problem", and it is usually complex to solve such problems. Therefore, in the embodiment of the present application, each disc is contracted towards the center of the disc, as shown in (a) of Figure 5 ; at the same time, the boundary of the polygon obstacle in the environment is externally inflated according to the disc radius, as shown in (b) of Figure 5 , and the dark gray represents the obstacle itself, and the light gray is the inflated boundary. In this way, the original obstacle avoidance constraint "the vehicle body does not collide with the obstacle" is simplified to "the center of the disc does not interfere with the inflated obstacle area", which ensures the safety distance between the vehicle and the obstacle and improves the calculation efficiency and the safety of the path.

[0119] In step 1, the upper planning of the present application is based on the improved hybrid algorithm, and the node expansion is performed through the center point of the vehicle. Specifically, the hybrid algorithm is used to search from the starting point to the target point, and a series of path expansion nodes are generated by using the kinematic constraints and cost functions of the vehicle. The present embodiment interpolates nodes on the expanded sub-node path to more accurately describe the moving track of the vehicle on the path, and then performs collision detection on the nodes.

[0120] The steps of collision detection will be described below, and the pseudo code is shown in algorithm 2

[0121] .

[0122] The embodiment of the present application converts the collision problem into "the center of the disc does not interfere with the inflated obstacle area", and first calculates the coordinates of the divided disc center according to the center point of the vehicle. In an embodiment, the "divided disc center coordinates" of step 2 are calculated according to the center point of the vehicle, and the calculation method specifically includes:

[0123] The first node of step 1 is represented as the vehicle pose point , , , , respectively, the coordinates of the vehicle center point in the global coordinates and the vehicle heading angle, and the first coordinates of the center of the disc in the global coordinate system is calculated by equation (10), , is the total number of discs, the number of discs in the longitudinal and lateral directions of the vehicle and are respectively ;

[0124] (10)

[0125] wherein, and respectively represent the equal division length of the vehicle in the longitudinal and lateral directions.

[0126] For example, let (where , ), that is, four equal division discs are used to approximate the rectangular vehicle, as shown in FIG. 1, in the global coordinate system, the coordinates of the center point of the vehicle Figure 6 and the heading angle of the vehicle are known, then the coordinates of the center of the four equal division discs are respectively , , , , , and the specific calculation is described as equation (10):

[0127]

[0128] wherein, and are respectively the equal division length of the four equal division discs in the longitudinal and lateral directions, is the corresponding interpolation point on the extended path.

[0129] Then, by traversing the coordinates of the center of each disc, it is judged whether the center of the disc intersects with the obstacle. If the center of the disc does not intersect with the obstacle, it can be determined that the vehicle does not collide with the obstacle at the node, and the corresponding planning node is a feasible node; otherwise, if the center of the disc is located inside the obstacle, it indicates that a collision occurs, and the node is determined to be infeasible.

[0130] The trajectory optimization problem of a four-wheel independent steering vehicle involves three main constraints: obstacle avoidance constraint, vehicle kinematic constraint and two-point boundary constraint. Meanwhile, the optimization objective function comprehensively considers the smoothness, comfort and efficiency of the trajectory. Specifically, the obstacle avoidance constraint ensures that the trajectory is always located in a safe and feasible driving area. The obstacle avoidance constraint provided by the present application not only considers static obstacles, but also considers the uncertainty of dynamic obstacles, and quantifies the motion of dynamic obstacles by using a risk field. The vehicle kinematic constraint ensures that the vehicle follows the actual physical and dynamic characteristics during steering. In addition, since the present application takes a four-wheel independent steering vehicle as the research object, different motion modes have different kinematic equations, so the trajectories under different modes are optimized separately. Considering that the trajectory of the vehicle under the spot steering mode is a point, there is no need to establish the kinematic constraint of this mode. The two-point boundary constraint specifies the starting and ending positions of the trajectory and makes requirements on the related poses of the trajectory. One of the innovations of the present application is the construction of the obstacle avoidance constraint, especially the method for constructing the probabilistic safe driving corridor for the uncertainty of the motion of dynamic obstacles.

[0131] The primary task of trajectory planning is to ensure safety, and one of the methods is to construct an obstacle avoidance constraint. However, in unstructured scenarios, static obstacles with complex shapes are randomly distributed, and such obstacle avoidance constraints usually have a large scale and are not differentiable, which makes it difficult for optimal control problem solvers to solve. In order to simplify these constraints and reduce the complexity of solving, the embodiment of the present application represents the safe area that meets the collision requirement by constructing a driving corridor, and converts the complex obstacle into a geometric area that is easy to handle, thereby improving the efficiency and solving accuracy of trajectory optimization.

[0132] Existing researches mostly construct a driving corridor constraint based on the outline of static obstacles. However, in actual unstructured scenarios, there are dynamic obstacles such as other vehicles and pedestrians, so it is not enough to simply consider static obstacles when constructing a driving corridor. At this time, considering the uncertainty of the motion of dynamic obstacles, it is not very appropriate to construct a driving corridor with a determined obstacle outline. The uncertainty of the motion of dynamic obstacles can be considered in the following aspects. On the one hand, vehicles collect multi-source information through multiple sensors to determine the position of dynamic obstacles. However, due to sensor measurement errors, external environmental changes or inherent uncertainties of perception algorithms, the positioning result of dynamic obstacles will not be accurate. On the other hand, the continuous motion of dynamic obstacles causes the obstacle outline to change constantly, thereby affecting the shape and size of the driving corridor. Therefore, during the construction of the driving corridor, the information needs to be updated in real time to ensure the accuracy of the corridor size and the safety of trajectory planning.

[0133] To address the aforementioned issues, this invention proposes a method for constructing a probabilistically safe driving corridor. The construction process considers the uncertainty of obstacle positions, using collision probability risk to describe collision scenarios. Specifically, dynamic obstacles in the scene are temporarily disregarded, focusing instead on static obstacles. This invention provides a method for adaptively expanding a safe driving corridor based on a static risk field. Through an adaptive step-size mechanism, the expansion step size of the driving corridor can be dynamically adjusted. Furthermore, a static risk field is used to quantitatively analyze collisions between the vehicle and static obstacles, strictly ensuring that the static risk value at any point within the driving corridor is below a threshold. This method can quickly obtain the maximum safe driving corridor while meeting safety requirements for the vehicle's current position. Building upon this, embodiments of this invention further consider the motion of dynamic obstacles, quantifying the uncertainty of dynamic obstacle motion using risk distribution. Based on the maximum safe driving corridor obtained above, the driving corridor is rapidly contracted according to the risk value, ensuring that any point within the driving corridor is below a set risk value, resulting in a probabilistically safe driving corridor. Finally, obstacle avoidance constraints are simplified to simple linear constraints.

[0134] This invention considers the dynamic obstacle motion and the uncertainty of the vehicle's perception module to model dynamic obstacles. This invention reconstructs the driving corridor by predicting the risk field of the obstacle trajectory, making its description of obstacle avoidance constraints more accurate.

[0135] There are in the environment The nth dynamic obstacle, assuming the nth... A dynamic obstacle at any time The state of motion is :

[0136] (11)

[0137] in and These represent the predicted lateral and longitudinal positions of the dynamic obstacle in Cartesian coordinates, respectively. , , , This represents the lateral velocity, longitudinal velocity, lateral acceleration, and longitudinal acceleration of the dynamic obstacle. During lateral and longitudinal motion, acceleration can be considered constant over a single time step.

[0138] So, dynamic obstacles The next moment The location information can be described by equation (12):

[0139] (12)

[0140] In the formula, For time step.

[0141] As shown in the above formulas, the uncertainty in the lateral and longitudinal positions of a dynamic obstacle is determined by the uncertainty in its initial position, acceleration, and velocity. Furthermore, the lateral and longitudinal motions of the dynamic obstacle are decoupled. , They are independent of each other and follow different Gaussian distributions. and Depend on , and discrete time interval Confirmed. Therefore, and They are also independent of each other and follow different Gaussian distributions. The random variable distributions of the dynamic obstacles are shown in Table 1 below.

[0142] Table 1

[0143]

[0144] In the table, The standard deviation of the initial longitudinal position distribution. The standard deviation of the initial lateral positional distribution. The standard deviation of the initial longitudinal velocity distribution. The standard deviation of the initial lateral velocity distribution. To predict the standard deviation of the longitudinal acceleration distribution, To predict the standard deviation of the lateral acceleration distribution.

[0145] In summary, dynamic obstacles The positional distribution in the horizontal and vertical directions can be represented as: , As shown in equation (13):

[0146] (13)

[0147] A linear combination of independent Gaussian distributions still follows a Gaussian distribution; therefore, the positional distribution of each dynamic obstacle can be considered to have covariance. .in, and The variances in the horizontal and vertical directions, respectively, can be described as shown in equation (14):

[0148] (14)

[0149] In summary, in a parking environment at any time Time The probability density function of the random position distribution of a dynamic obstacle It can be represented as:

[0150] (15)

[0151] in, and These are intermediate parameters and have no substantial physical meaning. , This sets the number of path sampling points.

[0152] The location distribution of other dynamic obstacles can be obtained in the same way. Therefore, based on the trajectory distribution of each dynamic obstacle obtained in turn, the risk level of the vehicle's position is assessed.

[0153] This invention uses collision risk to describe collisions between vehicles and dynamic obstacles, primarily based on the formula... Calculate the positional distribution of dynamic obstacles, and then assess the collision probability between the vehicle and each dynamic obstacle at any given time. With the The probability of colliding with a dynamic obstacle is expressed as: :

[0154] (16)

[0155] in, It is a vehicle in a two-dimensional plane The projection on At any moment Time The probability density function of the random location distribution of a dynamic obstacle. Is the car at the moment The position. Considering that the probability density function of the random position distribution of dynamic obstacles in the area around the vehicle changes little and can be approximated as a constant, the above equation (16) can be simplified to equation (17) in this embodiment of the invention to improve computational efficiency:

[0156] (17)

[0157] In the formula, In time With the The probability of colliding with a dynamic obstacle. For the vehicle in a two-dimensional plane The projected area on At any moment Time The probability density function of the random location distribution of a dynamic obstacle.

[0158] The time of the vehicle is calculated using the method described above. After considering the probability of collision with all obstacles, the vehicle in Time at point Collision probability is expressed as equation (18):

[0159] (18)

[0160] Most of the researches construct the driving corridor by using fixed step length, which is inefficient. Therefore, in order to improve the construction efficiency of the driving corridor, the adaptive step length method is adopted in the embodiment of the present application. This method can dynamically adjust the step length according to the distance between the vehicle and the obstacle. In addition, the driving corridor adopted by the traditional trajectory optimization is mostly directly constructed with the maximum size, which may cause the final optimized path point to be too close to the obstacle, and if the accuracy of the subsequent tracking control is not enough, it will increase the risk of collision between the vehicle and the obstacle. Therefore, the present application provides a method for adaptively expanding the safe driving corridor based on the static risk field, and the main idea is to use the static risk field in the driving risk field to describe the collision between the vehicle and the static obstacle, to calculate the risk field of the nearest distance between the driving corridor boundary and the static obstacle, and to require the risk field to be lower than a given threshold , so as to ensure that the optimized path point always maintains a safe distance from the obstacle, wherein the calculation formula of the static risk field can be expressed by equation (19):

[0161] (19)

[0162] In the formula, is the field strength coefficient, is the position of the ego vehicle, is the position of the static obstacle, is a high-order constant coefficient.

[0163] The specific expansion steps are as follows, as shown in Figure 7 , in Figure 7 (a) the k-1th expansion, the expansion step length of the upper edge of the driving corridor is 2s, which directly collides with the obstacle, and the expansion is invalid, the expansion step length of the right edge is 2s, which meets the safety requirement, and a feasible expansion area is constructed, still having a larger feasible space. In Figure 7 (b) the kth expansion, the expansion step length of the upper edge is reduced to s, but it does not meet the static risk field requirement, the expansion is invalid, and the expansion step length of the right edge is increased to 4s, which collides, and the expansion is invalid. In Figure 7 (c) the (k+1)th expansion, the expansion step length of the upper edge is reduced to s / 2, which meets the safety requirement, and the expansion step length of the right edge is 2s, which does not meet the static risk field requirement, and the expansion is invalid. In Figure 7 (b) the k+2th expansion, the expansion step length of the right edge is reduced to s, which meets the safety requirement.

[0164] Each direction of the vehicle ( The initial step size (representing the up, right, down, and left directions respectively) Set as A random value between [variable values]. Assume it is already in the direction [of the direction]. Upward expansion Second-rate.

[0165] This invention requires analyzing the expansion at each time step to dynamically adjust the step size for each expansion. If the... The extended carriageway boundary satisfies the requirements of the static risk field, indicating that there is still considerable feasible space in this direction. Therefore, the embodiments of the present invention can appropriately increase the number of... The step size, for example... To improve the efficiency of expansion in that direction; conversely, if the first The fact that this extension does not meet the requirements of a static risk field does not mean that in... The expansion in the direction is complete, but it indicates that the current step size is no longer suitable. In this embodiment of the invention, it is necessary to appropriately reduce the step size and continue the expansion. This embodiment of the invention may consider... Next, set the step size. And record the number of reductions. If The number of reductions in the direction has reached the set maximum number of reductions. If the extension length is not adjusted, the expansion process in that direction is stopped. In this way, by adaptively adjusting the extension length, in... Length of expansion in the direction To reach the maximum value required by the static risk field, the system expands in each direction sequentially. Preferably, step 3, "rapidly constructing a compliant driving corridor based on the adaptive expansion method of the static risk field," specifically includes:

[0166] By adaptively adjusting the extension length, in Expansion length in the direction To reach the maximum value required by the static risk field, the expansion is performed sequentially in each direction, point... The driving corridor is at its maximum size. Described as follows (20):

[0167] (20)

[0168] In the formula, These are the coordinates of the nodes on the initial path. , This is the total number of initial path sampling points. and Representing points respectively Vertical and horizontal coordinates in the global coordinate system represent the edges in the right, down and left directions on the driving corridor, respectively, corresponding to .

[0169] The adaptive step safety driving corridor method can quickly build a safe maximum driving corridor, but in this stage, the embodiment of the present application only considers static obstacles. Therefore, in order to further ensure safety, the embodiment of the present application considers the influence of dynamic obstacles to optimize and adjust the driving corridor, and performs local adjustment on the driving corridor . The probability of collision between the vehicle and the dynamic obstacle presents a typical two-dimensional Gaussian distribution feature, which gradually decreases from the mean center. In order to ensure the safety of the vehicle driving in the driving corridor, the embodiment of the present application requires that the probability of collision of each point at the boundary of the driving corridor with the dynamic obstacle is less than a set safety threshold .

[0170] Specifically, a plurality of points are sampled on the edge of , and the collision probability of the sampling points is calculated. Only when the collision probability of each point is less than the preset threshold, the region is considered to be safe; otherwise, the driving corridor in this direction must be contracted, and each time the driving corridor is reduced by a step, until the collision probability of all points is less than the safety threshold. Similarly, in order to improve the calculation efficiency of the contracted driving corridor, the embodiment of the present application contracts in the longitudinal and transverse directions in turn, wherein the transverse direction includes the edges and , and the longitudinal direction includes the edges and . This approach can reduce the interpolation calculation amount by about 2 times, thereby improving the overall calculation efficiency, as shown in (a) and (b) of FIG. 8. Please note that if the dynamic obstacle is contained in

[0171] before contraction, the driving corridor needs to be adjusted to a position that does not overlap with the dynamic obstacle first, as shown in (c) of FIG. 8. After the above method, a safe driving corridor is obtained, and its edge is represented as

[0172] , . Based on the construction of the probability safety driving corridor of each point, the obstacle avoidance constraint of the vehicle can be written as a simple linear constraint (21):

[0173] (21)

[0174] wherein, and , ​(x, y) represents the horizontal and vertical coordinates of the center of the entire vehicle.

[0175] In one embodiment, the vehicle kinematic constraints are set as follows:

[0176] In order to consider the actual motion ability of the vehicle and ensure that the planned trajectory can be effectively tracked by the vehicle, the trajectory optimization must follow strict kinematic constraints. Since the four-wheel independent steering vehicle has three motion modes, kinematic constraints need to be established respectively and defined as follows:

[0177] (22)

[0178] (23)

[0179] Equation are the Ackerman steering mode and the crabbing mode kinematic constraints respectively, the trajectory of the vehicle in the spin mode is a point, so there is no need to optimize and establish kinematic constraints. In the equations, and represent the horizontal and vertical coordinates of the center of the vehicle wheelbase; represents the yaw angle of the vehicle, that is, the included angle between the positive direction of the longitudinal axis of the vehicle body and the positive direction of the axis; and represent the speed and acceleration of the vehicle respectively; and represent the steering angle and angular velocity of the wheel respectively. And define the state variable and the control variable .

[0180] At the same time, the state variable and the control variable of the vehicle will be limited by the mechanical characteristics and motion ability of the vehicle, and there is a range of allowed values, mainly including:

[0181] (24)

[0182] In the equations, the upper and lower limits of the values of the speed , the acceleration , the steering angle and the steering angular velocity are described.

[0183] In one embodiment, the two-point boundary constraints are set as follows:

[0184] In order to consider the connection between the optimized trajectory segment and other trajectory segments, it is necessary to ensure that the poses of the starting point and the ending point of the optimization are consistent, and there is a vehicle motion mode switching between the trajectory segments, so it is necessary to make the vehicle return to the initial state (the speed, acceleration, steering angle and steering angular velocity are all 0). Therefore, the two-point boundary constraints are defined as: ​

[0185]

[0186] (25)

[0187] wherein, represents the optimization start point represents the pose at the time point t0. represents the optimization end point represents the pose at the time point tf.

[0188] In one embodiment, the cost function is set as follows:

[0189] The cost function includes driving comfort and smoothness of the trajectory, which can be written as:

[0190] (26)

[0191] wherein, and are normalized weighting coefficients, and the specific values thereof are obtained through experiments, - represents the overall planning time, and represent driving comfort, and represents the smoothness of the trajectory.

[0192] Step S3-3, numerical solution of the trajectory optimization problem.

[0193] After the optimal control problem of trajectory optimization is constructed, the problem needs to be solved to obtain the optimal solution. In order to effectively solve the problem, the present application discretizes the continuous variables in the optimal control problem. By discretizing the continuous variables, the original optimal control problem is converted into a discretized nonlinear programming problem by the embodiment of the present application.

[0194] Firstly, the constraint conditions and the objective function corresponding to each sampling time point t i uniformly distributed in the time domain [t0, tf] are discretized and modified, and the results are as follows: 1) The front and rear disc center position constraints in the discretized driving corridor constraint are as follows:

[0195] 1) The front and rear disc center position constraints in the discretized driving corridor constraint are as follows:

[0196] (27)

[0197] 2) The local rectangular constraint in the discretized driving corridor constraint is as follows:

[0198] (28)

[0199] ​The nonlinear kinematic equation constraints in the discretized kinematic constraints are as follows:

[0200] (29)

[0201] (30)

[0202] wherein the formula and are the Ackerman steering mode and the diagonal movement mode kinematic constraints respectively.

[0203] The state variable and control variable value range constraints in the discretized kinematic constraints are as follows:

[0204] (31)

[0205] The start point constraints in the discretized two-point boundary value constraints are as follows:

[0206] (32)

[0207] The end point constraints in the discretized two-point boundary value constraints are as follows:

[0208] (33)

[0209] The discretized objective function is as shown in the following formula (34):

[0210] (34)

[0211] wherein, is the objective function, is the overall planning time, and represent the driving comfort, represent the smoothness of the trajectory, and are normalized weighting coefficients, , is the total number of set path sampling points.

[0212] Then, the discretized constraints and the objective function above are summarized to form a complete nonlinear programming problem to be solved as follows:

[0213]

[0214] Finally, the open source solver IPOPT based on the interior point method widely used in the field of automatic driving is used to solve the nonlinear programming problem formed above to obtain the numerical optimal solution of the trajectory, and finally the optimal trajectory In this embodiment, the final planning in the optimal trajectory of the unstructured scene containing only static obstacles is as shown in the figure Figure 9 The blue curve in the figure is the initial path searched by the improved hybrid algorithm in the upper layer, and the blue curve is the trajectory after numerical optimization in the lower layer.

[0215] As shown in Figure 10 , it is a scene containing two dynamic obstacles. Since the initial trajectory algorithm does not consider dynamic obstacles, the planned trajectory collides with dynamic obstacles. In the optimization stage, the probability safety driving corridor is adopted, and the final trajectory is the blue curve, which realizes the avoidance of dynamic obstacles.

[0216] The application also provides a trajectory planning system for a narrow unstructured scene containing dynamic obstacles, which comprises a path node expansion unit, an initial path generation unit and an optimal trajectory generation unit, wherein:

[0217] The path node expansion unit is used to analyze the kinematic model characteristics of the four-wheel independent steering vehicle, construct a hybrid algorithm, obtain the related parameters of the unstructured scene, and search from the starting point to the target point by using the hybrid algorithm, utilize the kinematic constraints and cost functions of the vehicle, and generate a series of path expansion nodes, and interpolate nodes on the expansion node path.

[0218] The initial path generation unit is used to solve the initial path without collision according to the hybrid algorithm of the path node expansion unit combined with the improved collision detection algorithm; wherein the improved collision detection algorithm comprises:

[0219] According to the rectangular profile of the vehicle body, an equal division multi-disc profile and an obstacle inflation map based on the equal division multi-disc are constructed, the equal division multi-disc center coordinates are calculated, each disc center is traversed, and it is judged whether the disc center interferes with the obstacles of the obstacle inflation map: if not, it is determined that the vehicle does not collide with the obstacles at the node, and the corresponding planning node is a feasible node; if yes, it is determined that the corresponding planning node is an infeasible node.

[0220] The optimal trajectory generation unit is used to optimize the initial trajectory of the initial path generation unit by using a numerical optimization method based on the probability safety driving corridor: a safety driving corridor meeting the requirements is quickly constructed based on the static risk field adaptive expansion safety driving corridor method, then the safety driving corridor is adjusted according to the collision probability of the vehicle and the dynamic obstacles, a target function and constraint condition considering the smoothness, comfort, safety and efficiency of the trajectory are provided, an optimal control problem of trajectory optimization is constructed, and finally the optimal trajectory after optimization is numerically solved and output.

[0221] Currently, the trajectory planning research in the unstructured scene with narrow road and dynamic obstacles is relatively less, especially in this environment, the traditional trajectory planning method is difficult to solve the feasible solution. Therefore, the present application takes a four-wheel independent steering vehicle as the research object, and proposes a complete trajectory planning algorithm, wherein the upper planning stage adopts a mixed A* algorithm based on equal division multi-disc collision detection, and the lower optimization adopts a probability safety driving corridor to construct an obstacle avoidance constraint. Compared with the traditional mixed A* algorithm, the present application provides an equal division multi-disc collision detection method, which approximates the vehicle contour by using the equal division multi-disc, and simplifies the collision by transforming the problem description of the collision detection, so as to ensure that the four-wheel independent steering vehicle can find a safe and feasible path in the face of narrow road scene. For the obstacle avoidance constraint of trajectory optimization, the present application provides a probability safety corridor method, which includes a safety driving corridor method based on static risk field adaptive expansion and a probability safety corridor construction method considering dynamic obstacles. The adaptive step is used to expand to improve the efficiency, the static risk field is introduced to limit the maximum size of the driving corridor considering the control accuracy of the optimized path, and the safety of the planning is further ensured. The uncertainty of the dynamic obstacle motion is also considered to ensure that the obstacle avoidance constraint is more in line with the actual environment, the risk field is used to quantify the uncertainty of the dynamic obstacle motion, the collision probability of the obstacle and the vehicle is calculated, and the driving corridor is dynamically contracted based on this to ensure that the trajectory can always meet the safety requirements in the changing environment. The present application realizes the generation of continuous, smooth, comfortable, safe and efficient trajectory for the four-wheel independent steering vehicle in the unstructured scene with narrow road and dynamic obstacles.

[0222] In the above embodiments, the upper layer of the present application adopts an improved mixed A* algorithm and provides a collision detection method again, or is replaced by other trajectory planning algorithms based on search or sampling; in the lower trajectory planning, if other types of obstacle avoidance constraints are used and the indicators of the objective function are adjusted, similar effects can be achieved. These variants all embody the core idea of the present application.

[0223] Finally, it should be pointed out that: the above embodiments are only used to illustrate the technical solutions of the present application, but not to limit it. Those skilled in the art should understand that the technical solutions described in the above embodiments can be modified, or some technical features can be replaced by equivalents; these modifications or replacements do not make the essence of the corresponding technical solutions deviate from the spirit and scope of the technical solutions of the embodiments of the present application.

Claims

1. A trajectory planning method for a narrow unstructured scene containing dynamic obstacles, characterized in that, Comprise: Step 1, analyze the kinematic model characteristics of four-wheel independent steering vehicle, build hybrid algorithm, get the parameters related to unstructured scene, use hybrid algorithm to search from start point to target point, use kinematic constraints and cost function of vehicle to generate a series of path expansion nodes, interpolate nodes on the path of expansion nodes; Step 2, mixing according to step 1 An algorithm, combined with an improved collision detection algorithm, solves the initial collision-free path; wherein the improved collision detection algorithm comprises: According to the rectangular profile of the vehicle body, the equidivision multi-disc profile is constructed and the obstacle inflation map based on the equidivision multi-disc is constructed, the equidivision multi-disc center coordinates are calculated, each disc center is traversed, and it is judged whether the disc center interferes with the obstacle of the obstacle inflation map: if not, it is determined that the vehicle does not collide with the obstacle at the node, and the corresponding planning node is a feasible node; if yes, it is determined that the corresponding planning node is an infeasible node; Step 3, the initial trajectory of step 2 is optimized by using a numerical optimization method based on a probabilistic safety driving corridor: a static risk field-based adaptive expansion safety driving corridor method is used to quickly construct a required driving corridor, then the driving corridor is adjusted according to the collision probability of the vehicle and the dynamic obstacle, a target function and constraint condition considering the smoothness, comfort, safety and efficiency of the trajectory are provided, an optimal control problem of trajectory optimization is constructed, and finally the optimized optimal trajectory is output by numerical solution, wherein the edge of the driving corridor is represented as , , the probabilistic safety driving corridor is constructed based on each point, and the obstacle avoidance constraint of the vehicle is written as a linear constraint (21): （21） wherein and , denotes the horizontal and vertical coordinates of the center of the entire vehicle's equilateral polygonal disc.

2. The dynamic obstacle containing narrow unstructured scene trajectory planning method of claim 1, wherein, The method of "constructing an equidivision multi-disc profile according to the rectangular profile of the vehicle body" of step 2 comprises: The rectangular vehicle body is approximated by a plurality of discs having the same radius capable of covering the vehicle body, the discs extending into a redundant area not covered by the vehicle body, and the number of discs is predetermined to reduce the area of the redundant area , the maximum number of discs is obtained by equation (9) （9） wherein, to meet a preset safety margin between the vehicle and the obstacle a maximum value of the number of required discs, represented by equation (6), a maximum number of discs under the time complexity constraint, obtained from equation (8): （6） （8） wherein, , are the lengths of the equal divisions of the length and width of the rectangular vehicle, respectively, , are the length and width of the rectangular vehicle, respectively, is the time complexity of the collision detection algorithm, is the extended number of equal division multi-discs.

3. The dynamic obstacle containing narrow unstructured scene trajectory planning method of claim 2, wherein, The "equidivision multi-disc center coordinates" of step 2 are calculated according to the center point of the vehicle, and the calculation method specifically comprises: The first node of step 1 represents a vehicle pose point , , , , The coordinates of the vehicle center point in the global coordinates and the vehicle heading angle, respectively, the first The coordinates of the center of the disc in the global coordinates are obtained by formula (10), , is the total number of discs, the number of discs in the longitudinal and lateral directions of the vehicle and are both ; （10） wherein, and respectively represent the bisecting length of the vehicle in the longitudinal, lateral direction.

4. The dynamic obstacle containing narrow unstructured scene trajectory planning method of claim 1, wherein, The "method for quickly constructing a driving corridor meeting the requirements based on adaptive expansion of a static risk field" of step 3 specifically comprises: With adaptive adjustment of the extension length, in the direction of expansion length reaches the maximum value that meets the static risk field requirement, and each direction is sequentially extended, and the driving corridor with the maximum size is obtained at point is described as formula (20): （20） wherein, are coordinates of nodes on the initial path, , is the total number of initial path sampling points, and denote the longitudinal and lateral coordinates of the point in the global coordinate system, denote the edges in the right, down, and left directions on the travel corridor.

5. The dynamic obstacle containing narrow unstructured scene trajectory planning method of claim 1, wherein, The "collision probability of the vehicle and the dynamic obstacle" of step 3 specifically comprises: The ego vehicle is in at a time in a location with a collision probability is expressed as equation (18): （18） wherein, is the probability of collision with the th dynamic obstacle at time is the probability of collision with the th dynamic obstacle at time （17） wherein, is the projected area of the ego vehicle in a two-dimensional plane , is the probability density function of the random position distribution of the first dynamic obstacle at time .

6. The dynamic obstacle containing narrow unstructured scene trajectory planning method of any one of claims 1-5, wherein, The "objective function" of step 3 is expressed as formula (34): （34） wherein, is a target function, is a total planning time, and represents driving comfort, represents smoothness of a trajectory, and is a normalized weighting coefficient, , is a total number of set path sampling points.

7. The dynamic obstacle containing narrow unstructured scene trajectory planning method of claim 6, wherein, The "constraint conditions" of step 3 include obstacle avoidance constraints, vehicle kinematics constraints and two-point boundary constraints, wherein the obstacle avoidance constraints specifically comprise: (1) The positions of the centers of the front and rear discs in the global coordinate system in the discrete driving lane constraint The constraints are as follows; （27） (2) The local rectangular constraint in the discrete driving corridor constraint is as follows: （28） wherein , is the edge of the safety-compliant traffic corridor extending from the center of each disc, , respectively, denote the upper, lower, left, and right of the traffic corridor.

8. The dynamic obstacle containing narrow unstructured scene trajectory planning method of claim 7, wherein, The vehicle kinematics constraints specifically comprise: (1) The nonlinear kinematic equation constraint in the discrete kinematic constraint is as follows, formula and are the Ackerman steering mode and the crabbing mode kinematic constraints, respectively; （29） （30） (2) The state quantity and control quantity value range constraint in the discrete kinematics constraint is as follows: （31） wherein, , , , denote velocity, acceleration, steering angle and steering angle velocity, respectively, and the subscript , denote minimum, maximum, respectively, is the wheelbase of the vehicle.

9. The dynamic obstacle containing narrow unstructured scene trajectory planning method of claim 8, wherein, The two-point boundary constraints specifically comprise: (1) The starting point constraint in the discrete two-point boundary constraint is as follows: （32） (2) The termination point constraint in the discrete two-point boundary constraint is as follows: （33） wherein the subscripts , denote the start point and the end point, respectively.

10. A trajectory planning system for a narrow unstructured scene containing dynamic obstacles, characterized in that, Comprise: a path node expansion unit for analyzing kinematic model properties of a four-wheel independent steering vehicle, constructing a hybrid algorithm, obtaining unstructured scene related parameters, searching from a start point to a target point using the kinematic constraints of the vehicle and a cost function, generating a series of path expansion nodes, and interpolating a number of nodes on the path of the expansion nodes ​ an initial path generating unit for generating an initial path according to the mixing of the path node expanding unit An algorithm is used to solve the initial path without collision in combination with an improved collision detection algorithm; wherein the improved collision detection algorithm comprises: According to the rectangular profile of the vehicle body, the equidivision multi-disc profile is constructed and the obstacle inflation map based on the equidivision multi-disc is constructed, the equidivision multi-disc center coordinates are calculated, each disc center is traversed, and it is judged whether the disc center interferes with the obstacle of the obstacle inflation map: if not, it is determined that the vehicle does not collide with the obstacle at the node, and the corresponding planning node is a feasible node; if yes, it is determined that the corresponding planning node is an infeasible node; An optimal trajectory generation unit is configured to optimize the initial trajectory of the initial path generation unit by using a numerical optimization method based on a probabilistic safety driving corridor, which includes the following steps: a safety driving corridor satisfying the requirements is quickly constructed by using a static risk field adaptive expansion safety driving corridor method, then the safety driving corridor is adjusted according to the collision probability of the vehicle and the dynamic obstacle, a target function and constraint condition considering the smoothness, comfort, safety and efficiency of the trajectory are provided, an optimal control problem of trajectory optimization is constructed, and finally the optimized optimal trajectory is output by numerical solution, wherein the edge of the safety driving corridor is represented as , , and the obstacle avoidance constraint of the vehicle is written as a linear constraint (21) based on the probabilistic safety driving corridor constructed at each point. （21） wherein and , denotes the horizontal and vertical coordinates of the center of the entire vehicle's equilateral polygonal disc.

Images