Collision detection method in unstructured environment with dense obstacles based on ray tracing

Abstract

The present invention provides a collision detection method in an unstructured environment with dense obstacles based on ray tracing, comprising the following steps: based on the vehicle shape and size, equating the vehicle to an equivalent circle or a shape model of multiple equivalent circles distributed along the centerline of the vehicle's length; aggregating multiple grid cells in occupied states within a set distance into aggregated obstacles through an aggregation algorithm for an input occupancy grid map based on a global Cartesian coordinate system, and then dilating the boundary area of ​​the aggregated obstacles; performing collision detection at each path node of a planned path based on the heading angle information corresponding to the path node, i.e., the vehicle reference point, according to the vehicle shape model; constructing a target cost function for an optimization planner when the center of the circle is located within the expanded aggregated obstacle; and constructing a target cost function for the optimization planner when the center of the circle is located outside the expanded aggregated obstacle. The present invention reduces the complexity of collision detection and supports different types of planners.

Description

Technical Field

[0001] The present invention relates to the field of motion planning and control, and in particular to a collision detection method in an unstructured environment with dense obstacles based on ray tracing. Background Art

[0002] With the rapid development of assisted driving technologies, motion planning and control techniques in structured road environments, especially on highways, have become relatively mature. For example, the EM planner used in Baidu's open-source Apollo autonomous driving framework can effectively implement functions such as autonomous cruising along lane lines and lane changing and overtaking. This type of planner is typically based on the Frenet coordinate system of curves.

[0003] However, for typical applications such as full-area patrols in various campuses / camps, lawn mowing, battlefield reconnaissance, border patrols in the wild, and automated parking in parking lots, autonomously navigated vehicles or robots operate in unstructured environments, which typically use Cartesian coordinate systems. Planners designed for structured road environments are not suitable for free-space motion planning in unstructured environments. Free-space planners for unstructured environments can be divided into two categories: one is planning methods based on search or sampling, such as the mainstream hybrid A-star planning algorithm used by open-source autonomous driving frameworks like Apollo and Autoware for reversing scenarios; the other is planning methods based on optimization, such as the Timed Elastic Band (TEB) algorithm integrated into the Navigation 2 stack of the Robot Operating System (ROS2).

[0004] In unstructured environments, obstacle distribution is no longer constrained by road structure, such as lane lines and road edges. Obstacle information is typically represented as an occupancy grid map (OGM) for free space. Whether using a search, sampling, or optimization planner, obstacle collision detection requires checking the relative spatial position of each path node and all obstacles when performing feasibility checks or optimizing waypoint adjustments. The computational complexity is typically O(mn), where m represents the number of path nodes provided by the planner and n represents the number of obstacles or occupied grid cells (depending on whether occupied grid cells are aggregated into convex polygons or ellipses). In scenarios with dense obstacles, the complexity of collision detection increases linearly with the number of obstacles.

[0005] Compared with search-based or sampling-based planners, optimization-based planners have a smaller optimization space and can converge to the optimal solution with lower algorithmic complexity, but the optimized solution cannot guarantee the global optimal solution. Search-based or sampling-based planners cannot guarantee the search for the global optimal solution, but they can ensure that they will not fall into the local optimal solution. However, the computational complexity is high. Therefore, optimization-based planners have become the mainstream direction of planners due to their better real-time performance.

[0006] From the perspective of obstacle collision detection, search or sampling planners must perform obstacle collision detection during the path search or sampling process, while optimization planners need to directly use collision-free behavior as a constraint or cost objective in the optimization problem to achieve automatic obstacle avoidance planning. In comparison, search or sampling planners call obstacle collision detection algorithms more frequently than optimization planners. Optimization planners require collision detection algorithms to be second-order differentiable, and excessive constraints will increase the optimizer's solution space and slow convergence. Furthermore, regardless of the planner, obstacle collision detection is the final, critical step after path planning to determine whether the entire planned path is feasible.

