Track planning method and system for narrow unstructured scene containing dynamic obstacle

Abstract

The invention discloses a trajectory planning method and system for a narrow unstructured scene containing a dynamic obstacle, and the method comprises the steps: 1, analyzing the characteristics of a kinematic model of a four-wheel independent steering vehicle, constructing a mixed A * algorithm, and obtaining the related parameters of the unstructured scene; step 2, according to a mixed A * algorithm, combining with an improved collision detection algorithm to solve a collision-free initial path; the improved collision detection algorithm comprises the following steps: constructing an equal-division multi-disc contour and an obstacle expansion map according to a vehicle body contour, calculating the center coordinates of the equal-division multi-disc, and judging whether the center of each disc interferes with an obstacle on the obstacle expansion map or not by traversing the center of each disc; and step 3, optimizing the initial track by adopting a numerical optimization method based on a probabilistic safe driving corridor. According to the invention, the four-wheel independent steering vehicle is taken as a carrier, so that the vehicle can effectively generate a safe and feasible driving track in an unstructured scene with dense obstacles, narrow roads and dynamic obstacles.

Description

Technical Field

[0001] The present invention relates to the technical field of autonomous driving, and particularly to a trajectory planning method and system for a narrow unstructured scenario with dynamic obstacles. Background Art

[0002] A narrow unstructured scenario with dynamic obstacles can be understood as an unstructured scenario that includes dense obstacles, narrow roads, and dynamic obstacles. For the trajectory planning of such unstructured scenarios, the existing technologies mainly use methods such as search, sampling, curve interpolation, numerical optimization, and machine learning to generate driving trajectories, and use collision detection algorithms to ensure path safety. The current mainstream collision detection methods include the Axis-Aligned Bounding Box and the Oriented Bounding Box detection methods, but they have technical defects such as high consumption of computing resources and poor adaptability to narrow road scenarios, and it is difficult to meet the real-time planning requirements in complex environments.

[0003] Existing research mostly focuses on structured roads with clear signs and single environments, such as urban roads and highways, but there is a lack of effective solutions for unstructured scenarios with irregular obstacle distributions and narrow sections, such as parking lots and field sections. The traditional front-wheel drive steering configuration is difficult to cope with such complex environments due to limited mobility, while the distributed drive steering configuration has stronger terrain adaptability, but its trajectory planning algorithm still faces many technical bottlenecks: (1) When the optimal control theory constructs obstacle avoidance constraints, it needs to handle large-scale non-convex optimization problems. Existing methods simplify the constraint conditions by constructing a Driving Corridor, but the construction method with a fixed step size leads to a significant reduction in computational efficiency; (2) The lack of dynamic obstacle interaction modeling results in insufficient scenario adaptability of the trajectory planning scheme; (3) The driving corridor reconstruction mechanism does not consider the iterative characteristics of trajectory optimization, and is prone to generating suboptimal solutions or infeasible solutions.

[0004] There is a proposed hierarchical trajectory planning framework in the existing technology: the upper layer uses an improved hybrid A* algorithm to generate an initial path, and the lower layer realizes the dimensionality reduction processing of obstacle avoidance constraints through the construction of a driving corridor, and introduces an iterative reconstruction mechanism to improve the solution efficiency. Although this technology reduces the dependence on the quality of the initial solution through a lightweight iterative framework, there are still significant deficiencies: First, its obstacle avoidance constraint modeling does not consider the motion prediction of dynamic obstacles, resulting in limited application in actual scenarios; Second, the construction process of the driving corridor adopts a single processing strategy, which takes a long time and affects the overall optimization efficiency; Third, the corridor construction in narrow road scenarios lacks an adaptive adjustment mechanism, and is prone to trajectory feasibility problems.

[0005] Based on the above analysis, the existing technologies have the following common defects: ① It is difficult to balance the computational efficiency and accuracy of traditional collision detection methods in narrow scenarios; ② The driving corridor construction strategy lacks dynamic environment adaptability and computational efficiency optimization; ③ The lack of a dynamic obstacle interaction model leads to insufficient trajectory safety; ④ Fixed parameter settings are difficult to meet the diverse requirements of complex scenarios.

[0006] Therefore, there is an urgent need to develop a technical solution that can achieve efficient and safe trajectory planning in unstructured scenarios with dense obstacles, narrow roads, and dynamic obstacles. Summary of the Invention

[0007] The purpose of the present invention is to provide a vehicle trajectory planning method and system for complex narrow unstructured scenarios with dynamic obstacles, which uses a four-wheel independent steering vehicle as a carrier to enable the vehicle to effectively generate a safe and feasible driving trajectory in unstructured scenarios with dense obstacles, narrow roads, and dynamic obstacles.

[0008] To achieve the above purpose, the present invention provides a trajectory planning method for a narrow unstructured scenario with dynamic obstacles, which includes:

[0009] Step 1, analyze the kinematic model characteristics of a four-wheel independent steering vehicle, construct a hybrid A* algorithm, obtain relevant parameters of the unstructured scenario, search from the starting point to the target point using the hybrid A* algorithm, and generate a series of path expansion nodes by using the kinematic constraints and cost function of the vehicle, and interpolate N nodes on the expanded node path;

[0010] Step 2, according to the hybrid A* algorithm in Step 1, combined with an improved collision detection algorithm, solve for an initial collision-free path; wherein, the improved collision detection algorithm includes:

[0011] According to the rectangular contour of the vehicle body, construct an equally divided multi-disc contour and an obstacle inflation map based on the equally divided multi-disc, calculate the center coordinates of the equally divided multi-disc, and by traversing the center of each disc and determining whether the center of the disc interferes with the obstacles in the obstacle inflation map: if not, it is determined that the vehicle at this node does not collide with the obstacle, and the corresponding planned node is a feasible node; if so, it is determined that the corresponding planned node is an infeasible node;

[0012] Step 3, use a numerical optimization method based on a probability-safe driving corridor to optimize the initial trajectory in Step 2: quickly construct a driving corridor that meets the requirements based on the static risk field adaptive extended safe driving corridor method, and then adjust the driving corridor according to the collision probability between the vehicle and dynamic obstacles, provide an objective function and constraint conditions considering trajectory smoothness, comfort, safety, and efficiency, construct an optimal control problem for trajectory optimization, and finally numerically solve and output the optimized optimal trajectory.

[0013] Further, the method of "constructing an equally divided multi-disk contour according to the rectangular contour of the vehicle body" in step 2 includes:

[0014] Approximating the rectangular vehicle body by multiple disks with the same radius that can cover the vehicle body. The area where the disks extend beyond the uncovered vehicle body is the redundant area. The area ΔS of the redundant area is reduced by a preset number of disks n. n The maximum number n of disks max is obtained by Equation (9):

[0015]

[0016] where is the maximum value of the number of disks that meets the preset safety margin d between the vehicle and the obstacle, which is represented by Equation (6), safe is the maximum number of disks under the time complexity limit, which is obtained by Equation (8):

[0017]

[0018]

[0018] where are the equally divided lengths of the rectangular vehicle along the length and width respectively. L and B are the length and width of the rectangular vehicle respectively. O(n·N) is the time complexity of the collision detection algorithm, and n is the number of extended equally divided multi-disks.

[0019] Further, the "coordinates of the centers of the equally divided multi-disks" in step 2 are calculated based on the center point of the vehicle. The specific calculation method includes:

