A low-speed unmanned vehicle target heading angle decision method based on graph search

Abstract

The application discloses a low-speed unmanned vehicle target heading angle decision-making method based on graph search, finds an optimal or approximate optimal heading angle in a two-dimensional grid map based on a graph search algorithm, and gives an approximate optimal heading angle through a discrete search mode when there is no effective road edge reference information in an upper layer to guide the advancing direction of the unmanned vehicle. The application has the advantages of high reliability, and can decide an optimal or approximate optimal heading angle for guiding the advancing direction of the unmanned vehicle under the condition that there is no complete upper layer information.

Description

Technical Field

[0001] This invention relates to the field of decision planning in autonomous driving, and particularly to a method for determining the target heading angle of a low-speed autonomous vehicle based on graph search. Background Technology

[0002] With the rapid development of intelligent transportation systems and autonomous driving technology, low-speed unmanned vehicles are becoming increasingly widely used as part of smart cities, especially in cleaning operations on open urban roads. However, ensuring that unmanned vehicles can drive safely and efficiently in complex environments, while also coordinating effectively with human workers, remains a challenge.

[0003] This task involves an unmanned vehicle (UAV) autonomously determining its orientation to reach a planned destination during urban road cleaning operations, based on task requirements and environmental conditions, within a grid map, ensuring automatic following of cleaning personnel. During this following process, the heading angle at the target point must guarantee the safety of both the cleaning personnel and the vehicle, as well as current and future drivability. Currently, there is no solution to this problem. This is the area this application focuses on improving. Summary of the Invention

[0004] The technical problem to be solved by the present invention is to provide a target heading angle decision method for low-speed unmanned vehicles based on graph search, which has high reliability and can determine an optimal or near-optimal heading angle to guide the unmanned vehicle's direction of travel even without complete upper-level information.

[0005] To address the aforementioned technical problems, this invention provides a graph search-based target heading angle decision method for low-speed unmanned vehicles. Based on a graph search algorithm, it searches for an optimal or near-optimal heading angle in a two-dimensional grid map. When there is no effective roadside reference information at the upper level, a near-optimal heading angle is provided through a discretization search to guide the unmanned vehicle's direction of travel. The reference system involved is the vehicle coordinate system, a Cartesian coordinate system fixed to the vehicle, with the vehicle's own position as the origin, the vehicle's forward direction as the positive X-axis, and the leftward direction perpendicular to the vehicle's direction as the positive Y-axis. The heading angle is the angle between the vehicle's longitudinal axis, the line of intersection with the ground, and the geodetic inertial coordinate system.

[0006] The specific steps are as follows:

[0007] Step S0: Initialize the valid reception flags for the location information of cleaning personnel, grid map information, roadside information, and driving target point location information to false;

[0008] Step S1: Obtain the location information of the cleaning staff, grid map information, roadside information, and navigation point location information, and mark the corresponding flags as true;

[0009] Step S2: Check if a valid grid map, the location of the cleaning staff, and the location information of the navigation points have been received;

[0010] If the location of the cleaning staff, the grid map, or the location information flag of the driving target point is false, then return to step S1;

[0011] Step S3: Check whether valid roadside information has been received. If valid roadside information has been received, determine whether the cleaning staff and vehicle are on the same side of the roadside. If no valid roadside information has been received, use the previous judgment result of same side vs. opposite side.

[0012] Step S4: Calculate the reference heading angle;

[0013] If the cleaning staff and the vehicle are on the same side of the curb, the reference heading angle is set as the angle between the line connecting the navigation point and the cleaning staff's position and the X-axis of the vehicle coordinate system.

[0014] If the cleaning staff and vehicles are on opposite sides of the curb, the reference heading angle is set as the angle between the curb line and the X-axis of the vehicle coordinate system.

[0015] Step S5: Perform a passage detection;

[0016] Check whether the navigation point is passable along the direction of the reference heading angle in the grid map. If it is passable, publish the heading angle information through the communication architecture of the vehicle computing platform, and then return to step S0 to make a new decision; if it is not passable, jump to step S6.