[0007] Therefore, reducing the computational complexity of the collision detection algorithm in unstructured environment with dense obstacles is of great significance for improving the real-time performance of motion planning and control or reducing the computing resources occupied by the motion planning and control algorithm. Summary of the Invention

[0008] In order to solve at least one technical problem in the prior art, an embodiment of the present invention provides a collision detection method in an unstructured environment with dense obstacles based on ray tracing. In the free space corresponding to the unstructured environment, facing the scene with dense obstacles, according to the obstacle detection results represented by the occupancy grid map, collision detection is implemented at each path node of the planned path based on ray tracing, which is independent of the number of obstacles; at the same time, a gradient-continuous target cost function is constructed based on the distance between the center of the vehicle equivalent circle and the obstacle obtained in the collision detection process, so that different types of planners such as search or sampling planners and optimization planners can be flexibly supported. In order to achieve the above technical objectives, the technical solutions adopted by the embodiment of the present invention are:

[0009] An embodiment of the present invention provides a collision detection method in an unstructured environment with dense obstacles based on ray tracing, comprising the following steps:

[0010] Step S10, based on the vehicle shape and size, equating the vehicle to an equivalent circle or a shape model of multiple equivalent circles distributed along the center line of the vehicle length direction; determining the center of the equivalent circle;

[0011] Step S20: Using an aggregation algorithm, the input occupancy grid map based on the global Cartesian coordinate system is aggregated into a clustered obstacle by aggregating multiple grid cells within a set distance. The clustered obstacle is then expanded by a radius equal to the radius of the vehicle equivalent circle to obtain the expanded clustered obstacle.

[0012] Step S30: At each path node of the planned path, based on the heading angle yaw information corresponding to the path node, i.e., the vehicle reference point, collision detection is performed according to the vehicle's shape model, assuming that the heading angle yaw is consistent with the vehicle's heading angle θ; and constructing a target cost function for the optimization planner when the center of the circle is within the expanded aggregated obstacle.

[0013] Step S40, using a corresponding ray tracing method based on the vehicle's shape model, constructing a target cost function for the optimization planner when the center of the circle is outside the expanded aggregated obstacle;

[0014] Step S50 , repeating steps S30 and S40 until the collision detection of all path nodes on the path and the construction of the corresponding target cost function are completed.

[0015] Furthermore, step S10 specifically includes:

[0016] For a small vehicle, the vehicle is equivalent to an equivalent circle, and determining the center of the equivalent circle includes: using the center of gravity of the vehicle as a reference point of the vehicle, that is, the center of the equivalent circle;

[0017] For large vehicles, the vehicle is equivalent to more than three equivalent circles. The center of the equivalent circle is determined by selecting the center of the rear axle of the vehicle as the vehicle reference point, and in the Cartesian coordinate system, the center coordinates (x i ,y i ) is expressed by formula (1) and formula (2):

[0018]

[0019]

[0020] Among them, (x, y) is the coordinate of the vehicle reference point, θ is the vehicle heading angle, (x i ,y i ) represents the coordinates of the center of the i-th equivalent circle, D F Indicates the distance between the vehicle reference point and the front of the vehicle, D R Indicates the distance between the vehicle reference point and the rear of the vehicle;

[0021] When used for an optimization class planner, equations (1) and (2) serve as hard constraints for the optimization class planner.

[0022] Further, in step S20, the aggregation algorithm includes a monotone chain convex hull MCCH algorithm, which aggregates multiple grid units in occupied states within a set distance into a convex polygon through the monotone chain convex hull MCCH algorithm; or, the aggregation algorithm includes a minimum volume closure ellipsoid MVEE algorithm, which aggregates multiple grid units in occupied states within a set distance into an ellipse through the minimum volume closure ellipsoid MVEE algorithm.

[0023] Furthermore, in step S20, aggregating the grid units in a plurality of occupied states within a set distance into an aggregated obstacle includes aggregating grid units in a plurality of occupied states whose mutual distance is less than the vehicle width into an aggregated obstacle.

[0024] Furthermore, step S30 specifically includes:

[0025] Step S31: For a vehicle equivalent to an equivalent circle, if the center of the equivalent circle is within the expanded aggregated obstacle, and this is only applied to search or sampling planners, a collision is determined to have occurred, and the obstacle collision detection result for the entire path is directly output, and the collision detection process ends. Otherwise, no collision is determined, and the process goes to step S50.

[0026] If applied to an optimization planner, when the center of the equivalent circle is inside the expanded aggregate obstacle, ray tracing is required to generate the target cost function when the center of the circle is inside the expanded aggregate obstacle. Specifically, ray tracing is performed along the vertical left and vertical right directions of the heading angle until the boundary of the expanded aggregate obstacle is found. Let the distance between the center of the circle and the boundary of the expanded aggregate obstacle in the vertical left direction be L, the distance between the center of the circle and the boundary of the expanded aggregate obstacle in the vertical right direction be R, and the anti-collision protection distance be D. Then the target cost function when the center of the circle is inside the expanded aggregate obstacle is defined as: [(L+D)×(R+D)] 2 , go to step S50;

[0027] Otherwise, go to step S40 to construct a target cost function when the circle center is outside the expanded aggregated obstacle;

[0028] Step S32: For a vehicle that is equivalent to multiple equivalent circles, if the center of any of the equivalent circles is within the expanded aggregated obstacle, and this is only applied to search or sampling planners, a collision is determined to have occurred, and the obstacle collision detection result for the entire path is directly output, and the collision detection process ends. Otherwise, no collision is determined, and the process proceeds to step S50.

[0029] If applied to an optimization planner, for each circle center located within the expanded aggregate obstacle, use the ray tracing algorithm to perform ray tracing along the vertical left direction and vertical right direction of the heading angle until the boundary of the expanded aggregate obstacle is found; let the distance between the i-th circle center in the vertical left direction and the boundary of the expanded aggregate obstacle be L i , the distance from the boundary of the expanded aggregate obstacle in the vertical right direction is R i , the anti-collision protection distance is D, then the target cost function when the center of the circle is located within the expanded aggregate obstacle is defined as: ∑ i w i *[(L i +)×(R i +)] 2 , where e i is the weight of the i-th circle center located within the expanded aggregated obstacle; if all the circle centers are within the expanded aggregated obstacle, the target cost function calculation at the path node is completed, and the process goes to step S50;

[0030] Otherwise, go to step S40 to construct a target cost function when the center of the circle is outside the expanded aggregated obstacle.

[0031] Furthermore, step S40 specifically includes:

[0032] Step S41: For a vehicle equivalent to an equivalent circle, when the center of the equivalent circle is outside the expanded aggregated obstacle, ray tracing is performed using a ray tracing algorithm along the vertical left and vertical right directions of the heading angle until the boundary of the expanded aggregated obstacle is found. Assuming that the distance between the center of the circle and the boundary of the expanded aggregated obstacle in the vertical left direction is L, the distance between the center of the circle and the boundary of the expanded aggregated obstacle in the vertical right direction is R, and the anti-collision protection distance is D, the target cost function when the center of the circle is outside the expanded aggregated obstacle is defined as: if -(LD)×(RD) is less than 0, then the function is 0; if -(LD)×(RD) is greater than or equal to 0, then the function is [-(LD)×(RD)] 2 ;