[0020] The i-th node in step 1 is represented as the vehicle pose point (x i ,y i ), are the coordinates of the vehicle center point in the global coordinate system and the vehicle heading angle respectively. The coordinates of the center of the j-th disk in the global coordinate system are calculated by Equation (10), j ∈ [1, n], n is the total number of disks, and the number of disks n 1 and n 2 in the longitudinal and transverse directions of the vehicle are both n = n 1 ·n 2 ;

[0021]

[0022] where and represent the equally divided lengths of the vehicle in the longitudinal and transverse directions respectively.

[0023] Furthermore, the "method for quickly constructing a driving corridor that meets requirements based on the static risk field adaptive expansion safety driving corridor method" in step 3 specifically includes:

[0024] Using adaptive adjustment to expand the length, the expansion length L in the m direction m reaches the maximum value that meets the requirements of the static risk field, and each direction is expanded in turn. At point a driving corridor C with the largest size is obtained max described by the following formula (20):

[0025]

[0026] where (x, y) are the coordinates of the nodes on the initial path, k = 1, 2,..., N f , N f is the total number of sampling points on the initial path, and respectively represent the longitudinal and lateral coordinates of point in the global coordinate system, and E m represents the edges in the upper, right, lower, and left directions of the driving corridor.

[0027] Furthermore, the "collision probability between the vehicle and dynamic obstacles" in step 3 specifically includes:

[0028] The collision probability P of the ego vehicle at position at time t is t expressed as formula (18):

[0029]

[0030] where is the probability of colliding with the i 0 th dynamic obstacle at time t, expressed as formula (17):

[0031]

[0032] where S D is the projected area of the ego vehicle on the two-dimensional plane x - y, is the probability density function of the random position distribution of the i 0 th dynamic obstacle at time t.

[0033] Furthermore, the "objective function" in step 3 is expressed as formula (34):

[0034]

[0035] where is the objective function, T is the overall planning time, a 2 (τ) and ω2 Comfort(τ) represents driving comfort, φ 2 Smoothness(τ) represents the smoothness of the trajectory, w 1 and w 2 are normalized weighting coefficients, k = 1, 2,..., N f , N f is the total number of set path sampling points.

[0036] Furthermore, the "constraint conditions" in step 3 include obstacle avoidance constraints, vehicle kinematic constraints, and two-point boundary value constraints. Among them, the obstacle avoidance constraints specifically include:

[0037] (1) The positions (x j , y j ) of the centers of the front and rear disks in the discrete driving corridor constraint in the global coordinates are constrained as follows;

[0038]

[0039] (2) The local rectangle constraint in the discrete driving corridor constraint is as follows;

[0040]

[0041] Among them, is the edge of the driving corridor that meets the safety requirements extended from the center of each disk. m = 1, 2, 3, 4 represent the upper, lower, left, and right of the driving corridor respectively.

[0042] Furthermore, the vehicle kinematic constraints specifically include:

[0043] (1) The non-linear kinematic equation constraint in the discrete kinematic constraint is as follows. Equations (29) and (30) are the kinematic constraints of the Ackermann steering mode and the diagonal movement mode respectively;

[0044]

[0045] (2) The value range constraints of the state variables and control variables in the discrete kinematic constraint are as follows;

[0046]

[0047] Among them, v, a, δ, ω represent speed, acceleration, steering angle, and steering angular velocity respectively. The subscripts min and max represent the minimum value and the maximum value respectively. t step is the equal time step, l m is the wheelbase of the vehicle.

[0048] Furthermore, the two-point boundary value constraints specifically include:

[0049] (1) The starting point constraint in the two-point boundary value constraint after discretization is as follows;

[0050]

[0051] (2) The ending point constraint in the two-point boundary value constraint after discretization is as follows;

[0052]

[0053] Among them, the subscripts s and f represent the starting point and the ending point respectively.

[0054] The present invention also provides a trajectory planning system for a narrow unstructured scenario with dynamic obstacles, which includes:

[0055] A path node expansion unit, which is used to analyze the kinematic model characteristics of a four-wheel independently steered vehicle, construct a hybrid A* algorithm, obtain relevant parameters of the unstructured scenario, search from the starting point to the target point using the hybrid A* algorithm, generate a series of path expansion nodes by using the kinematic constraints and cost function of the vehicle, and interpolate N nodes on the expanded node path;

[0056] An initial path generation unit, which is used to solve a collision-free initial path according to the hybrid A* algorithm of the path node expansion unit and in combination with an improved collision detection algorithm; among them, the improved collision detection algorithm includes:

[0057] According to the rectangular contour of the vehicle body, construct an equally divided multi-disk contour and an obstacle inflation map based on the equally divided multi-disk, calculate the center coordinates of the equally divided multi-disk, and by traversing the center of each disk and determining whether the center of the disk interferes with the obstacles in the obstacle inflation map: if not, it is determined that the vehicle at this node does not collide with the obstacle, and the corresponding planned node is a feasible node; if so, it is determined that the corresponding planned node is an infeasible node;

[0058] An optimal trajectory generation unit, which is used to optimize the initial trajectory of the initial path generation unit by using a numerical optimization method based on a probability-safe driving corridor: quickly construct a driving corridor that meets the requirements by using the static risk field adaptive extended safe driving corridor method, and then adjust the driving corridor according to the collision probability between the vehicle and the dynamic obstacle, provide an objective function and constraint conditions considering trajectory smoothness, comfort, safety, and efficiency, construct an optimal control problem for trajectory optimization, and finally numerically solve and output the optimized optimal trajectory.

[0059] Due to the above technical solutions adopted by the present invention, the following advantages are achieved:

[0060] The present invention aims at four-wheel independent steering vehicles, and provides a trajectory planning method and system for unstructured scenarios facing narrow roads with dynamic obstacles. This method combines an improved hybrid A* algorithm with a numerical optimization method, integrating upper-level path planning and lower-level trajectory optimization, and provides an efficient and safe trajectory planning scheme.

[0061] Compared with the method in Technology 1, the present invention adopts a probabilistic safety driving corridor method based on a risk field. First, the safety driving corridor is adaptively expanded based on the static risk field. Then, the uncertainty of the movement of dynamic obstacles is quantified by a risk distribution, and the boundaries of the driving corridor are quickly adjusted according to the risk values, enabling the trajectory planner to output a trajectory that effectively avoids collisions with static and dynamic obstacles. Brief Description of the Drawings

[0062] Figure 1 It is a schematic diagram of the kinematic model of a four-wheel independent steering vehicle in different modes according to an embodiment of the present invention, where: (a) Ackermann steering mode, (b) diagonal movement mode, (c) in-situ steering mode.

[0063] Figure 2 It is a schematic diagram of the principle of the double-disk coverage obstacle avoidance method according to an embodiment of the present invention.

[0064] Figure 3 It is a schematic diagram of the redundant space according to an embodiment of the present invention, where: (a) The redundant space is sufficient for the vehicle to pass through, (b) The redundant space is not sufficient for the vehicle to pass through.

[0065] Figure 4 It is a schematic diagram of the equal division multi-disk coverage according to an embodiment of the present invention.

[0066] Figure 5 It is a schematic diagram of the expansion of obstacles in an unstructured scenario according to an embodiment of the present invention, where: (a) Multiple disks cover the vehicle, (b) The center mass points of multiple disks.

[0067] Figure 6 It is a structural diagram of the equal division multi-disk coverage according to an embodiment of the present invention.

[0068] Figure 7 It is a schematic diagram of a safety driving corridor with different step lengths according to an embodiment of the present invention, where: (a) (k - 1)th expansion, (b) kth expansion, (c) (k + 1)th expansion, (d) (k + 2)th expansion.

[0069] Figure 8 It is a schematic diagram of the dynamic contraction of the driving corridor in sequence from (a) → (b) → (c) according to an embodiment of the present invention.