[0017] Step S6: Adjust the heading angle and return to step S5 until the passage test is passed; if no heading angle that can pass the passage test is found during the traversal, a decision failure message is published through the communication architecture of the vehicle computing platform, and then the process returns to step S0 to make a new decision.

[0018] This invention utilizes a graph search algorithm to find the optimal or near-optimal heading angle in a discretized state space, thereby guiding the target heading angle decision of an autonomous vehicle. This method not only handles static obstacles but also adapts to changes in dynamic environments, providing autonomous vehicles with a more reliable and flexible navigation strategy.

[0019] The beneficial effects of this invention are:

[0020] 1) This invention provides a target heading angle decision method in low-speed scenarios. This method has high reliability and can determine an optimal or near-optimal heading angle to guide the travel direction of unmanned vehicles without complete upper-level information.

[0021] 2) The heading angle decision method of the present invention takes into account the influence of dynamic and static obstacles on the unmanned vehicle. By adding a passage detection function, it avoids the unmanned vehicle from colliding with obstacles when it reaches the navigation point, thus ensuring the driving safety of the unmanned vehicle. Attached image description:

[0022] The accompanying drawings, which form part of this application, are used to provide a further understanding of the invention. The illustrative embodiments of the invention and their descriptions are used to explain the invention and do not constitute an improper limitation of the invention.

[0023] In the attached diagram:

[0024] Figure 1 This is a flowchart illustrating a specific embodiment of the present invention;

[0025] Figure 2 This is a schematic diagram of a passage detection method according to a specific embodiment of the present invention;

[0026] Figure 3 and Figure 4 This is a diagram showing the heading angle decision results of a real-vehicle experiment according to a specific embodiment of the present invention. Detailed implementation method:

[0027] The embodiments of the present invention will be described in detail below with reference to the accompanying drawings.

[0028] This invention provides a target heading angle decision method for low-speed unmanned vehicles based on graph search, comprising the following specific steps:

[0029] Step S0: Initialize the valid reception flags of the cleaning personnel's location information, grid map information, roadside information, and driving target point (i.e., navigation point) location information to false.

[0030] Step S1: Obtain the location information of the cleaning personnel, grid map information, roadside information, and the location information of the driving target point. The specific steps are as follows:

[0031] Step S11: Obtain the coordinates (x, y) of the cleaning personnel in the vehicle coordinate system through the ROS (Robotics Operating System) topic communication mechanism of the onboard computing platform. p ,y p When valid cleaning personnel location information is received, the corresponding flag is set to true;

[0032] Step S12: Obtain two-dimensional grid map information in the vehicle coordinate system through the ROS topic communication mechanism of the vehicle computing platform. When the grid map information containing complete data content is received, the corresponding flag bit is set to true.

[0033] A grid map is a mapping of the vehicle's environment. It decomposes the space into interconnected but non-overlapping grids. Each grid is classified as occupied, free, or unknown based on the data value it stores. A storage value of 0 represents "free", which is a drivable area without obstacles; a storage value of 100 represents "occupied", which is a drivable area with obstacles; and a storage value of -1 represents "unknown", which is a drivable area with unknown risks.

[0034] Step S13: Obtain roadside information in the vehicle coordinate system through the ROS topic communication mechanism of the vehicle computing platform. When valid roadside information is received, the corresponding flag bit is recorded as true.

[0035] The roadside line is generated by fitting a parametric equation to LiDAR point cloud data. The equation for the straight line is y = kx + b. The equation parameters, including the slope k and the intercept b, are obtained through ROS topic communication.

[0036] Step S14: Obtain the navigation point location information (x0, y0) in the vehicle coordinate system through the ROS topic communication mechanism of the vehicle computing platform. When a valid driving target point location information is received, the corresponding flag is set to true.

[0037] A navigation point is the local planned destination that a vehicle is expected to reach.

[0038] Step S2: Check whether a valid grid map, the location of the cleaning personnel, and the location information of the driving target point have been received;

[0039] If any information flag in the location of the cleaning staff, the grid map, or the location of the driving target point is false, return to step S1.

[0040] Step S3: Check if valid roadside information has been received. If valid roadside information has been received, determine whether the cleaning personnel and the vehicle (i.e., the origin of the vehicle coordinate system) are on the same side of the roadside. The determination method is as follows:

[0041]

[0042] Where: x p y p Given the location of the cleaning personnel in the vehicle coordinate system, if k and b do not receive valid roadside information, the previous judgment result of the same side versus opposite side will be used.

[0043] Step S4: Calculate the reference heading angle;

[0044] If the cleaning staff and vehicle are on the same side of the curb, the reference heading angle is set as the angle between the line connecting the navigation point and the cleaning staff's position and the X-axis of the vehicle's coordinate system. The calculation formula is as follows:

[0045]

[0046] If the cleaning staff and vehicles are on opposite sides of the curb, the reference heading angle is set as the angle between the curb line and the X-axis of the vehicle coordinate system, and the calculation formula is as follows:

[0047] θ = arctan(k).

[0048] Step S5: Passage detection;

[0049] In a 2D raster map, set the size centered on the navigation point location.

[0050] m*m(0<m<m max The search space of m ∈ N, where m max Given the maximum size of the raster map, iterate through all raster cells (x, x) in the search space that are occupied (i.e., obstacles). i y i ), where (i > 0, i ∈ N), and simultaneously calculate the distance d between each occupied grid cell and the grid cell where the navigation point is located. 1i and the distance d from each occupied grid cell to the navigation point along the aforementioned heading angle. 2i The calculation formula is as follows:

[0051]

[0052] Check if the navigation point is passable along the aforementioned heading angle direction. If it exists, satisfy d. 1i ≤d 1threshold &&d 2i ≤d 2threshold If a cell occupies a grid, it is impassable; otherwise, it is passable.

[0053] in:

[0054]

[0055] d 1threshold d 2threshold These are empirical values ​​related to vehicle body dimensions, where the vehicle is simplified here to have a length and width of D. L and D W The rectangle, d safe These are human experience values, generally:

[0056]

[0057] If the route is passable, the heading angle information is published through the communication architecture of the vehicle computing platform; if the route is not passable, proceed to step S6.

[0058] Step S6: Adjust the heading angle for those that fail the passage detection;

[0059] The heading angle is adjusted iteratively based on incremental step size. The heading angle is finely adjusted within a predetermined range of [-180°, 180°]. Each adjustment is made by a fixed step size (±1°). Then, the process returns to step S5 until the passage detection is passed.

[0060] If no heading angle that can pass the passage detection is found during the traversal, a decision failure message is published through the ROS communication mechanism of the vehicle computing platform, and then the process returns to step S0 to make a new decision.

[0061] The passage detection method described in the specific embodiments of the present invention is as follows: Figure 2 As shown, the shaded area on the right is the area on the curb, and the shaded area on the left is the detection area for passage detection. At this time, the cleaning staff and the unmanned vehicle are on opposite sides of the curb. The reference heading angle is set to the angle θ between the curb line and the X-axis of the vehicle coordinate system. However, there are grids occupied by obstacles in the detection area, so the reference heading angle cannot pass the passage detection and needs to be adjusted. Figure 3 and Figure 4 This is a schematic diagram of the actual vehicle test results of a specific embodiment of the present invention. The blue area is the non-drivable area, the black area is the drivable area, the "Target person" is the cleaning personnel to be followed, the "Road boundary" is the fitted roadside line, and the red arrow in the middle is the heading angle decision result. Figure 3 The cleaning staff and the unmanned vehicle are on opposite sides of the curb. The reference heading angle is set as the angle between the curb line and the X-axis of the vehicle coordinate system, and the reference heading angle passes the passage detection. Figure 4 The cleaning staff and the unmanned vehicle were on the same side of the curb. The reference heading angle was set as the angle between the curb line and the X-axis of the vehicle coordinate system, and the reference heading angle passed the passage detection.

[0062] The above description is merely a preferred embodiment of the present invention and is not intended to limit the invention. Various modifications and variations can be made to the present invention by those skilled in the art. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the scope of protection of the present invention.

Claims