[0033] Step S42: For a vehicle that is equivalent to multiple equivalent circles, when the center of any of the equivalent circles is outside the expanded aggregated obstacle, ray tracing is performed using a ray tracing algorithm along the vertical left and vertical right directions of the heading angle for each circle center outside the expanded aggregated obstacle until the boundary of the expanded aggregated obstacle is found; let the distance between the i-th circle center in the vertical left direction and the boundary of the expanded aggregated obstacle be L i , the distance from the boundary of the expanded aggregate obstacle in the vertical right direction is R i, the anti-collision protection distance is D, then the target cost function when the center of the circle is outside the expanded aggregate obstacle is defined as: if -(L i -)×(R i -) is less than 0, the function is 0, if -(L i -)×(R i -) is greater than or equal to 0, then the function is ∑ i w i *[-(L i -)×(R i -)[ 2 , where w i is the weight of the center of the i-th circle outside the expanded aggregate obstacle.

[0034] Furthermore, the ray tracing algorithm adopts the Bresenham algorithm.

[0035] Furthermore, in steps S30 and S40, a maximum tracking distance is pre-set for each tracking direction, i.e., the vertical left direction and the vertical right direction along the heading angle, and ray tracing in each tracking direction is stopped when the tracking distance exceeds the maximum tracking distance.

[0036] The technical solution provided by the embodiment of the present invention has the following beneficial effects:

[0037] 1) For unstructured environments and the common occupancy grid obstacle representation, a collision detection algorithm with O(m+n) complexity is implemented based on ray tracing. This significantly reduces the complexity of collision detection in scenarios with densely distributed obstacles and unconstrained by structured road conditions.

[0038] 2) For optimization-type planners, a target cost function with continuous gradient at obstacle boundaries is defined. By tracing rays perpendicular to the left and right sides of the path node's heading angle, the planner supports step-by-step optimization iterations, allowing the path node to gradually move away from obstacles without changing the heading angle. Compared with optimization-type planning algorithms such as TEB, the number of optimized constraints or cost functions is reduced from O(mn) to O(m+n). BRIEF DESCRIPTION OF THE DRAWINGS

[0039] Figure 1 Flowchart of a method in an embodiment of the present invention.

[0040] Figure 2 Schematic diagram of ray tracing of a vehicle equivalent to multiple equivalent circles in an embodiment of the present invention. DETAILED DESCRIPTION

[0041] In order to make the purpose, technical solutions and advantages of the present invention more clearly understood, the present invention will be further described in detail below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present invention and are not intended to limit the present invention.

[0042] In the description of the embodiments of the present invention, it should be noted that the terms "center," "upper," "lower," "left," "right," "vertical," "horizontal," "inner," and "outer," etc., indicating orientations or positional relationships, are based on the orientations or positional relationships shown in the accompanying drawings and are intended solely to facilitate the description of the present invention and simplify the description. They do not indicate or imply that the devices or components referred to must have a specific orientation, be constructed, or operate in a specific orientation. Therefore, they should not be construed as limitations on the present invention. Furthermore, the terms "first," "second," and "third" are used for descriptive purposes only and should not be construed as indicating or implying relative importance.

[0043] In the description of the embodiments of the present invention, it should be noted that, unless otherwise expressly specified or limited, the terms "installed," "connected," and "connected" should be understood in a broad sense. For example, they can refer to fixed connections, detachable connections, or integral connections; they can refer to mechanical connections or electrical connections; they can refer to direct connections or indirect connections through an intermediate medium; they can refer to internal connections between two components; and they can refer to wireless connections or wired connections. Those skilled in the art will understand the specific meanings of the above terms in the present invention based on specific circumstances.

[0044] In addition, the technical features involved in the different embodiments of the present invention described below can be combined with each other as long as they do not conflict with each other.

[0045] An embodiment of the present invention provides a collision detection method in an unstructured environment with dense obstacles based on ray tracing, comprising the following steps:

[0046] Step S10, based on the vehicle shape and size, equating the vehicle to an equivalent circle or a shape model of multiple equivalent circles distributed along the center line of the vehicle length direction; determining the center of the equivalent circle;

[0047] Specifically, for a small vehicle, the vehicle is equivalent to an equivalent circle, and determining the center of the equivalent circle includes: using the center of gravity of the vehicle as a reference point of the vehicle, that is, the center of the equivalent circle;

[0048] For large vehicles, the vehicle is equivalent to more than three equivalent circles. The center of the equivalent circle is determined by selecting the center of the rear axle of the vehicle as the vehicle reference point, and in the Cartesian coordinate system, the center coordinates (x i ,y i ) is expressed by formula (1) and formula (2):

[0049]

[0050]