[0070] Figure 9 It is a schematic diagram of the trajectory in a static obstacle scenario according to an embodiment of the present invention.

[0071] Figure 10 It is a schematic diagram of the trajectory in the dynamic obstacle scenario of the embodiment of the present invention. Specific embodiments

[0072] In the drawings, the same or similar reference numerals are used to represent the same or similar elements or elements with the same or similar functions. The embodiments of the present invention will be described in detail below with reference to the drawings.

[0073] In the description of the present invention, the orientation or positional relationship indicated by the terms "center", "longitudinal", "transverse", "front", "rear", "left", "right", "vertical", "horizontal", "top", "bottom", "inner", "outer", etc. is based on the orientation or positional relationship shown in the drawings. It is only for the convenience of describing the present invention and simplifying the description, rather than indicating or implying that the device or element referred to must have a specific orientation, be constructed and operated in a specific orientation, and therefore cannot be understood as a limitation on the protection scope of the present invention.

[0074] The upper layer of the embodiment of the present invention uses an improved hybrid A* algorithm to solve the initial path, and a collision detection method of the algorithm is re-provided, thereby solving the problem that the traditional method has no solution in the face of narrow roads. In addition, the lower layer uses a numerical optimization method based on the driving corridor to optimize the initial trajectory. The present invention considers the uncertainty of the dynamic obstacle trajectory and re-provides a method for constructing the driving corridor, making the trajectory more in line with the actual scenario on the premise of meeting safety.

[0075] The trajectory planning method for a narrow unstructured scenario with dynamic obstacles provided by the embodiment of the present invention includes:

[0076] Step 1, analyze the kinematic model characteristics of a four-wheel independently steered vehicle, construct a hybrid A* algorithm, obtain relevant parameters of the unstructured scenario, search from the starting point to the target point using the hybrid A* algorithm, and generate a series of path expansion nodes using the kinematic constraints and cost function of the vehicle, and interpolate N nodes on the expanded node path.

[0077] Step 2, according to the hybrid A* algorithm in Step 1, combined with an improved collision detection algorithm, solve the collision-free initial path; wherein, the improved collision detection algorithm includes:

[0078] According to the rectangular contour of the vehicle body, construct an equally divided multi-disc contour and an obstacle inflation map based on the equally divided multi-disc, calculate the center coordinates of the equally divided multi-disc, and judge whether the center of the disc interferes with the obstacles in the obstacle inflation map by traversing the center of each disc: if not, it is determined that the vehicle at this node does not collide with the obstacle, and the corresponding planning node is a feasible node; if so, 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 scenario.

[0079] Step 3: Optimize the initial trajectory in Step 2 using a numerical optimization method based on a probabilistic safe driving corridor: Rapidly construct a driving corridor that meets the requirements using the static risk field adaptive extended safe driving corridor method. Then, adjust the driving corridor based on the collision probability between the vehicle and dynamic obstacles. Provide an objective function and constraints that consider trajectory smoothness, comfort, safety, and efficiency, construct an optimal control problem for trajectory optimization, and finally numerically solve it and output the optimized optimal trajectory.

[0080] The following details the specific implementation process of the vehicle trajectory planning method for complex unstructured scenarios with dynamic obstacles provided by the embodiments of the present invention in combination with the pseudocode of Algorithm 1:

[0081]

[0082]

[0083] In one embodiment, the construction method of the hybrid A* algorithm specifically includes:

[0084] The analysis of the vehicle kinematic model is the core of trajectory planning to ensure that the planned trajectory can conform to the vehicle's motion characteristics. As Figure 1 shown, Figure 1 illustrates the global coordinate system XOY. The vehicle geometric parameters include the front overhang length L f , the rear overhang length L r , the wheelbase L m , the total length L, the vehicle width B, the front wheel steering angle δ 1 , the rear wheel steering angle δ 2 ; the center point of the vehicle's wheelbase is (x, y), and the vehicle's heading angle is The two constitute the vehicle's pose

[0085] The embodiments of the present invention consider the kinematic models of a four-wheel independent steering vehicle in three motion modes: Ackermann steering, wedge steering, and in-place steering. Among them, the Ackermann steering mode is the basic motion mode of a four-wheel independent steering vehicle. The vehicle's pose has both translation and rotation, which can provide high mobility and flexibility, especially showing good steering performance in a narrow space, as Figure 1 (a) of Figure 1 shows. The diagonal movement mode is a special motion mode of a four-wheel independent steering vehicle. The vehicle's pose has only translation and no rotation, as Figure 1 (b) of

[0086] In addition, constructing the hybrid A* algorithm requires providing a node expansion method and a cost function, which can be obtained by referring to existing literature.

[0087] In one embodiment, the method for obtaining the parameters related to the unstructured scenario specifically includes:

[0088] After analyzing the kinematic model of the four-wheel independently steered vehicle, the embodiments of the present invention need to obtain the following geometric and kinematic parameters to ensure that the trajectory planning can accurately reflect the actual motion ability of the vehicle, including obtaining the geometric parameters of the vehicle and the minimum speed v min , the maximum speed v max , the minimum acceleration a min , the maximum acceleration a max , the maximum steering angle δ max , the maximum steering angular velocity ω max and other kinematic parameters. At the same time, it is necessary to obtain the starting pose the ending pose and the obstacle map of the unstructured scenario. Subsequently, the obtained parameters related to the unstructured scenario are input into the subsequent trajectory planner.

[0089] To ensure that the vehicle avoids collisions with obstacles during the entire trajectory planning process, its trajectory must satisfy the collision avoidance constraint. Traditional collision detection methods usually need to traverse the vehicle contour, transforming the problem of "whether the vehicle collides with an obstacle" into the problem of "whether the vehicle contour interferes with the obstacle area". However, such methods have a relatively cumbersome calculation process. To solve these problems, the embodiments of the present invention adopt the equal-divided multi-disk method, approximating the rectangular vehicle contour as multiple disks, transforming the problem of "polygon-polygon interference" into the problem of "point-polygon interference" to achieve simpler and more efficient collision detection.

[0090] As Figure 2 shown, the contour of the rectangular vehicle is approximately represented by a double-disk model, where: the rectangular body is covered by two disks with the same radius, aiming to simplify the collision detection process. However, although this covering method can reflect the vehicle's shape to a certain extent, since the total area of the disks is larger than the area of the actual rectangular body, an additional redundant area ΔS 2 (such as the gray area in Figure 2 ) is generated during the covering process. This part of the redundant space does not correspond to the actual occupied area of the vehicle, so the area that could originally pass during trajectory planning is misjudged as impassable.

[0091] As Figure 3As shown, redundant space can cause the trajectory planning algorithm to fail to find a feasible solution when searching for a feasible path in narrow roads or areas with dense obstacles. Therefore, how to reduce the size of redundant space has become a key issue in improving the efficiency and accuracy of trajectory planning.

[0092] As Figure 4 shown, for the convenience of modeling, the embodiments of the present invention equally divide the rectangular vehicle longitudinally and transversely, that is:

[0093]

[0094] where n 1 and n 2 respectively represent the number of disks in the longitudinal and transverse directions of the vehicle, n = n 1 ·n 2 represents the number of equally divided multi-disks used to cover the vehicle, L and B are the length and width of the vehicle respectively, and respectively represent the equally divided lengths in the longitudinal and transverse directions of the vehicle.

[0095] The centers of the circles are located at these equally divided points, and the disks can just evenly divide the rectangular vehicle. Then, the radius of the disk can be calculated as R n , expressed as the following formula (2):