1. A target heading angle decision method for low-speed unmanned vehicles based on graph search, comprising the following steps: Step S0: Initialize the valid reception flags for the location information of cleaning personnel, grid map information, roadside information, and navigation point location information to false; Step S1: Obtain the location information of the cleaning staff, grid map information, roadside information, and navigation point location information, and mark the corresponding flags as true; Step S2: Check if a valid grid map, the location of the cleaning staff, and the location information of the navigation points have been received; If the location flag of the cleaning staff, the grid map, or the navigation point location information is false, return to step S1; Step S3: Check if valid roadside information has been received. If valid roadside information has been received, then determine whether the cleaning personnel and vehicles are on the same side of the roadside. If no valid roadside information is received, the previous judgment result for the same side versus the opposite side will be used. Step S4: Calculate the reference heading angle; If the cleaning staff and the vehicle are on the same side of the curb, the reference heading angle is set as the angle between the line connecting the navigation point and the position of the cleaning staff and the X-axis of the vehicle coordinate system. If the cleaning staff and vehicles are on opposite sides of the curb, the reference heading angle is set as the angle between the curb line and the X-axis of the vehicle coordinate system. Step S5: Perform a passage detection; Check whether the navigation point is passable along the direction of the reference heading angle in the grid map. If it is passable, publish the heading angle information through the communication architecture of the vehicle computing platform, and then return to step S0 to make a new decision. If it is impassable, proceed to step S6; Checking whether a waypoint is passable along the reference heading angle in a grid map specifically includes: setting a size centered on the waypoint location in the 2D grid map. The search space, in which Given the maximum size of the raster map, iterate through all raster cells in the search space that are currently occupied. ,in Simultaneously calculate the distance between each occupied grid cell and the grid cell where the navigation point is located. and the distance from each occupied grid cell to the navigation point along the aforementioned heading angle on a straight line. The calculation formula is as follows: ； Check if the navigation point is passable along the aforementioned heading angle. If it is, then... and If a cell occupies a grid, it is impassable; otherwise, it is passable. in: ； These are empirical values ​​related to vehicle body dimensions; the vehicle here is simplified to a length and width of [missing values]. and A rectangle, These are human experience values, and are: ； Step S6: After adjusting the heading angle, return to step S5 until the passage detection is passed; if no heading angle that can pass the passage detection is found during the traversal, a decision failure message is published through the communication architecture of the vehicle computing platform, and then return to step S0 to make a new decision.

2. The low-speed unmanned vehicle target heading angle decision method based on graph search according to claim 1, characterized in that: In step S3, it is determined whether the cleaning personnel and the vehicle are on the same side of the roadside. The determination method is as follows: ； in: These are the coordinates of the cleaning personnel in the vehicle coordinate system. Equation of the straight line along the road The slope, The intercept is used; if no valid roadside information is received, the previous judgment result for the same side versus opposite side is used.

3. The low-speed unmanned vehicle target heading angle decision method based on graph search according to claim 1, characterized in that: In step S4, if the cleaning staff and the vehicle are on the same side of the curb, the reference heading angle is set as the angle between the line connecting the navigation point and the cleaning staff's position and the X-axis of the vehicle coordinate system. The calculation formula is as follows: 。 4. The low-speed unmanned vehicle target heading angle decision method based on graph search according to claim 1, characterized in that: In step S4, if the cleaning personnel and the vehicle are on opposite sides of the curb, the reference heading angle is set as the angle between the curb line and the X-axis of the vehicle coordinate system, and the calculation formula is as follows: 。 5. The target heading angle decision method for low-speed unmanned vehicles based on graph search according to claim 1, characterized in that: Step S6 involves iteratively adjusting the heading angle based on incremental step size, within a predetermined range. The heading angle is finely adjusted in one fixed step of ±1° each time, and then the process returns to step S5 until the passage detection is passed.

6. A computer system comprising a memory, a processor, and a computer program stored in the memory, characterized in that: The processor executes the computer program to implement the steps of the target heading angle decision method according to any one of claims 1-5.

7. A computer-readable storage medium having a computer program stored thereon, characterized in that: When executed by a processor, the computer program implements the steps of the target heading angle decision method according to any one of claims 1-5.

8. A computer program product, comprising a computer program, characterized in that: When executed by a processor, the computer program implements the steps of the target heading angle decision method according to any one of claims 1-5.

Images