[0051] Among them, (x, y) is the coordinate of the vehicle reference point, θ is the vehicle heading angle, (x i ,y i ) represents the coordinates of the center of the i-th equivalent circle, D F Indicates the distance between the vehicle reference point and the front of the vehicle, D R Indicates the distance between the vehicle reference point and the rear of the vehicle;

[0052] When used for an optimization-class planner, formulas (1) and (2) serve as hard constraints for the optimization-class planner;

[0053] Step S10 is an initialization step, which only needs to be performed once and does not need to be performed every time a collision is detected;

[0054] Step S20: Using an aggregation algorithm, the input occupancy grid map based on the global Cartesian coordinate system is aggregated into a clustered obstacle by aggregating multiple grid cells within a set distance. The clustered obstacle is then expanded by a radius equal to the radius of the vehicle equivalent circle to obtain the expanded clustered obstacle.

[0055] In some embodiments, the aggregation algorithm includes a monotone chain convex hull MCCH algorithm, which aggregates multiple grid units in occupied states within a set distance into a convex polygon through the monotone chain convex hull MCCH algorithm; or, the aggregation algorithm includes a minimum volume closure ellipsoid MVEE algorithm, which aggregates multiple grid units in occupied states within a set distance into an ellipse through the minimum volume closure ellipsoid MVEE algorithm;

[0056] Preferably, aggregating the grid cells in a plurality of occupied states within a set distance into an aggregated obstacle comprises aggregating grid cells in occupied states whose mutual distance is less than the width of the vehicle into an aggregated obstacle;

[0057] It should be noted that aggregating the occupancy grid into convex polygons or ellipses is a general method for reducing the number of obstacles involved in collision detection in scenes with dense obstacles. Even collision detection algorithms with a complexity of O(mn) need to use such methods to reduce the number of obstacles n. Therefore, this application does not increase the complexity.

[0058] Step S30: Based on the heading angle yaw information corresponding to the path node (i.e., the vehicle reference point), collision detection is performed according to the vehicle's shape model at each path node of the planned path, assuming that the heading angle yaw is consistent with the vehicle's heading angle θ; and constructing a target cost function for the optimization planner when the center of the circle is within the expanded aggregated obstacle. This includes:

[0059] Step S31: For a vehicle equivalent to an equivalent circle, if the center of the equivalent circle is within the expanded aggregated obstacle, and this is only applied to search or sampling planners, a collision is determined to have occurred, and the obstacle collision detection result for the entire path is directly output, and the collision detection process ends. Otherwise, no collision is determined, and the process goes to step S50.

[0060] If applied to an optimization planner, when the center of the equivalent circle is within the expanded aggregated obstacle, ray tracing is required to generate the target cost function when the center of the circle is within the expanded aggregated obstacle; so that through step-by-step optimization iterations, the path node gradually shifts to a direction with a smaller distance from the obstacle boundary, and eventually moves away from the obstacle; specifically, ray tracing is performed using a ray tracing algorithm along the vertical left and vertical right directions of the heading angle until the expanded aggregated obstacle boundary is found; assuming that the distance between the center of the circle in the vertical left direction and the expanded aggregated obstacle boundary is L, the distance between the center of the circle in the vertical right direction and the expanded aggregated obstacle boundary is R, and the anti-collision protection distance is D, then the target cost function when the center of the circle is within the expanded aggregated obstacle is defined as: [(L+D)×(R+D)] 2 , go to step S50;

[0061] Otherwise, go to step S40 to construct a target cost function when the circle center is outside the expanded aggregated obstacle;

[0062] In some embodiments, the ray tracing algorithm may employ the Bresenham algorithm;

[0063] Step S32: For a vehicle that is equivalent to multiple equivalent circles, if the center of any of the equivalent circles is within the expanded aggregated obstacle, and this is only applied to search or sampling planners, a collision is determined to have occurred, and the obstacle collision detection result for the entire path is directly output, and the collision detection process ends. Otherwise, no collision is determined, and the process proceeds to step S50.