[0096]

[0097] Combined with the above formula and analyzed according to geometric relations, the area ΔS n of the redundant space can be obtained as:

[0098]

[0099] According to the above formula, by increasing the number of disks n covering the vehicle, the actual contour area of the vehicle can be approximated more precisely, thereby reducing the redundant space. The equally divided multi-disk model is to more accurately depict the actual contour of the vehicle by optimizing the position and number of disks, avoid affecting the effectiveness of trajectory planning due to excessive redundant space, and better meet the trajectory planning requirements in the environment of narrow roads. This improvement significantly enhances the passability of the vehicle and the feasibility of the trajectory planning algorithm, providing a more reliable initial solution for the subsequent optimization process.

[0100] In addition, during the trajectory planning process, in addition to avoiding collisions, the requirement of safety margin must also be considered. That is, the vehicle must maintain a sufficient safety distance from obstacles during driving, and avoid the collision risk caused by control errors and planning errors. The embodiments of the present invention stipulate that the minimum distance d between the vehicle and the obstacle is greater than a preset safety margin d safe。In the embodiments of the present invention, due to the existence of redundant space, it can be required that the extension lengths of the redundant space generated by the disks in the horizontal and vertical directions are each greater than the safety margin, thereby ensuring the safety of the entire planning process, that is

[0101]

[0102] where respectively represent the extension lengths of the disk in the horizontal and vertical directions, and their calculation formulas are as follows.

[0103]

[0104] Combining formulas (1), (2), (4) and (4), the maximum value of the number of disks that meet the safety margin d safe can be obtained As shown in formula (6):

[0105]

[0106] However, although increasing the number of disks can improve the approximation accuracy of the vehicle profile, it will also significantly increase the solution time. As can be seen from the pseudocode of Algorithm 2, in the equal - division multi - disk method, the increase in the number of disks directly leads to an increase in the number of calculation loops, thereby increasing the time complexity. This means that during the trajectory planning process, as the number of disks increases, the amount of calculation required for each iteration also increases, which in turn affects the real - time performance and computational efficiency of the algorithm. An overly long solution time may not meet the real - time requirements of the system. Therefore, when increasing the number of disks, a trade - off must be made between accuracy and computational efficiency to ensure that the algorithm can provide sufficient accurate obstacle avoidance ability without causing excessive computational overhead.

[0107] To evaluate the time cost of the multi - disk collision detection method, the embodiments of the present invention adopt an analysis method of asymptotic time complexity, where the time complexity is described by the big O notation to describe the growth relationship between the algorithm execution time and the data scale. In the equal - division multi - disk collision detection method adopted in the embodiments of the present invention, the algorithm process includes a main loop and a nested secondary loop. Therefore, the overall time complexity is a combination of the two loop structures. According to the process analysis of Algorithm 1, its time complexity can be expressed as formula (7):

[0108] O(n·N) (7)

[0109] where n is the number of equal - division multi - disks expanded, and N is the number of nodes.

[0110] This time complexity indicates that as the number of disks increases, the computational effort required for collision detection will increase significantly. Therefore, it is necessary to limit the number of disks to avoid excessive computational effort, which may affect the real-time performance and overall performance of the algorithm, i.e., Equation (8):

[0111]

[0112] where is the maximum number of disks under the time complexity limit. In the present invention, let

[0113] The maximum number of disks n max is expressed as Equation (9):

[0114]

[0115] In view of this, in one embodiment, the method of "constructing an equally divided multi-disk contour according to the rectangular contour of the vehicle body" in step 2 includes:

[0116] Approximating the rectangular vehicle body by multiple disks with the same radius that can cover the vehicle body. The area where the disks extend beyond the uncovered vehicle body is the redundant area. The area ΔS of the redundant area is reduced by presetting the number of disks n n , the maximum number of disks n max is obtained through Equation (9).

[0117] When using disks to approximate the rectangular vehicle contour, it is necessary to determine whether the disks collide with obstacles during the traversal process. The collision problem is still the "interference problem between polygons", and solving such problems is usually relatively complex. For this reason, in the embodiment of the present invention, each disk is shrunk towards its center, as shown in (a) of Figure 5 ; at the same time, the boundaries of the polygonal obstacles in the environment are externally expanded according to the disk radius, as shown in (b) of Figure 5 , where the dark gray represents the obstacle itself, and the light gray is the expanded boundary. In this way, the original obstacle avoidance constraint "the vehicle body does not collide with obstacles" is simplified to "the disk center does not interfere with the expanded obstacle area", ensuring the safety distance between the vehicle and the obstacles and improving the computational efficiency and path safety.

[0118] In step 1, the upper layer planning of the present invention is based on an improved hybrid A* algorithm, and node expansion is performed through the center point of the vehicle. Specifically, the hybrid A* 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. In the embodiment of the present invention, N nodes are interpolated on the path of the expanded sub-nodes to more accurately describe the movement trajectory of the vehicle on the path, and then collision detection is performed on the nodes.

[0119] The steps of collision detection will be specifically described below. The pseudo-code is shown in Algorithm 2.

[0120]

[0121] In the embodiments of the present invention, the collision problem is transformed into "the center of the disc does not interfere with the expanded obstacle area". First, in the embodiments of the present invention, the coordinates of the equally divided multi-disc centers are calculated based on the center point of the vehicle. In one embodiment, the "coordinates of the equally divided multi-disc centers" in step 2 are obtained by calculating based on the center point of the vehicle. The specific calculation method includes:

[0122] The i-th node in step 1 is represented as the vehicle pose point (x i ,y i ), which are the coordinates of the vehicle center point in the global coordinate system and the vehicle heading angle respectively, and the coordinates of the j-th disc center in the global coordinate system are obtained by calculation through Equation (10), where j ∈ [1, n], n is the total number of discs, and the number of discs n 1 and n 2 in the longitudinal and transverse directions of the vehicle are both n = n 1 ·n 2 ;

[0123]

[0124] Among them, and respectively represent the equally divided lengths in the longitudinal and transverse directions of the vehicle.

[0125] For example: Let n = 4 (where n 1 = 2, n 2 = 2), that is, a rectangular vehicle is approximated by four equally divided discs. As Figure 6 shown, in the global coordinate system, given the coordinates (x i ,y i ) of the vehicle center point and the vehicle heading angle φ i , then the coordinates of the four equally divided disc centers are respectively represented as The specific calculation description is Equation (10):

[0126]

[0127] Among them, and are the equally divided lengths in the longitudinal and transverse directions of the equally divided four discs respectively, and i ∈ [1, N] is the interpolation point corresponding to the extended path.

[0128] After that, by traversing the coordinates of the center of each disk, it is determined whether there is interference with the obstacle. If the center of the circle does not intersect with the obstacle, it can be determined that the vehicle at this node does not collide with the obstacle, and the corresponding planned node is a feasible node; otherwise, if the center of the circle is inside the obstacle, it means a collision has occurred and the node is determined to be infeasible.

[0129] 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 value constraint. At the same time, 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 within a safe and feasible driving area. The obstacle avoidance constraint provided by the present invention not only considers static obstacles but also takes into account the uncertainty of dynamic obstacles, and uses a risk field to quantify the movement of dynamic obstacles. The vehicle kinematic constraint ensures that the vehicle follows the actual physical and dynamic characteristics during the steering process. In addition, since the present invention takes a four-wheel independent steering vehicle as the research object and different motion modes have different kinematic equations, the trajectories in different modes are optimized separately. Considering that the trajectory of the vehicle in the in-place steering mode is a point, there is no need to establish the kinematic constraint for this mode. The two-point boundary value constraint stipulates the starting and ending positions of the trajectory and makes requirements on the relevant poses of the trajectory. One of the innovations of the present invention is the construction of the obstacle avoidance constraint, especially the method for constructing a probability safety driving corridor for the motion uncertainty of dynamic obstacles.