[0064] If applied to an optimization planner, for each circle center located within the expanded aggregate obstacle, use the ray tracing algorithm to perform ray tracing along the vertical left direction and vertical right direction of the heading angle until the boundary of the expanded aggregate obstacle is found; let the distance between the i-th circle center in the vertical left direction and the boundary of the expanded aggregate obstacle be L i , the distance from the boundary of the expanded aggregate obstacle in the vertical right direction is R i , the anti-collision protection distance is D, then the target cost function when the center of the circle is located within the expanded aggregate obstacle is defined as: ∑ i w i *[(L i +)×(R i +)] 2, where w i is the weight of the i-th circle center located within the expanded aggregated obstacle; if all the circle centers are within the expanded aggregated obstacle, the target cost function calculation at the path node is completed, and the process goes to step S50;

[0065] Otherwise, go to step S40 to construct a target cost function when the circle center is outside the expanded aggregated obstacle;

[0066] In some embodiments, the ray tracing algorithm may employ the Bresenham algorithm;

[0067] Step S40, based on the vehicle's shape model, uses a corresponding ray tracing method to construct a target cost function for the optimization planner when the center of the circle is outside the expanded aggregated obstacle; including:

[0068] Step S41: For a vehicle equivalent to an equivalent circle, when the center of the equivalent circle is outside the expanded aggregated obstacle, ray tracing is performed using a ray tracing algorithm along the vertical left and vertical right directions of the heading angle until the boundary of the expanded aggregated obstacle is found. Assuming that the distance between the center of the circle and the boundary of the expanded aggregated obstacle in the vertical left direction is L, the distance between the center of the circle and the boundary of the expanded aggregated obstacle in the vertical right direction is R, and the anti-collision protection distance is D, the target cost function when the center of the circle is outside the expanded aggregated obstacle is defined as: if -(LD)×(RD) is less than 0, then the function is 0; if -(LD)×(RD) is greater than or equal to 0, then the function is [-(LD)×(RD)] 2 ;

[0069] The basis function is defined as -(LD) × (RD) here. The purpose is to ensure that the target cost function when the center of the circle is outside the expanded aggregate obstacle and the target cost function when the center of the circle is inside the expanded aggregate obstacle maintain gradient continuity at the obstacle boundary. This allows the path node to continue to shift further away from the obstacle after reaching the obstacle boundary through iterative optimization.

[0070] Step S42: For a vehicle that is equivalent to multiple equivalent circles, when the center of any of the equivalent circles is outside the expanded aggregated obstacle, ray tracing is performed using a ray tracing algorithm along the vertical left and vertical right directions of the heading angle for each circle center outside the expanded aggregated obstacle until the boundary of the expanded aggregated obstacle is found; let the distance between the i-th circle center in the vertical left direction and the boundary of the expanded aggregated obstacle be L i , the distance from the boundary of the expanded aggregate obstacle in the vertical right direction is R i , the anti-collision protection distance is D, then the target cost function when the center of the circle is outside the expanded aggregate obstacle is defined as: if -(L i -)×(Ri -) is less than 0, the function is 0, if -(L i -)×(R i -) is greater than or equal to 0, then the function is ∑ i w i *[-(L i -)×(R i -)] 2 , where w i is the weight of the center of the i-th circle outside the expanded aggregate obstacle;

[0071] Step S50 , repeating steps S30 and S40 until the collision detection of all path nodes on the path and the construction of the corresponding target cost function are completed.

[0072] Furthermore, in order to reduce the computational complexity of the ray tracing process, a maximum tracing distance is pre-set for each tracing direction, i.e., the vertical left direction and the vertical right direction along the heading angle. When the ray tracing in each tracing direction exceeds the maximum tracing distance, the ray tracing is stopped.

[0073] Finally, it should be noted that the above specific implementation methods are only used to illustrate the technical solutions of the present invention and are not limiting. Although the present invention has been described in detail with reference to the embodiments, those skilled in the art should understand that the technical solutions of the present invention can be modified or replaced by equivalents without departing from the spirit and scope of the technical solutions of the present invention, which should all be included in the scope of the claims of the present invention.

Claims

1. A collision detection method in an unstructured environment with dense obstacles based on ray tracing, characterized in that: The following steps are involved: Step S10, based on the vehicle shape and size, equating the vehicle to an equivalent circle or a shape model of multiple equivalent circles distributed along the center line of the vehicle length direction; determining the center of the equivalent circle; Step S20: Using an aggregation algorithm, the input occupancy grid map based on the global Cartesian coordinate system is aggregated into a clustered obstacle by aggregating multiple grid cells within a set distance. The clustered obstacle is then expanded by a radius equal to the radius of the vehicle equivalent circle to obtain the expanded clustered obstacle. Step S30: At each path node of the planned path, based on the heading angle yaw information corresponding to the path node, i.e., the vehicle reference point, collision detection is performed according to the vehicle's shape model, assuming that the heading angle yaw is consistent with the vehicle's heading angle θ; and constructing a target cost function for the optimization planner when the center of the circle is within the expanded aggregated obstacle. Step S40, using a corresponding ray tracing method based on the vehicle's shape model, constructing a target cost function for the optimization planner when the center of the circle is outside the expanded aggregated obstacle; Step S50 , repeating steps S30 and S40 until the collision detection of all path nodes on the path and the construction of the corresponding target cost function are completed.

2. The collision detection method in an unstructured environment with dense obstacles based on ray tracing according to claim 1, characterized in that: Step S10 specifically includes: For a small vehicle, the vehicle is equivalent to an equivalent circle, and determining the center of the equivalent circle includes: using the center of gravity of the vehicle as a reference point of the vehicle, that is, the center of the equivalent circle; For large vehicles, the vehicle is equivalent to more than three equivalent circles. The center of the equivalent circle is determined by selecting the center of the rear axle of the vehicle as the vehicle reference point. In the Cartesian coordinate system, the center coordinates (x i ,y i ) is expressed by formula (1) and formula (2): Among them, (x, y) is the coordinate of the vehicle reference point, θ is the vehicle heading angle, (x i ,y i ) represents the coordinates of the center of the i-th equivalent circle, D F Indicates the distance between the vehicle reference point and the front of the vehicle, D R Indicates the distance between the vehicle reference point and the rear of the vehicle; When used for an optimization class planner, equations (1) and (2) serve as hard constraints for the optimization class planner.

3. The collision detection method in an unstructured environment with dense obstacles based on ray tracing according to claim 2, characterized in that: In step S20, the aggregation algorithm includes a monotone chain convex hull MCCH algorithm, which aggregates multiple grid units in occupied states within a set distance into a convex polygon through the monotone chain convex hull MCCH algorithm; or, the aggregation algorithm includes a minimum volume closure ellipsoid MVEE algorithm, which aggregates multiple grid units in occupied states within a set distance into an ellipse through the minimum volume closure ellipsoid MVEE algorithm.

4. The collision detection method in an unstructured environment with dense obstacles based on ray tracing according to claim 2, characterized in that: In step S20 , aggregating a plurality of grid cells in occupied states within a set distance into an aggregated obstacle includes aggregating grid cells in occupied states whose mutual distance is less than the vehicle width into an aggregated obstacle.