[0130] The primary task of trajectory planning is to ensure safety, and one method is to construct an obstacle avoidance constraint. However, in an unstructured scenario, static obstacles with complex shapes are randomly distributed, and this kind of obstacle avoidance constraint usually has a large scale and is non-differentiable, which makes it difficult for the optimal control problem solver to solve. To simplify these constraints and reduce the complexity of the solution, the embodiments of the present invention represent the safe area that meets the collision requirements by constructing a driving corridor, converting the complex obstacles into geometric areas that are easy to handle, thereby improving the efficiency and solution accuracy of trajectory optimization.

[0131] Most existing studies construct driving corridor constraints based on the contours of static obstacles. However, in actual unstructured scenarios, there are dynamic obstacles such as other vehicles and pedestrians. Therefore, it is insufficient to only consider static obstacles when constructing a driving corridor. At this time, considering the uncertainty of the movement of dynamic obstacles, it becomes inappropriate to construct a driving corridor with a definite obstacle contour. The uncertainty of the movement of dynamic obstacles can be considered from the following aspects. On the one hand, a vehicle collects multi-source information through multiple sensors to determine the position of dynamic obstacles. However, due to sensor measurement errors, external environmental changes, or the inherent uncertainty of the perception algorithm, the positioning results of dynamic obstacles will be inaccurate. On the other hand, the continuous movement of dynamic obstacles causes their obstacle contours to change continuously, thus affecting the shape and size of the driving corridor. Therefore, during the construction of the driving corridor, it is necessary to update information in real time to ensure the accuracy of the corridor size and the safety of trajectory planning.

[0132] To address the above problems, the present invention proposes a method for constructing a probabilistically safe driving corridor. During the construction process, the uncertainty of the obstacle position is considered, and the collision situation is described by the collision probability risk. Specifically, the dynamic obstacles in the scenario are not considered for the time being, and the focus is on static obstacles. The present invention provides a method for adaptively expanding a safe driving corridor based on a static risk field. Through an adaptive step mechanism, the expansion step of the driving corridor can be dynamically adjusted. In addition, the static risk field is used to quantitatively analyze the collision between the vehicle and static obstacles, strictly ensuring that the static risk value at any point inside the driving corridor is lower than the threshold. This method can quickly obtain the maximum safe driving corridor that meets the safety requirements based on the current position of the vehicle. On this basis, the embodiments of the present invention further consider the movement of dynamic obstacles, quantify the uncertainty of the movement of dynamic obstacles with a risk distribution, and quickly shrink the driving corridor based on the risk value on the basis of the maximum safe driving corridor obtained above, ensuring that any point within the driving corridor is below the set risk value, obtaining a probabilistically safe driving corridor, and finally simplifying the obstacle avoidance constraint into a simple linear constraint.

[0133] The embodiments of the present invention consider the uncertainty of the movement of dynamic obstacles and the vehicle's perception module to model dynamic obstacles. The present invention reconstructs the driving corridor through a risk field that predicts the obstacle trajectory, making the description of the obstacle avoidance constraint more accurate.

[0134] There are N 0 dynamic obstacles in the environment. Assume that the motion state of the i 0 (i 0 ∈[1, N 0 )th dynamic obstacle at time t is S(x, t):

[0135]

[0136] where and respectively represent the lateral position and the longitudinal position of the predicted dynamic obstacle in Cartesian coordinates. represent the lateral velocity, longitudinal velocity, lateral acceleration, and longitudinal acceleration of the dynamic obstacle. In lateral and longitudinal motions, the acceleration can be considered constant over a single time step.

[0137] Then, for the dynamic obstacle i 0 the position information at the next moment t + 1 can be described by Equation (12):

[0138]

[0139] where ΔT is the time step.

[0140] As can be seen from the above formula, the uncertainty of the lateral and longitudinal positions of the dynamic obstacle is determined by the initial position uncertainty, acceleration uncertainty, and velocity uncertainty. In addition, the lateral and longitudinal motions of the dynamic obstacle are decoupled, independent of each other, and follow different Gaussian distributions, and are determined by and the discrete time interval ΔT. Therefore, and are also independent of each other and follow different Gaussian distributions. The random variable distributions of the dynamic obstacle are shown in Table 1 below.

[0141] Table 1

[0142]

[0143]

[0144] In the table, σ px is the standard deviation of the initial longitudinal position distribution, σ py is the standard deviation of the initial lateral position distribution, σ vx is the standard deviation of the initial longitudinal velocity distribution, σ vy is the standard deviation of the initial lateral velocity distribution, σ ax is the standard deviation of the predicted longitudinal acceleration distribution, σ ay is the standard deviation of the predicted lateral acceleration distribution.

[0145] In summary, the position distributions of the dynamic obstacle i 0 in the lateral and longitudinal directions can be expressed as as shown in Equation (13) below:

[0146]

[0147] The linear combination of independent Gaussian distributions still follows a Gaussian distribution. Therefore, the position distributions of individual dynamic obstacles can be considered to have a covariance where and represent the variances in the lateral and longitudinal directions respectively, and can be described as shown in Equation (14) below:

[0148]

[0149] In summary, the probability density function of the random position distribution of the i-th 0 dynamic obstacle at time t in the parking environment can be expressed as:

[0150]

[0151] where and are intermediate parameters without substantial physical meaning, k = 1, 2,..., N f , N f is the number of set path sampling points.

[0152] The position distributions of other dynamic obstacles can be obtained in the same way. Then, based on the trajectory distributions of each dynamic obstacle obtained in sequence, the risk level of the vehicle position is evaluated.

[0153] The present invention uses the collision risk to describe the collision between the vehicle and the dynamic obstacle. First, the position distribution of the dynamic obstacle is calculated based on Equation (15), and then the collision probability between the vehicle and each dynamic obstacle at any time is evaluated. The probability of collision with the i-th 0 dynamic obstacle at time t is expressed as

[0154]

[0155] where D is the projection of the host vehicle on the two-dimensional plane x-y, is the probability density function of the random position distribution of the i-th dynamic obstacle at time t, and (x ego , y ego ) is the position of the host vehicle at time t. Considering that the probability density function of the random position distribution of the dynamic obstacles in the surrounding area of the host vehicle changes little and can be approximated as a constant, the embodiment of the present invention can simplify the above Equation (16) to Equation (17) to improve the calculation efficiency:

[0156]

[0157] In the formula, is the probability of collision with the i-th 0The probability of collision with a dynamic obstacle, S D is the projected area of the host vehicle on the two-dimensional plane x-y, and is the probability density function of the random position distribution of the i 0 -th dynamic obstacle at time t.

[0158] After calculating the collision probability between the host vehicle and all obstacles at time t according to the above method, the collision probability P ego ,y ego ) of the host vehicle at point (x t at time t is expressed as Equation (18):

[0159]

[0160] Most of the methods for constructing a driving corridor in existing research adopt a fixed step size, which is inefficient. Therefore, in order to improve the construction efficiency of the driving corridor, the embodiments of the present invention adopt an adaptive step size method. This method can dynamically adjust the step size according to the distance between the vehicle and the obstacle. In addition, most of the driving corridors adopted in traditional trajectory optimization directly construct the maximum size, which may cause the finally optimized path points to be too close to the obstacle. If the accuracy of subsequent tracking control is insufficient, it will increase the risk of collision between the vehicle and the obstacle. Therefore, the present invention provides a method for adaptively expanding a safe driving corridor based on a static risk field. 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, calculate the risk field of the closest distance between the boundary of the driving corridor and the static obstacle, and require that this risk field is lower than a given threshold ∈ static , so as to ensure that the optimized path points always maintain a safe distance from the obstacle. The calculation formula of the static risk field can be expressed by Equation (19):

[0161]

[0162] In the formula, A 0 is the field strength coefficient, x ego ,y ego is the position of the host vehicle, x eobs ,y obs is the position of the static obstacle, and β is a high-order constant coefficient.

[0163] The specific expansion steps are as follows. As shown in Figure 7 , in the (a) of Figure 7 , during the (k-1)-th expansion, the expansion step size of the upper edge of the driving corridor is 2s, and it directly collides with the obstacle, so the expansion is invalid. The expansion step size of the right edge is 2s, which meets the safety requirements, and a feasible expansion area is constructed, and there is still a large feasible space. In Figure 7(b) In the k-th expansion, the expansion step size of the reduced upper edge is s, but it does not meet the requirements of the static risk field, so the expansion is invalid. The expansion step size of the right edge increases to 4s and a collision occurs, so the expansion is invalid. At Figure 7 (c) In the (k + 1)-th expansion, the expansion step size of the reduced upper edge is s / 2, which meets the safety requirements. The expansion step size of the right edge is 2s, but it does not meet the requirements of the static risk field, so the expansion is invalid. At Figure 7 (b) In the (k + 2)-th expansion, the expansion step size of the right edge is reduced to s, which meets the safety requirements.

[0164] Set the initial step size s of each direction m (m = 1, 2, 3, 4, representing the upper, right, lower, and left directions respectively) of the vehicle to a random value between [0.5, 1). Assume that it has been expanded k 0 times in the direction m.

[0165] In the embodiment of the present invention, it is necessary to analyze the expansion situation at each time step to dynamically adjust the step size of each expansion. If the boundary of the driving corridor in the k-th expansion meets the requirements of the static risk field, it indicates that there is still a large feasible space in this direction. The embodiment of the present invention can appropriately increase the step size of the (k 0 + 1)-th time. For example, to improve the expansion efficiency in this direction; on the contrary, if the i-th expansion does not meet the requirements of the static risk field, it does not mean that the expansion in the m direction is completed, but indicates that the current step size is no longer suitable. The embodiment of the present invention needs to appropriately reduce the step size and continue the expansion. The embodiment of the present invention can consider setting the step size of the (k 0 + 1)-th time and record the reduction times. If the reduction times in the m direction reach the set maximum reduction times N 1 , the expansion process in this direction is stopped. In this way, by using adaptive adjustment of the expansion length, the expansion length L m in the m direction reaches the maximum value that meets the requirements of the static risk field, and the expansion is performed on each direction in turn. Preferably, "rapidly constructing a driving corridor that meets the requirements based on the method of adaptively expanding the safe driving corridor according to the static risk field" in step 3 specifically includes:

[0166] By using adaptive adjustment of the expansion length, the expansion length L m in the m direction reaches the maximum value that meets the requirements of the static risk field, and the expansion is performed on each direction in turn. At the point the driving corridor C with the maximum size is obtained, which is max described by the following formula (20):

[0167]

[0168] where (x, y) are the coordinates of the nodes on the initial path, and k = 1, 2,..., N f, N f is the total number of initial path sampling points, and Respectively indicate points The longitudinal and transverse coordinates in the global coordinate system, E m Indicates the edges in the upper, right, lower and left directions of the corridor, corresponding to m=1, 2, 3, 4 respectively.

[0169] The adaptive step-length safe driving method corridor can quickly construct the maximum safe driving corridor, but at this stage, the embodiment of the present invention only considers static obstacles. Therefore, in order to further ensure safety, the embodiment of the present invention considers the influence of dynamic obstacles to optimize the driving corridor and locally adjust the driving corridor C. max The probability of a vehicle colliding with a dynamic obstacle shows a typical two-dimensional Gaussian distribution characteristic, which is gradually reduced from the mean center to the outside. In order to ensure the safety of vehicles when driving in the driving corridor, the embodiment of the present invention requires that the probability P of each point at the boundary of the driving corridor colliding with a dynamic obstacle dyn Less than the set safety threshold ∈ dynamic .

[0170] Specifically, in E m The edge upsampling k 1 points, and calculate the collision probability of the sampling points. dyn The area is considered safe only when the values ​​of the edges E and E are all smaller than the preset threshold. Otherwise, the corridor in this direction must be shrunk, and the step size Δs is reduced each time until the collision probability of all points is smaller than the safety threshold. Similarly, in order to improve the calculation efficiency of shrinking the corridor, the embodiment of the present invention shrinks the corridor in the longitudinal and transverse directions in sequence, where the transverse direction includes the edge E 2 and E 4 , the longitudinal direction includes the edge E 1 and E 3 , this approach can reduce the amount of interpolation calculation by about 2 times, thereby improving the overall calculation efficiency, such as Figure 8 As shown in (a)(b) above.

[0171] Please note that if the C before contraction max If the corridor contains dynamic obstacles, you need to first adjust the driving corridor to a position that does not overlap with the dynamic obstacles, such as Figure 8 As shown in (c) in .

[0172] After the above method, a driving corridor that meets the safety requirements is obtained, and its edge is represented by E ∈,m , m = 1, 2, 3, 4. Based on the construction of a probabilistic safe driving corridor for each point, the vehicle's obstacle avoidance constraint can be written as a simple linear constraint (21):

[0173]

[0174] Among them, and i 1 ∈[1, n] represents the abscissa and ordinate of the center of the equally divided multi-disc of the entire vehicle.

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

[0176] Considering the actual motion ability of the vehicle and ensuring 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 separately as follows:

[0177]

[0178] Formulas (22) and (23) are the kinematic constraints of the Ackermann steering mode and the diagonal movement mode respectively. The trajectory of the vehicle in the in-place steering mode is a point and does not need to be optimized, so kinematic constraints do not need to be established. In the formulas, x(t) and y(t) represent the abscissa and ordinate of the center point of the vehicle wheelbase; represents the yaw angle of the vehicle, that is, the angle between the positive direction of the vehicle longitudinal axis and the positive direction of the X axis; v(t) and a(t) represent the speed and acceleration of the vehicle respectively; δ(t) and ω(t) represent the steering angle and angular velocity of the wheel respectively. And the state quantity is defined as The control quantity u(t) = [a(t), ω(t)].

[0179] At the same time, the state quantity and control quantity of the vehicle are limited by the mechanical characteristics and motion ability of the vehicle, and there is an allowable range of values, mainly including:

[0180]

[0181] In the formula: describes the upper and lower bounds of the values of the speed v(t), acceleration a(t), steering angle δ(t) and steering angular velocity ω(t).

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

[0183] Considering 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 the same. At the same time, there is a vehicle motion mode switch 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 value constraints are defined as:

[0184]

[0185] In the formula, represents the pose at the starting point of optimization \(t = t s moment; represents the pose at the ending point of optimization \(t = t f moment.

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

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

[0188]

[0189] where \(w 1 and \(w 2 are normalized weighting coefficients, and their specific values are obtained through experiments. \(T = t f -t s represents the overall planning time, \(a 2 (\tau)\) and \(\omega 2 (\tau)\) represent driving comfort, and \(\varphi 2 (\tau)\) represents the smoothness of the trajectory.

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

[0191] After constructing the optimal control problem of trajectory optimization, it is necessary to solve this problem to obtain the optimal solution. To effectively solve this problem, the present invention discretizes the continuous variables in the optimal control problem. By discretizing the continuous variables, the embodiment of the present invention transforms the original optimal control problem into a discretized nonlinear programming problem.

[0192] First, for each sampling moment s uniformly distributed in the time domain \([t f \), the constraint conditions and the objective function corresponding to it are discretized and transformed, and the result is: 1) The constraints on the positions of the centers of the front and rear disks in the discretized driving corridor are as follows;

[0193] 2) The local rectangle constraints in the discretized driving corridor are as follows;

[0194]

[0195] 3) The constraints of the nonlinear kinematic equation in the discretized kinematic constraints are as follows;

[0196]

[0197] 4) The constraints of the discretized kinematic constraints are as follows;

[0198]

[0199] Among them, formulas (29) and (30) are the kinematic constraints of the Ackermann steering mode and the diagonal movement mode respectively. 4) The value range constraints of the state variables and control variables in the discretized kinematic constraints are as follows;

[0200]

[0201] 5) The starting point constraints in the discretized two-point boundary value constraints are as follows;

[0202]

[0203] 6) The ending point constraints in the discretized two-point boundary value constraints are as follows;