5. The collision detection method in an unstructured environment with dense obstacles based on ray tracing according to claim 2, characterized in that: Step S30 specifically includes: Step S31: For a vehicle equivalent to an equivalent circle, if the center of the equivalent circle is within the expanded aggregated obstacle, and this is only applied to search or sampling planners, a collision is determined to have occurred, and the obstacle collision detection result for the entire path is directly output, and the collision detection process ends. Otherwise, no collision is determined, and the process goes to step S50. If applied to an optimization planner, when the center of the equivalent circle is inside the expanded aggregate obstacle, ray tracing is required to generate the target cost function when the center of the circle is inside the expanded aggregate obstacle. Specifically, ray tracing is performed along the vertical left and vertical right directions of the heading angle until the boundary of the expanded aggregate obstacle is found. Let the distance between the center of the circle and the boundary of the expanded aggregate obstacle in the vertical left direction be L, the distance between the center of the circle and the boundary of the expanded aggregate obstacle in the vertical right direction be R, and the anti-collision protection distance be D. Then the target cost function when the center of the circle is inside the expanded aggregate obstacle is defined as: [(L+D)×(R+D)] 2 , go to step S50; Otherwise, go to step S40 to construct a target cost function when the circle center is outside the expanded aggregated obstacle; Step S32: For a vehicle that is equivalent to multiple equivalent circles, if the center of any of the equivalent circles is within the expanded aggregated obstacle, and this is only applied to search or sampling planners, a collision is determined to have occurred, and the obstacle collision detection result for the entire path is directly output, and the collision detection process ends. Otherwise, no collision is determined, and the process proceeds to step S50. If applied to an optimization planner, for each circle center located within the expanded aggregate obstacle, use the ray tracing algorithm to perform ray tracing along the vertical left direction and vertical right direction of the heading angle until the boundary of the expanded aggregate obstacle is found; let the distance between the i-th circle center in the vertical left direction and the boundary of the expanded aggregate obstacle be L i , the distance from the boundary of the expanded aggregate obstacle in the vertical right direction is R i , the anti-collision protection distance is D, then the target cost function when the center of the circle is located within the expanded aggregate obstacle is defined as: ∑ i w i *[(L i +D)×(R i +D)] 2 , where w i is the weight of the i-th circle center located within the expanded aggregated obstacle; if all the circle centers are within the expanded aggregated obstacle, the target cost function calculation at the path node is completed, and the process goes to step S50; Otherwise, go to step S40 to construct a target cost function when the center of the circle is outside the expanded aggregated obstacle.

6. The collision detection method in an unstructured environment with dense obstacles based on ray tracing according to claim 5, characterized in that: Step S40 specifically includes: Step S41: For a vehicle equivalent to an equivalent circle, when the center of the equivalent circle is outside the expanded aggregated obstacle, ray tracing is performed using a ray tracing algorithm along the vertical left and vertical right directions of the heading angle until the boundary of the expanded aggregated obstacle is found. Assuming that the distance between the center of the circle and the boundary of the expanded aggregated obstacle in the vertical left direction is L, the distance between the center of the circle and the boundary of the expanded aggregated obstacle in the vertical right direction is R, and the anti-collision protection distance is D, the target cost function when the center of the circle is outside the expanded aggregated obstacle is defined as: if -(LD)×(RD) is less than 0, then the function is 0; if -(LD)×(RD) is greater than or equal to 0, then the function is [-(LD)×(RD)] 2 ; Step S42: For a vehicle that is equivalent to multiple equivalent circles, when the center of any of the equivalent circles is outside the expanded aggregated obstacle, ray tracing is performed using a ray tracing algorithm along the vertical left and vertical right directions of the heading angle for each circle center outside the expanded aggregated obstacle until the boundary of the expanded aggregated obstacle is found; let the distance between the i-th circle center in the vertical left direction and the boundary of the expanded aggregated obstacle be L i , the distance from the boundary of the expanded aggregate obstacle in the vertical right direction is R i , the anti-collision protection distance is D, then the target cost function when the center of the circle is outside the expanded aggregate obstacle is defined as: if -(L i -D)×(R i -D) is less than 0, the function is 0, if -(L i -D)×(R i -D) is greater than or equal to 0, then the function is ∑ i w i *[-(L i -D)×(R i -D)] 2 , where w i is the weight of the center of the i-th circle outside the expanded aggregate obstacle.

7. The collision detection method in an unstructured environment with dense obstacles based on ray tracing according to claim 5 or 6, characterized in that: The ray tracing algorithm adopts the Bresenham algorithm.

8. The collision detection method in an unstructured environment with dense obstacles based on ray tracing according to claim 5 or 6, characterized in that: In steps S30 and S40, a maximum tracking distance is pre-set for each tracking direction, i.e., the vertical left direction and the vertical right direction along the heading angle. Ray tracing in each tracking direction stops when the tracking distance exceeds the maximum tracking distance.

Images