[0204]

[0205] 7) The discretized objective function is shown in formula (34) as follows:

[0206]

[0207] Among them, is the objective function, T is the overall planning time, a 2 (τ) and ω 2 (τ) represent driving comfort, φ 2 (τ) represents the smoothness of the trajectory, w 1 and w 2 are normalized weighting coefficients, k = 1, 2,..., N f , N f is the total number of set path sampling points.

[0208] Then, the above discretized constraint conditions and objective function are summarized to form a complete nonlinear programming problem to be solved as:

[0209] min objective function (34)

[0210]

[0211] Finally, the open-source solver IPOPT based on the interior point method widely used in the field of autonomous driving is used to solve the above-constructed nonlinear programming problem to obtain the numerical optimal solution of the trajectory, and finally the optimal trajectory is obtained In this embodiment, the optimal trajectory finally planned in an unstructured scenario containing only static obstacles is as Figure 9 shown. The blue curve in the figure is the initial path searched by the upper-layer improved hybrid A*, and the blue curve is the trajectory after numerical optimization by the lower layer.

[0212] AsFigure 10 As shown, it is a scenario containing two dynamic obstacles. Since the initial trajectory algorithm does not consider dynamic obstacles, the planned trajectory collides with the dynamic obstacles. In the optimization stage, a probabilistic safety driving corridor is adopted, and the final trajectory obtained is the blue curve, realizing the avoidance of dynamic obstacles.

[0213] The present invention also provides a trajectory planning system for a narrow unstructured scenario with dynamic obstacles, which includes a path node expansion unit, an initial path generation unit, and an optimal trajectory generation unit, where:

[0214] The path node expansion unit is used to analyze the kinematic model characteristics of a four-wheel independently steerable vehicle, construct a hybrid A* algorithm, obtain relevant parameters of the unstructured scenario, search from the starting point to the target point using the hybrid A* algorithm, and generate a series of path expansion nodes by using the kinematic constraints and cost function of the vehicle, and interpolate N nodes on the expanded node path.

[0215] The initial path generation unit is used to solve a collision-free initial path according to the hybrid A* algorithm of the path node expansion unit and in combination with an improved collision detection algorithm; among them, the improved collision detection algorithm includes:

[0216] According to the rectangular contour of the vehicle body, construct an equally divided multi-disc contour and an obstacle inflation map based on the equally divided multi-disc, calculate the center coordinates of the equally divided multi-disc, and by traversing the center of each disc and determining whether the center of the disc interferes with the obstacles in the obstacle inflation map: if not, it is determined that the vehicle does not collide with the obstacles at this node, and the corresponding planned node is a feasible node; if so, it is determined that the corresponding planned node is an infeasible node.

[0217] The optimal trajectory generation unit is used to optimize the initial trajectory of the initial path generation unit by adopting a numerical optimization method based on a probabilistic safety driving corridor: quickly construct a driving corridor that meets the requirements based on the static risk field adaptive expansion safety driving corridor method, and then adjust the driving corridor according to the collision probability between the vehicle and the dynamic obstacles, provide an objective function and constraint conditions considering trajectory smoothness, comfort, safety, and efficiency, construct an optimal control problem for trajectory optimization, and finally numerically solve and output the optimized optimal trajectory.

[0218] At present, there is relatively little research on trajectory planning in unstructured scenarios with narrow roads and dynamic obstacles. Especially in such an environment, it is difficult for traditional trajectory planning methods to find a feasible solution. Therefore, this invention takes a four-wheel independently steered vehicle as the research object and proposes a complete trajectory planning algorithm. In the upper-level planning stage, a hybrid A* algorithm based on equal-partition multi-disk collision detection is adopted, and in the lower-level optimization, an obstacle avoidance constraint is constructed by a probabilistic safety driving corridor. Compared with the traditional hybrid A* algorithm, this invention provides an equal-partition multi-disk collision detection method, which uses equal-partition multi-disks to approximate the vehicle contour and simplifies the collision by transforming the problem description of collision detection, ensuring that the four-wheel independently steered vehicle can find a safe and feasible path in the face of narrow road scenarios. For the obstacle avoidance constraint of trajectory optimization, this invention provides a probabilistic safety corridor method, which includes a method for adaptively expanding the safety driving corridor based on a static risk field and a method for constructing a probabilistic safety corridor considering dynamic obstacles. The efficiency is improved by using an adaptive step size to expand. Considering the path control accuracy problem after optimization, a static risk field is introduced to limit the maximum size of the driving corridor, further ensuring the safety of the planning. The uncertainty of the movement of dynamic obstacles is also considered, ensuring that the obstacle avoidance constraint is more in line with the actual environment. The risk field is used to quantify the uncertainty of the movement of dynamic obstacles, calculate the collision probability between the obstacle and the vehicle, and dynamically shrink the driving corridor based on this to ensure that the trajectory can always meet the safety requirements in a changing environment. This invention realizes the generation of continuous, smooth, comfortable, safe and efficient trajectories for four-wheel independently steered vehicles in unstructured scenarios with narrow roads and dynamic obstacles.

[0219] In the above embodiments, an improved hybrid A* algorithm is adopted in the upper layer of this invention, and a collision detection method is provided again, or other trajectory planning algorithms based on search or sampling can be used for substitution; in the lower-level trajectory planning, if other types of obstacle avoidance constraints are adopted and the indicators of the objective function are adjusted, similar effects may be achieved. These variants all reflect the core idea of this invention.

[0220] Finally, it should be pointed out that the above embodiments are only used to illustrate the technical solutions of this invention, rather than to limit them. Those of ordinary skill in the art should understand that the technical solutions recorded in the foregoing embodiments can be modified, or some of the technical features can be equivalently replaced; 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 this invention.

Claims

1. A trajectory planning method for narrow unstructured scenes with dynamic obstacles, characterized in that: include: Step 1: Analyze the kinematic model characteristics of the four-wheel independent steering vehicle, construct a hybrid A* algorithm, obtain the relevant parameters of the unstructured scene, use the hybrid A* algorithm to search from the starting point to the target point, use the vehicle's kinematic constraints and cost function to generate a series of path extension nodes, and interpolate N nodes on the extension node path; Step 2: According to the hybrid A* algorithm of step 1, combined with the improved collision detection algorithm, solve the initial collision-free path; wherein the improved collision detection algorithm includes: According to the rectangular outline of the vehicle body, the equally divided multi-disk outline and the obstacle expansion map based on the equally divided multi-disk are constructed, and the coordinates of the center of the equally divided multi-disk are calculated. By traversing the center of each disk, it is determined whether the center of the disk interferes with the obstacle in the obstacle expansion map: if not, it is determined that the vehicle at the node has not collided with the obstacle, and the corresponding planning node is a feasible node; if so, the corresponding planning node is determined to be an infeasible node; Step 3, optimize the initial trajectory of step 2 using a numerical optimization method based on probabilistic safe driving corridors: quickly construct a driving corridor that meets the requirements based on the static risk field adaptive expansion safe driving corridor method, then adjust the driving corridor according to the collision probability between the vehicle and the dynamic obstacle, provide objective functions and constraints that consider trajectory smoothness, comfort, safety, and efficiency, construct a trajectory optimization optimal control problem, and finally numerically solve and output the optimized optimal trajectory.

2. The trajectory planning method for a narrow unstructured scene with dynamic obstacles as claimed in claim 1, characterized in that: The method of "constructing equally divided multi-disk contours according to the rectangular contour of the vehicle body" in step 2 includes: A rectangular body is approximately represented by multiple disks with the same radius that can cover the body. The area where the disks extend out and do not cover the body is the redundant area. The area of ​​the redundant area is reduced by presetting the number of disks n. n , the maximum number of disks n max Through formula (9), we can obtain: in, To meet the preset safety margin d between the vehicle and the obstacle safe The maximum number of disks required is expressed by equation (6): is the maximum number of disks under the time complexity constraint, obtained by formula (8): in, are the lengths of the rectangular vehicle divided equally along the length and width, L and B are the length and width of the rectangular vehicle respectively, O(n·N) is the time complexity of the collision detection algorithm, and n is the number of expanded equally divided multi-disks.

3. The trajectory planning method for a narrow unstructured scene with dynamic obstacles as claimed in claim 2, characterized in that: The "equally divided multi-disc center coordinates" in step 2 are calculated based on the center point of the vehicle. The specific calculation method includes: The i-th node in step 1 is represented as the vehicle pose point (x i ,y i ), are the coordinates of the vehicle center point in global coordinates and the vehicle heading angle, and the coordinates of the center of the j-th disk in global coordinates It is obtained by calculation through formula (10), j∈[1,n], n is the total number of disks, and the number of disks n1 and n2 in the longitudinal and lateral directions of the vehicle are both n=n1·n2; in, and Respectively represent the equal lengths of the vehicle in the longitudinal and transverse directions.

4. The trajectory planning method for a narrow unstructured scene with dynamic obstacles as claimed in claim 1, characterized in that: Step 3 of "rapidly constructing a driving corridor that meets the requirements based on the static risk field adaptive expansion safe driving corridor method" specifically includes: By adaptively adjusting the expansion length, the expansion length L in the m direction m To reach the maximum value that meets the requirements of the static risk field, expand in each direction in turn. The maximum driving corridor C is obtained at max It is described as the following formula (20): Where (x, y) is the coordinate of the node on the initial path, k = 1, 2, ..., N f , N f is the total number of initial path sampling points, and Respectively indicate points The longitudinal and transverse coordinates in the global coordinate system, E m Indicates the edges in the upper, right, lower, and left directions of the corridor.

5. The trajectory planning method for a narrow unstructured scene with dynamic obstacles as claimed in claim 1, characterized in that: The "collision probability between the vehicle and the dynamic obstacle" in step 3 specifically includes: The vehicle is at position (x ego ,y ego ) has a collision probability P t It is expressed as formula (18): in, is the probability of colliding with the i0th dynamic obstacle at time t, expressed as formula (17): Among them, S D is the projection area of ​​the vehicle on the two-dimensional plane xy, is the probability density function of the random position distribution of the i0th dynamic obstacle at time t.

6. The trajectory planning method for a narrow unstructured scene with dynamic obstacles according to any one of claims 1 to 5, characterized in that: The "objective function" of step 3 is expressed as formula (34): in, is the objective function, T is the overall planning time, a 2 (τ) and ω 2 (τ) represents driving comfort, φ 2 (τ) represents the smoothness of the trajectory, w1 and w2 are normalized weighting coefficients, k = 1, 2, ..., N f , N f is the total number of sampling points for the set path.

7. The trajectory planning method for a narrow unstructured scene with dynamic obstacles as claimed in claim 6, characterized in that: The "constraints" in step 3 include obstacle avoidance constraints, vehicle kinematic constraints, and two-point boundary value constraints. The obstacle avoidance constraints specifically include: (1) The positions of the centers of the front and rear disks in the discretized corridor constraint in global coordinates (x j ,y j )The constraints are as follows; (2) The local rectangular constraints in the discretized driving corridor constraints are as follows; in, The edge of the driving corridor that meets the safety requirements is extended from the center of each disk, m=1,2,3,4, representing the top, bottom, left and right of the driving corridor respectively.

8. The trajectory planning method for a narrow unstructured scene with dynamic obstacles as claimed in claim 7, characterized in that: The vehicle kinematic constraints include: (1) The nonlinear kinematic equations in the discretized kinematic constraints are as follows: Equations (29) and (30) are the kinematic constraints for the Ackerman steering mode and the oblique movement mode, respectively; (2) The value ranges of the state and control variables in the discretized kinematic constraints are as follows; Where v, a, δ, and ω represent velocity, acceleration, steering angle, and steering angular velocity, respectively. The subscripts min and max represent the minimum and maximum values, respectively. step For equal time steps, l m is the vehicle wheelbase.

9. The trajectory planning method for a narrow unstructured scene with dynamic obstacles as claimed in claim 8, characterized in that: The two-point boundary value constraints include: (1) The starting point constraint in the discretized two-point boundary value constraint is as follows; (2) The end point constraints in the discretized two-point boundary value constraints are as follows; The subscripts s and f represent the starting point and the ending point respectively.

10. A trajectory planning system for narrow unstructured scenes with dynamic obstacles, characterized in that: include: A path node extension unit is used to analyze the kinematic model characteristics of a four-wheel independent steering vehicle, construct a hybrid A* algorithm, obtain unstructured scene related parameters, use the hybrid A* algorithm to search from the starting point to the target point, use the vehicle's kinematic constraints and cost functions to generate a series of path extension nodes, and interpolate N nodes on the extension node path; The initial path generation unit is used to solve a collision-free initial path based on the hybrid A* algorithm of the path node expansion unit combined with the improved collision detection algorithm; wherein the improved collision detection algorithm includes: According to the rectangular outline of the vehicle body, the equally divided multi-disk outline and the obstacle expansion map based on the equally divided multi-disk are constructed, and the coordinates of the center of the equally divided multi-disk are calculated. By traversing the center of each disk, it is determined whether the center of the disk interferes with the obstacle in the obstacle expansion map: if not, it is determined that the vehicle at the node has not collided with the obstacle, and the corresponding planning node is a feasible node; if so, the corresponding planning node is determined to be an infeasible node; The optimal trajectory generation unit is used to optimize the initial trajectory of the initial path generation unit by adopting a numerical optimization method based on a probabilistic safe driving corridor: based on the static risk field adaptive expansion safe driving corridor method, a driving corridor that meets the requirements is quickly constructed, and then the driving corridor is adjusted according to the collision probability between the vehicle and the dynamic obstacle, providing an objective function and constraint conditions that consider trajectory smoothness, comfort, safety, and efficiency, constructing a trajectory optimization optimal control problem, and finally numerically solving and outputting the optimized optimal trajectory.

Images