Static obstacle avoidance method, system, equipment and medium

Abstract

The invention provides a static obstacle avoidance method, system and device and a medium, and the method comprises the steps: obtaining a first coordinate of a vehicle and a second coordinate of an obstacle, and converting the first coordinate into a third coordinate and converting the second coordinate into a fourth coordinate when the obstacle is judged to be a static obstacle; performing path planning under a Frenet coordinate system based on an improved A star algorithm to obtain a rough solution path; smoothing the rough solution path based on an adaptive pure tracking planning algorithm to obtain a smooth path; whether the vehicle collides with the obstacle passing through the smooth path or not is judged, and if not, the smooth path serves as a planned path; and if yes, adjusting the smooth path based on a collision adjustment algorithm, and taking the adjusted smooth path as a planned path until the vehicle passes through the obstacle and does not collide. The algorithm provided by the invention has a faster path planning speed.

Description

Technical Field

[0001] The present disclosure generally relates to the field of autonomous driving planning technology, and more particularly to a static obstacle avoidance method, system, device and medium. Background Art

[0002] In the park autonomous driving, the motion planning module is an important part. Most of them are structured roads. Motion planning on structured roads is basically equivalent to the autonomous driving planning task in urban environments. As a key component of the autonomous driving system, the motion planning module's main task is to generate a safe, efficient and comfortable drivable trajectory based on map, perception and positioning information. Currently, smart cars have been implemented in low-speed and simple road scenarios, but in some specific environments, some existing motion planning algorithms cannot complete the planning task well.

[0003] Although the current autonomous driving motion planning algorithms are very mature, some special scenarios still cannot meet the requirements of motion planning. In the case of many static obstacles, common planning algorithms have certain defects. For example, when using numerical optimization methods to plan in the Frenet coordinate system, the time used to find a rough solution using a dynamic programming algorithm is very slow, which greatly affects the real-time performance of planning and thus affects the safety of autonomous driving. Summary of the invention

[0004] In view of the above-mentioned defects or deficiencies in the prior art, it is desirable to provide a static obstacle avoidance method, system, device and medium to solve the above-mentioned problems.

[0005] The first aspect of the present application provides a static obstacle avoidance method, comprising: Acquire a first coordinate of the vehicle and a second coordinate of the obstacle, and when determining that the obstacle is a static obstacle, convert the first coordinate into a third coordinate, and convert the second coordinate into a fourth coordinate; the first coordinate and the second coordinate are coordinates in a Cartesian coordinate system, and the third coordinate and the fourth coordinate are coordinates in a Frenet coordinate system; Based on the improved A-star algorithm, path planning is performed in the Frenet coordinate system to obtain a rough solution path; Smoothing the rough solution path based on an adaptive pure tracking planning algorithm to obtain a smooth path; Determine whether the vehicle collides with the obstacle when passing through the smooth path. If not, use the smooth path as the planned path; if so, adjust the smooth path based on a collision adjustment algorithm until the vehicle passes through the obstacle without colliding, and use the adjusted smooth path as the planned path.

[0006] According to the technical solution provided in the embodiment of the present application, the path planning is performed in the Frenet coordinate system based on the improved A-star algorithm to obtain a rough solution path, including: Determine a plurality of obstacle jump points around the fourth coordinate based on the Frenet coordinate system, and determine a plurality of obstacle jump point auxiliary points between two adjacent obstacle jump points; Searching the obstacle jump point auxiliary point by the third coordinate based on the A-star algorithm, and when it is determined that the obstacle jump point auxiliary point is found, jumping to the obstacle jump point according to the obstacle jump point auxiliary point; The search is continued with the obstacle jump point as a reference until the target point is found, thereby obtaining the rough solution path.

[0007] According to the technical solution provided in the embodiment of the present application, the step of jumping to the obstacle jump point according to the obstacle jump point auxiliary point includes: Determine an obstacle jump point connection point on the path between the third coordinate and the searched obstacle jump point auxiliary point, wherein the distance between the obstacle jump point connection point and the searched obstacle jump point auxiliary point is equal to the distance between the searched obstacle jump point auxiliary point and the obstacle jump point to be jumped to; The obstacle jump point connection point jumps to the obstacle jump point, and the path between the obstacle jump point connection point and the obstacle jump point is a partial path of the rough solution path.

[0008] According to the technical solution provided in the embodiment of the present application, before smoothing the rough solution path based on the adaptive pure tracking planning algorithm, the method further includes: Converting the coordinates of the points on the rough solution path from the Frenet coordinate system to the Cartesian coordinate system; The method of smoothing the rough solution path based on the adaptive pure tracking planning algorithm to obtain a smooth path includes: Building a virtual chassis of the vehicle, setting a tracking speed and a tracking time; Determine each point in the rough solution path as a tracking point, select one of the tracking points, and track from the first coordinate to the selected tracking point; Calculating the steering angle of the virtual chassis according to a first angle and a first distance, wherein the first angle is an angle between the selected tracking point and the obstacle when the position of the vehicle is taken as a vertex, and the first distance is a distance between the first coordinate and the selected tracking point; Calculate the actual path point coordinates according to the first coordinates, the tracking time, the tracking speed and the steering angle; Using the actual path point coordinates as a starting point, continue tracking toward the next tracking point until the actual path point coordinates and the steering angle corresponding to each tracking point are obtained; The smooth path is obtained according to each set of corresponding steering angles and actual path point coordinates.

[0009] According to the technical solution provided in the embodiment of the present application, the adjusting the smooth path based on the collision adjustment algorithm includes: Determine a collision point on the smooth path, and determine a collision conflict segment according to the collision point, wherein the collision conflict segment includes a point on the smooth path that is within a set range from the collision point; Calculating the potential field repulsion of the obstacle according to a potential field method, and determining a first adjustment distance according to the potential field repulsion and a body width of the vehicle; The actual path point on the collision and conflict segment is moved in a direction away from the obstacle by the first adjustment distance to obtain the adjusted smooth path.

[0010] According to the technical solution provided in the embodiment of the present application, after moving the actual path point on the collision conflict segment in the direction away from the obstacle by the first adjustment distance and before obtaining the adjusted smooth path, the method includes: Obtain an edge point, and move the edge point in a direction away from the obstacle by a second adjustment distance; the edge point is the actual path point adjacent to the collision conflict segment forward and backward, and the second adjustment distance is half of the first adjustment distance.

[0011] According to the technical solution provided in the embodiment of the present application, determining the collision conflict segment according to the collision point includes: Obtaining a tracking threshold, and calculating a coordinate difference between each of the actual path points and the collision point; The points whose coordinate difference is less than the tracking threshold are placed in the collision conflict segment.

[0012] A second aspect of the present application provides a static obstacle avoidance system, comprising: an acquisition module, the acquisition module being configured to acquire a first coordinate of the vehicle and a second coordinate of the obstacle, and when it is determined that the obstacle is a static obstacle, converting the first coordinate into a third coordinate, and converting the second coordinate into a fourth coordinate; the first coordinate and the second coordinate are coordinates in a Cartesian coordinate system, and the third coordinate and the fourth coordinate are coordinates in a Frenet coordinate system; A first processing module, wherein the first processing module is configured to perform path planning in a Frenet coordinate system based on an improved A-star algorithm to obtain a rough solution path; A second processing module, the second processing module is configured to smooth the rough solution path based on an adaptive pure tracking planning algorithm to obtain a smooth path; The third processing module is configured to determine whether the vehicle collides with the obstacle when passing through the smooth path. If not, the smooth path is used as the planned path; if so, the smooth path is adjusted based on a collision adjustment algorithm until the vehicle passes through the obstacle without colliding, and the adjusted smooth path is used as the planned path.

[0013] The third aspect of the present application provides a terminal device, including a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor implements the steps of the static obstacle avoidance method as described above when executing the computer program.

[0014] A fourth aspect of the present application provides a computer-readable storage medium, on which a computer program is stored, characterized in that when the computer program is executed by a processor, the steps of the static obstacle avoidance method as described above are implemented.

[0015] Compared with the prior art, the beneficial effects of the present application are: by converting the coordinates of the vehicle and the obstacle from the Cartesian coordinate system to the Frenet coordinate system, and performing path planning based on the improved A-star algorithm in the Frenet coordinate system, the time for obtaining a rough solution path is greatly shortened compared with the traditional A-star algorithm; and then when the rough solution path is smoothed by an adaptive pure tracking planning algorithm, the time for obtaining a smooth path is also greatly reduced; by judging whether a vehicle collides with an obstacle when passing through an obstacle on a smooth path, the reliability of the smooth path can be verified, and when a collision occurs, the smooth path is adjusted by a collision adjustment algorithm to overcome the problem of low reliability of the smooth path. BRIEF DESCRIPTION OF THE DRAWINGS

[0016] Other features, objects and advantages of the present application will become more apparent by reading the detailed description of non-limiting embodiments made with reference to the following drawings: Figure 1 A flowchart of the steps of the static obstacle avoidance method provided in Example 1;. Figure 2 It is a schematic diagram of the position of static obstacles in the Cartesian coordinate system; Figure 3 It is a schematic diagram of the position of static obstacles in the Frenet coordinate system; Figure 4 This is a schematic diagram of the planning principle of the A-star algorithm; Figure 5 To improve the A-star algorithm planning principle diagram; Figure 6 This is a schematic diagram of the principle of the adaptive pure tracking planning algorithm; Figure 7 Overall schematic diagram for avoiding collision; Figure 8 To find the schematic diagram of the collision conflict segment; Fig. 9 Adjust the schematic for collision; Fig.10 Schematic diagram of adjusting collision point using potential field method; Fig.11 A schematic diagram of the structure of a static obstacle avoidance system provided in Example 2; Fig.12 This is a schematic diagram of the structure of the terminal device provided in Example 3.

[0017] Figure numbers: 1. Acquisition module; 2. First processing module; 3. Second processing module; 4. Third processing module; 500. Computer system; 501. CPU; 502. ROM; 503. RAM; 504. Bus; 505. I / O interface; 506. Input part; 507. Output part; 508. Storage part; 509. Communication part; 510. Drive; 511. Removable medium. DETAILED DESCRIPTION

[0018] The present application will be further described in detail below in conjunction with the accompanying drawings and embodiments. It is to be understood that the specific embodiments described herein are only used to explain the relevant inventions, rather than to limit the inventions. It should also be noted that, for ease of description, only the parts related to the invention are shown in the accompanying drawings.

[0019] It should be noted that, in the absence of conflict, the embodiments and features in the embodiments of the present application can be combined with each other. The present application will be described in detail below with reference to the accompanying drawings and in combination with the embodiments.

[0020] Example 1 Please refer to Figure 1-Figure 10 , the present application provides a static obstacle avoidance method, comprising: S100, obtaining a first coordinate of the vehicle and a second coordinate of the obstacle, and when determining that the obstacle is a static obstacle, converting the first coordinate into a third coordinate, and converting the second coordinate into a fourth coordinate; the first coordinate and the second coordinate are coordinates in a Cartesian coordinate system, and the third coordinate and the fourth coordinate are coordinates in a Frenet coordinate system; S200, performing path planning in the Frenet coordinate system based on the improved A-star algorithm to obtain a rough solution path; S300, smoothing the rough solution path based on an adaptive pure tracking planning algorithm to obtain a smooth path; S400, determining whether a collision occurs when the vehicle passes through the obstacle along the smooth path; if not, using the smooth path as the planned path; if so, adjusting the smooth path based on a collision adjustment algorithm until the vehicle passes through the obstacle without a collision, and using the adjusted smooth path as the planned path.

[0021] Specifically, one of the technical difficulties of path planning is that it is difficult to express the relative position between the vehicle and the road during the planning process, resulting in an unclear relative relationship between the two. Therefore, the path planned by the traditional planning algorithm in the Cartesian coordinate system has a good effect on open roads, but for highway environments, ignoring lane information leads to an increase in the degree of freedom of the path and is prone to violating road traffic regulations. The Frenet coordinate system is established based on the reference trajectory. In this coordinate system, any point can be regarded as a linear combination of a horizontal unit vector and a longitudinal unit vector. Therefore, trajectory planning in the Frenet coordinate system has a natural advantage.

[0022] Step S100 includes: firstly, obtaining the coordinates of the vehicle and the obstacle in the Cartesian coordinate system, that is, the first coordinate and the second coordinate. Since the Frenet coordinate system is more convenient for representing the relative position between the vehicle and the obstacle, the global path is used as a reference path. A corresponding path is intercepted from the global reference path according to the position of the vehicle as a reference path for local planning. Then, the reference path is converted to the Frenet coordinate system to establish the following: Figure 3 SL diagram shown; according to Figure 4 Scattered sampling is performed, and then the upper and lower boundaries of the scattered points are determined according to the road boundaries provided by the global map. The upper and lower boundaries are boundary lines that are two-thirds of the vehicle body width away from the road boundaries to ensure that the vehicle will not collide with the edge of the road when driving along the planned path. At the same time, the first coordinate system and the second coordinate system in the Cartesian coordinate system are transformed to obtain the third coordinate and the fourth coordinate in the Frenet coordinate system respectively. The transformation relationship between the Cartesian coordinate system and the Frenet coordinate system refers to the following formula (I) and formula (II): Formula (I) In the formula, Indicates the horizontal coordinate of the point to be converted in the Frenet coordinate system. Indicates the value on the reference line corresponding to the trajectory point to be converted. Indicates the ordinate of the point to be converted in the Frenet coordinate system. Indicates the horizontal coordinate of the point to be converted in the Cartesian coordinate system. Indicates the ordinate of the point to be converted in the Cartesian coordinate system. Indicates the x value of the point on the reference line corresponding to the required trajectory point in the Cartesian coordinate system, Indicates the y value of the point on the reference line corresponding to the required trajectory point in the Cartesian coordinate system, Indicates the heading angle of the point on the reference line corresponding to the required trajectory point in the Cartesian coordinate system; Formula (II).

[0023] Step S200 includes: after the vehicle, obstacle and road edge are represented in the SL graph, a rough solution path to avoid the obstacle is found by combining the A-star algorithm and the obstacle jump point. The A-star algorithm uses three main functions: , and To evaluate the quality of the path, From the starting point to any node The actual path cost, whenever moving from the starting point to a new node via the path, The movement cost to reach the node is accumulated. For example, if the movement cost per step is 1, then: Formula (III); Represents a slave node The estimated cost to reach the destination, usually from the node Straight-line distance to the target, heuristic function The definition of , for a point in the grid , if the target point is , a commonly used heuristic is the Euclidean distance: Formula (IV); Representation Node The total cost estimate, combined with the above , Two values, get To evaluate the total cost of the nodes to decide which nodes should be given priority, Formula (V); Since the SL coordinate system is used, the direction from the starting point to the target point is always from left to right, so it is meaningless to search from the back of the current point. In order to reduce unnecessary searches, the points searched backward are pruned and the points behind are not searched, thereby reducing the search time. At the same time, in order to improve the path searched by the A-star algorithm during obstacle avoidance, it can provide a smoother rough solution path for later path smoothing. By defining obstacle jump points, useless search points are reduced, the search efficiency is improved, and the probability of collision after smoothing is greatly reduced.

[0024] The method of using the improved A-star algorithm and the obstacle jump point combination method to obtain a rough solution is as follows: S210, determining a plurality of obstacle jump points around the fourth coordinate based on the Frenet coordinate system, and determining a plurality of obstacle jump point auxiliary points between two adjacent obstacle jump points; S220, searching the obstacle jump point auxiliary point by the third coordinate based on the A-star algorithm, and when it is determined that the obstacle jump point auxiliary point is found, jumping to the obstacle jump point according to the obstacle jump point auxiliary point; S230: Continue searching with the obstacle jump point as a reference until the target point is found, thereby obtaining the rough solution path.

[0025] This embodiment defines an obstacle jump point and an obstacle jump point connection point. Figure 5 , expand the obstacle into a rectangular area with a length along the S axis and a width along the L axis, and define the four corner positions of the points on the outer periphery of the long direction area as the obstacle jump points, such as Figure 5 As shown by the black triangle point in the figure; the point between two adjacent obstacle jump points is defined as the obstacle jump point auxiliary point, such as Figure 5 As shown by the black square point in the figure, the role of this obstacle jump point auxiliary point is that when the A-star algorithm searches for the obstacle jump point auxiliary point, the obstacle jump point auxiliary point directly helps the algorithm jump to the obstacle jump point, thereby avoiding the obstacle. The obstacle jump point is also determined according to the direction of the obstacle jump point. When the obstacle is on the left half of the lane, the obstacle direction is to the right, that is, the obstacle jump point is to the right of the obstacle jump point auxiliary point, that is, Figure 5 In this case; when the obstacle is on the right side of the lane, the obstacle jump point direction is left, then the obstacle jump point is to the left of the obstacle jump point auxiliary point. The function of this point is to ensure that the vehicle can avoid the obstacle in a safer direction.

[0026] If only the obstacle jumping point auxiliary point jumps to the obstacle jumping point, the path may not be smooth enough. Therefore, in step S220, jumping to the obstacle jumping point according to the obstacle jumping point auxiliary point specifically includes: S221, determining an obstacle jump point connection point on the path between the third coordinate and the searched obstacle jump point auxiliary point, wherein the distance between the obstacle jump point connection point and the searched obstacle jump point auxiliary point is equal to the distance between the searched obstacle jump point auxiliary point and the obstacle jump point to be jumped to; S222: Jump from the obstacle jump point connection point to the obstacle jump point, where the path between the obstacle jump point connection point and the obstacle jump point is a partial path of the rough solution path.

[0027] This embodiment further defines an obstacle jump point connection point, which is determined based on the obstacle jump point auxiliary point and the obstacle jump point. The purpose of the obstacle jump point connection point is to improve the smoothness of the path. When the obstacle jump point auxiliary point is determined when the vehicle approaches the obstacle, the obstacle jump point connection point is determined based on the distance between the obstacle jump point auxiliary point and the obstacle jump point at a position on the path where the vehicle approaches the obstacle and is at the same distance from the obstacle jump point auxiliary point, such as Figure 5 As shown by the white triangle point in the figure, the purpose of smoothing the path can be achieved by changing the path from the obstacle jump point connection point to the obstacle jump point; the greater the distance between the obstacle jump point auxiliary point and the obstacle jump point, and the greater the distance between the obstacle jump point connection point and the obstacle jump point auxiliary point, the higher the path smoothness will be.

[0028] The improved A-star algorithm combined with the jump point method is used to quickly find a passable rough solution path to speed up the solution speed. However, the rough solution path is not the local optimum, so when the solution is used as the solution space, it may not be a convex space. If the problem is converted into a quadratic programming problem to find the optimal solution, it may lead to no solution. Therefore, this embodiment uses an adaptive pure tracking planning algorithm to solve the problem. First, the coordinates of the points on the rough solution path obtained in step S230 are converted from the Frenet coordinate system to the Cartesian coordinate system, and then step S300 is executed. Step S300 specifically includes: S310, constructing a virtual chassis of the vehicle, and setting a tracking speed and a tracking time; S320, determining each point in the rough solution path as a tracking point, selecting one of the tracking points, and tracking from the first coordinate to the selected tracking point; S330, calculating the steering angle of the virtual chassis according to a first angle and a first distance, wherein the first angle is an angle between the selected tracking point and the obstacle when the position of the vehicle is taken as a vertex, and the first distance is a distance between the first coordinate and the selected tracking point; S340, calculating actual path point coordinates according to the first coordinates, the tracking time, the tracking speed and the steering angle; S350, continuing to track the next tracking point with the actual path point coordinates as the starting point, until the actual path point coordinates and the steering angle corresponding to each tracking point are obtained; S360: Obtain the smooth path according to each group of corresponding steering angles and actual path point coordinates.

[0029] The pure tracking planning algorithm uses a controller to drive the virtual chassis to obtain a tracking path, and uses the tracked path as the planning path, which can meet the planned path in kinematics so that the vehicle can meet the kinematic feasibility when tracking. The path planning using the pure tracking planning algorithm is to establish a virtual chassis, use the virtual chassis to simulate the movement of the vehicle, and plan the trajectory that the vehicle can track while meeting the kinematic requirements. The main steps are to establish a virtual chassis, use the positioning module to determine the initial position and posture, and use the rough solution path obtained in step S230 as the input of the pure tracking planning algorithm. The points on the rough solution path are used as the tracking points to track the virtual chassis. The algorithm determines the front wheel rotation angle required by the vehicle, that is, the steering angle, by the current posture position of the vehicle and the relative position with the points on the rough solution path that need to be tracked. In order to reduce the amount of calculation, the algorithm uses a determined tracking speed for tracking. The specific tracking speed is determined according to the speed limit on the road in the map. Assuming that the speed limit on a road is 5m / s, the algorithm tracks the planned trajectory at a speed of 5m / s, which can meet the tracking of vehicles with a speed below 5m / s. Here, 5m / s is the tracking speed .

[0030] Principle reference of adaptive pure tracking planning algorithm Figure 6 , the pure tracking planning algorithm controller expects the ego vehicle to follow a circular path segment consisting of Point Arrival Click here Point is the location of the vehicle. Point is the next tracking point selected. is the distance from the current position of the vehicle to the next tracking point, that is, the first distance, also called the foresight distance, which is a specified parameter. The algorithm determines the first tracking point on the rough solution path according to the foresight distance, that is, point; confirmed After the point, the steering angle profile can be derived ,according to Figure 6 The geometric relationship shown above shows the arc radius Satisfying formula (six): Formula (VI) In the formula, refer to Figure 6 , That is, the first angle; According to formula (six), formula (seven) is obtained: Formula (VII) Assumptions , we get formula (eight): Formula (8) Therefore, we get formula (IX): Formula (IX) In the formula, refer to Figure 6 ; In formula (9), It is the steering angle required in the adaptive pure tracking planning algorithm to track the vehicle from its current position to the next tracking point. The steering angle is used as the input of the adaptive pure tracking planning algorithm to solve the optimal steering angle, and then converted into an optimization problem to solve the optimal wheel steering angle to reach point D. When simulating the motion of the virtual chassis, the adaptive pure tracking controller The control requirements are updated every 1 second. However, the obtained smooth path is not continuous in curvature, because the continuity of curvature is directly related to the continuity of the steering angle, which changes every 1 second. In order to ensure that the smooth path obtained is curvature continuous, assume that the previous steering angle command is , and in There are new steering angle commands , requiring the virtual chassis to be During the response, the angle changes from arrive Continuous and rapid changes, which means is the solution to the following optimal control problem, as shown in formula (10): Formula (10) The analytical solution of formula (10) can be easily found by Pontryagin's maximum distance, Not necessarily equal to ,but We will do our best to Previously reached , useful and the tracking speed , which can simulate the virtual chassis in During the movement, the virtual chassis The actual path point coordinates and the corresponding steering angles are obtained and recorded, and after all the tracking points are tracked, a smooth path that meets the kinematic requirements can be obtained.

[0031] The solution speed of the planning path obtained by using the tracking algorithm is very fast, but the disadvantage of the algorithm is that if the vehicle is not conservatively expanded for collision when obtaining the rough solution, collision problems may occur when the tracking algorithm is used for planning. This requires a collision adjustment algorithm to avoid collision. This embodiment provides a collision adjustment algorithm to push away the path points where collision occurs, and then smooth it to obtain a path without collision. The specific method includes: S410, determining a collision point on the smooth path, and determining a collision conflict segment according to the collision point, wherein the collision conflict segment includes a point on the smooth path that is within a set range from the collision point; S420, calculating the potential field repulsion of the obstacle according to a potential field method, and determining a first adjustment distance according to the potential field repulsion and a body width of the vehicle; S430: Move the actual path point on the collision segment by the first adjustment distance in a direction away from the obstacle to obtain the adjusted smooth path.

[0032] The input of the collision detection is the smooth path planned by the adaptive pure tracking planning algorithm. Steps S410-S430 are specifically as follows: Number each point of the smooth path and simulate the vehicle to perform collision detection at each point, such as Figure 7 As shown, the detection point is used as the center to find the collision local line segment. If only one point is adjusted during collision adjustment, the curvature will change too much and the effect will not be obvious. Therefore, a collision conflict segment should be determined with the collision point as the center. The determination of the collision conflict segment according to the collision point includes: S411, obtaining a tracking threshold, and calculating a coordinate difference between each actual path point and the collision point; S412: Put the points whose coordinate difference is less than the tracking threshold into the collision conflict segment.

[0033] Specifically, the collision line segment is selected using formula (XI): Formula (XI) Formula (12) like Figure 7The line segment enclosed by the dotted circle in is the line segment that needs to be adjusted for collision. Formula (XI) searches for the collision line segment from the collision point in the direction of the vehicle's advance, and formula (XII) searches for the collision line segment from the collision point in the opposite direction of the vehicle's travel. In addition, there will be a situation where two collision points are relatively close. If the directions of several collision points are the same and the distances are relatively close, these collision points and collision line segments can be merged into a local collision line segment. If the collision directions are opposite, a point at an intermediate distance between the two points is intercepted as the dividing point to form two local collision segments.

[0034] like Figure 8 As shown, in the event of a collision, represents the coordinates of the path point where the collision occurred in the smooth path, and It represents the forward tracking threshold for searching for points related to the collision point. Once a collision occurs, all points on the smooth path whose distance from the collision point is less than the forward tracking threshold are placed in the collision conflict segment. Specifically, Figure 8 The point in the curly brackets below the forward threshold in is the collision segment backward from the collision point determined by the forward tracking threshold and formula (XI). It represents the backward tracking distance, that is, the backward collision conflict segment related to the collision point is calculated through this value and formula (XII). The forward and backward collision conflict segments together are the collision conflict segments that need to be determined based on the collision conflict point.

[0035] In order to adjust the collision point so that the vehicle does not collide when it drives to that point, it is necessary to adjust the position of the collision point. However, experience tells us that simply adjusting the collision point is not enough to avoid obstacles. This requires us to adjust some points before and after the collision in advance. In formula (XI), It represents the distance from the collision point The previous number is The calculation starts from the point Numbered points, these numbered points are The distance is less than the backward tracking distance The value of; in formula (XII) It represents the distance from the collision point The previous number is The calculation starts from the point Numbered points, these numbered points are The distance is less than the backward tracking distance The value of , and then put these numbered points into the collision conflict segment, so that a complete collision conflict segment is obtained.

[0036] The principle of the collision adjustment algorithm provided in this embodiment is to imitate the drone planning algorithm egoplanner, push away a certain distance according to the direction of the obstacle potential field, then combine the processed conflict collision segments and the path points without collision, and then use the adaptive pure tracking planning algorithm for smoothing to obtain an adjusted smooth path without collision. The specific adjustment principle of the algorithm is as follows: The obstacles are determined by the repulsive force in the artificial potential field method. The points on the path where the obstacles collide need to be determined as follows: Fig. 9 The obstacle potential field is used to calculate the distance required for movement. The factor that determines the obstacle repulsion potential field is the distance between the vehicle and the obstacle. When the vehicle does not enter the obstacle's influence range, the potential energy value it receives is zero. After the vehicle enters the obstacle's influence range, the greater the distance between the two, the greater the potential energy value received by the vehicle.

[0037] The potential field function of the repulsive potential field is shown in the following formula (13): Formula (13) In the formula, is a positive proportionality coefficient, is a vector whose direction is from the obstacle to the vehicle and whose magnitude is the Euclidean distance between the vehicle and the obstacle , is a constant, indicating the maximum range of influence of obstacles on the vehicle. When the vehicle enters the obstacle’s influence range, that is, the relative distance between the vehicle and the obstacle is less than When the vehicle starts to be affected by the repulsive force of the obstacle, the smaller the relative distance between the vehicle and the obstacle, the greater the repulsive force, and the higher its potential energy; the larger the relative distance between the vehicle and the obstacle, the smaller the repulsive force, and the lower its potential energy.

[0038] The potential field repulsion of the corresponding repulsive potential field is shown in the following formula (XIV): Formula (XIV) In the formula, represents the potential field repulsion; When the collision conflict segment is determined by formula (14) Fig.10 The distance of the repulsive potential field represented by When the size is within the range of Figure 7 Between the two dotted circles in The size is three quarters of the vehicle's own width. Different from the existing algorithm, which determines the distance to push the point by by calculating the collision area generated when the collision point on the smooth path collides with the obstacle, connects the area with the distance, and then uses a continuous iteration method to make the path point away from the collision place, this embodiment uses the potential field method to determine the distance to be moved at one time in order to improve efficiency, and then uses an adaptive pure tracking planning algorithm to smooth the path to avoid obstacles.

[0039] The potential field repulsion of formula (14) and the distance from the center of the obstacle define the distance that the collision point needs to move, and we get formula (15): Formula (15) In the formula, Indicates the distance the collision point needs to move. Indicates the width of the vehicle body; When the potential field repulsion The corresponding distance satisfy When the potential field repulsion force is The corresponding distance satisfy When the collision point is moved by half the width of the vehicle body, multiple iterations are avoided, thereby improving efficiency and also improving the safety of avoidance.

[0040] Further, after moving the actual path point on the collision section in a direction away from the obstacle by the first adjustment distance and before obtaining the adjusted smooth path, the method includes: Obtain an edge point, and move the edge point in a direction away from the obstacle by a second adjustment distance; the edge point is the actual path point adjacent to the collision conflict segment forward and backward, and the second adjustment distance is half of the first adjustment distance.

[0041] In order to make the collision-adjusted path smoother, the edge points adjacent to the collision conflict segment need to be pushed out a certain distance. Formula (16) is used to determine the edge points adjacent to the collision conflict segment. The points on the original smooth path and the points pushed away on the collision and conflict segments are transitioned through edge points, making the adjusted path smoother. Fig.10 As shown, is the edge point, and the edge point is pushed away by a certain distance, which is equal to half of the distance of the points adjacent to the edge point on the collision segment, that is, .

[0042] Formula (XVI) in, are the coordinates of the edge points, is the point on the collision segment, It is the code of the collision point on the collision conflict segment. It is necessary to determine The minimum distance yes The index of the corresponding collision point in the collision conflict segment , search forward from this point or search about 5 points to determine .

[0043] By using formula (17), the points of the collision segment are pushed away in a direction perpendicular to the heading angle of the edge point. Formula (XVII) In the formula, Indicates the collision point before adjustment. Indicates the collision point after adjustment. Indicates the heading angle plus 90 degrees.

[0044] All collision points are pushed away from obstacles according to formula (17), and then smoothed using an adaptive pure tracking algorithm to obtain a motion trajectory without collision, namely the planned path.

[0045] Example 2 Please refer to Fig.11 , this embodiment provides a static obstacle avoidance system, including: an acquisition module, the acquisition module being configured to acquire a first coordinate of the vehicle and a second coordinate of the obstacle, and when it is determined that the obstacle is a static obstacle, converting the first coordinate into a third coordinate, and converting the second coordinate into a fourth coordinate; the first coordinate and the second coordinate are coordinates in a Cartesian coordinate system, and the third coordinate and the fourth coordinate are coordinates in a Frenet coordinate system; A first processing module, wherein the first processing module is configured to perform path planning in a Frenet coordinate system based on an improved A-star algorithm to obtain a rough solution path; A second processing module, the second processing module is configured to smooth the rough solution path based on an adaptive pure tracking planning algorithm to obtain a smooth path; The third processing module is configured to determine whether the vehicle collides with the obstacle when passing through the smooth path. If not, the smooth path is used as the planned path; if so, the smooth path is adjusted based on a collision adjustment algorithm until the vehicle passes through the obstacle without colliding, and the adjusted smooth path is used as the planned path.

[0046] Example 3 Please refer to Fig.12 This embodiment provides a terminal device, including a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor implements the steps of the static obstacle avoidance method described in Example 1 when executing the computer program.

[0047] Specifically, the computer system 500 of the terminal device includes a CPU 501, which can perform various appropriate actions and processes according to the program stored in the ROM 502 or the program loaded from the storage part 508 into the RAM 503. In the RAM 503, various programs and data required for system operation are also stored. The CPU 501, the ROM 502 and the RAM 503 are connected to each other through the bus 504. The I / O interface 505 is also connected to the bus 504. The following components are connected to the I / O interface 505: an input part 506 including a keyboard, a mouse, etc.; an output part 507 including a cathode ray tube (CRT), a liquid crystal display (LCD), etc. and a speaker, etc.; a storage part 508 including a hard disk, etc.; and a communication part 509 including a network interface card such as a LAN card, a modem, etc. The communication part 509 performs communication processing via a network such as the Internet. A drive is also connected to the I / O interface 505 as needed. A removable medium 511, such as a magnetic disk, an optical disk, a magneto-optical disk, a semiconductor memory, or the like, is mounted on the drive 510 as needed, so that a computer program read therefrom is installed into the storage section 508 as needed.

[0048] In particular, according to an embodiment of the present invention, the above reference process Figure 1 The described process can be implemented as a computer software program. For example, embodiment 3 of the present invention includes a computer program product, which includes a computer program carried on a computer readable medium, and the computer program contains program code for executing the method shown in the flow chart. In such an embodiment, the computer program can be downloaded and installed from a network through a communication part, and / or installed from a removable medium. When the computer program is executed by CPU501, the above-mentioned functions defined in the present computer system 500 are executed.

[0049] Example 4 This embodiment provides a computer-readable storage medium, on which a computer program is stored, and is characterized in that when the computer program is executed by a processor, the steps of the static obstacle avoidance method as described in Example 1 are implemented.

[0050] It should be noted that the computer-readable medium shown in the present invention may be a computer-readable signal medium or a computer-readable storage medium or any combination of the above two. The computer-readable storage medium may be, for example, but not limited to, an electrical, magnetic, optical, electromagnetic, infrared, or semiconductor system, device or device, or any combination of the above. More specific examples of computer-readable storage media may include, but are not limited to: an electrical connection with one or more wires, a portable computer disk, a hard disk, a random access memory (RAM), a read-only memory (ROM), an erasable programmable read-only memory (EPROM or flash memory), an optical fiber, a portable compact disk read-only memory (CD-ROM), an optical storage device, a magnetic storage device, or any suitable combination of the above. In the present invention, a computer-readable storage medium may be any tangible medium containing or storing a program that can be used by or in combination with an instruction execution system, device or device. In the present invention, a computer-readable signal medium may include a data signal propagated in a baseband or as part of a carrier wave, which carries a computer-readable program code. This propagated data signal may take a variety of forms, including but not limited to electromagnetic signals, optical signals, or any suitable combination of the above. Computer-readable signal media may also be any computer-readable medium other than computer-readable storage media, which may send, propagate or transmit a program for use by or in conjunction with an instruction execution system, apparatus or device. The program code contained on the computer-readable medium may be transmitted using any appropriate medium, including but not limited to: wireless, wire, optical cable, RF, etc., or any suitable combination of the above.

[0051] The flow chart and block diagram in the accompanying drawings illustrate the possible architecture, function and operation of the system, method and computer program product according to the present invention. In this regard, each box in the flow chart or block diagram can represent a module, a program segment, or a part of a code, and the above-mentioned module, program segment, or a part of a code contains one or more executable instructions for realizing the specified logical function. It should also be noted that in some alternative implementations, the functions marked in the box can also occur in a different order from the order marked in the accompanying drawings. For example, two boxes represented in succession can actually be executed substantially in parallel, and they can sometimes be executed in the opposite order, depending on the functions involved. It should also be noted that each box in the block diagram or flow chart, and the combination of the boxes in the block diagram or flow chart can be implemented with a dedicated hardware-based system that performs the specified function or operation, or can be implemented with a combination of dedicated hardware and computer instructions.

[0052] The units involved in the embodiments of the present invention may be implemented by software or hardware, and the units described may also be arranged in a processor. The names of these units do not, in some cases, limit the units themselves. The units or modules described may also be arranged in a processor, for example, they may be described as: an acquisition module 10, a first processing module 20, a second processing module 30, and a third processing module 40. The names of these units or modules do not, in some cases, limit the units or modules themselves, for example, the acquisition module 10 may also be described as "acquiring a first coordinate of a vehicle and a second coordinate of an obstacle, and when judging that the obstacle is a static obstacle, converting the first coordinate into a third coordinate, and converting the second coordinate into a fourth coordinate".

[0053] As another aspect, the present invention further provides a computer-readable medium, which may be included in the electronic device described in the above embodiment; or may exist independently without being assembled into the electronic device. The above computer-readable medium carries one or more programs, and when the above one or more programs are executed by an electronic device, the electronic device implements the static obstacle avoidance method described in the above embodiment 1.

[0054] For example, the electronic device may implement Figure 1 As shown in: S100, obtaining the first coordinate of the vehicle and the second coordinate of the obstacle, and when determining that the obstacle is a static obstacle, converting the first coordinate into a third coordinate, and converting the second coordinate into a fourth coordinate; the first coordinate and the second coordinate are coordinates in a Cartesian coordinate system, and the third coordinate and the fourth coordinate are coordinates in a Frenet coordinate system; S200, performing path planning in a Frenet coordinate system based on an improved A-star algorithm to obtain a rough solution path; S300, smoothing the rough solution path based on an adaptive pure tracking planning algorithm to obtain a smooth path; S400, determining whether the vehicle collides with the obstacle when passing through the smooth path, and if not, using the smooth path as the planned path; if so, adjusting the smooth path based on a collision adjustment algorithm until the vehicle passes through the obstacle without colliding, and using the adjusted smooth path as the planned path It should be noted that, although several modules or units of the device for action execution are mentioned in the above detailed description, this division is not mandatory. In fact, according to the embodiments of the present invention, the features and functions of two or more modules or units described above can be embodied in one module or unit. On the contrary, the features and functions of one module or unit described above can be further divided into being embodied by multiple modules or units.

[0055] In addition, although the steps of the method of the present invention are described in a specific order in the drawings, this does not require or imply that the steps must be performed in this specific order, or that all the steps shown must be performed to achieve the desired results. Additionally or alternatively, some steps may be omitted, multiple steps may be combined into one step, and / or one step may be decomposed into multiple steps, etc.

[0056] Through the description of the above implementations, those skilled in the art can easily understand that the example implementations described here can be implemented by software, or by combining software with necessary hardware.

[0057] The above description is only a preferred embodiment of the present application and an explanation of the technical principles used. Those skilled in the art should understand that the scope of the invention involved in the present application is not limited to the technical solution formed by a specific combination of the above technical features, but should also cover other technical solutions formed by any combination of the above technical features or their equivalent features without departing from the inventive concept. For example, the above features are replaced with the technical features with similar functions disclosed in this application (but not limited to) by each other.

Claims

1. A static obstacle avoidance method, characterized in that: include: Acquire a first coordinate of the vehicle and a second coordinate of the obstacle, and when determining that the obstacle is a static obstacle, convert the first coordinate into a third coordinate, and convert the second coordinate into a fourth coordinate; the first coordinate and the second coordinate are coordinates in a Cartesian coordinate system, and the third coordinate and the fourth coordinate are coordinates in a Frenet coordinate system; Based on the improved A-star algorithm, path planning is performed in the Frenet coordinate system to obtain a rough solution path; Smoothing the rough solution path based on an adaptive pure tracking planning algorithm to obtain a smooth path; Determine whether the vehicle collides with the obstacle when passing through the smooth path. If not, use the smooth path as the planned path; if so, adjust the smooth path based on a collision adjustment algorithm until the vehicle passes through the obstacle without colliding, and use the adjusted smooth path as the planned path.

2. The static obstacle avoidance method according to claim 1, characterized in that: The path planning is performed in the Frenet coordinate system based on the improved A-star algorithm to obtain a rough solution path, including: Determine a plurality of obstacle jump points around the fourth coordinate based on the Frenet coordinate system, and determine a plurality of obstacle jump point auxiliary points between two adjacent obstacle jump points; Searching the obstacle jump point auxiliary point by the third coordinate based on the A-star algorithm, and when it is determined that the obstacle jump point auxiliary point is found, jumping to the obstacle jump point according to the obstacle jump point auxiliary point; The search is continued with the obstacle jump point as a reference until the target point is found, thereby obtaining the rough solution path.

3. The static obstacle avoidance method according to claim 2, characterized in that: The step of jumping to the obstacle jumping point according to the obstacle jumping point auxiliary point comprises: Determine an obstacle jump point connection point on the path between the third coordinate and the searched obstacle jump point auxiliary point, wherein the distance between the obstacle jump point connection point and the searched obstacle jump point auxiliary point is equal to the distance between the searched obstacle jump point auxiliary point and the obstacle jump point to be jumped to; The obstacle jump point connection point jumps to the obstacle jump point, and the path between the obstacle jump point connection point and the obstacle jump point is a partial path of the rough solution path.

4. The static obstacle avoidance method according to claim 3, characterized in that: Before smoothing the rough solution path based on the adaptive pure tracking planning algorithm, the following step is also included: Converting the coordinates of the points on the rough solution path from the Frenet coordinate system to the Cartesian coordinate system; The method of smoothing the rough solution path based on the adaptive pure tracking planning algorithm to obtain a smooth path includes: Building a virtual chassis of the vehicle, setting a tracking speed and a tracking time; Determine each point in the rough solution path as a tracking point, select one of the tracking points, and track from the first coordinate to the selected tracking point; Calculating the steering angle of the virtual chassis according to a first angle and a first distance, wherein the first angle is an angle between the selected tracking point and the obstacle when the position of the vehicle is taken as a vertex, and the first distance is a distance between the first coordinate and the selected tracking point; Calculate the actual path point coordinates according to the first coordinates, the tracking time, the tracking speed and the steering angle; Using the actual path point coordinates as a starting point, continue tracking toward the next tracking point until the actual path point coordinates and the steering angle corresponding to each tracking point are obtained; The smooth path is obtained according to each set of corresponding steering angles and actual path point coordinates.

5. The static obstacle avoidance method according to claim 4, characterized in that: The adjusting the smooth path based on a collision adjustment algorithm comprises: Determine a collision point on the smooth path, and determine a collision conflict segment according to the collision point, wherein the collision conflict segment includes a point on the smooth path that is within a set range from the collision point; Calculating the potential field repulsion of the obstacle according to a potential field method, and determining a first adjustment distance according to the potential field repulsion and a body width of the vehicle; The actual path point on the collision and conflict segment is moved in a direction away from the obstacle by the first adjustment distance to obtain the adjusted smooth path.

6. The static obstacle avoidance method according to claim 5, characterized in that: After moving the actual path point on the collision section in a direction away from the obstacle by the first adjustment distance and before obtaining the adjusted smooth path, the method includes: Obtain an edge point, and move the edge point in a direction away from the obstacle by a second adjustment distance; the edge point is the actual path point adjacent to the collision conflict segment forward and backward, and the second adjustment distance is half of the first adjustment distance.

7. The static obstacle avoidance method according to claim 6, characterized in that: The step of determining the collision conflict segment according to the collision point comprises: Obtaining a tracking threshold, and calculating a coordinate difference between each of the actual path points and the collision point; The points whose coordinate difference is less than the tracking threshold are placed in the collision conflict segment.

8. A static obstacle avoidance system, characterized in that: include: an acquisition module, the acquisition module being configured to acquire a first coordinate of the vehicle and a second coordinate of the obstacle, and when it is determined that the obstacle is a static obstacle, converting the first coordinate into a third coordinate, and converting the second coordinate into a fourth coordinate; the first coordinate and the second coordinate are coordinates in a Cartesian coordinate system, and the third coordinate and the fourth coordinate are coordinates in a Frenet coordinate system; A first processing module, wherein the first processing module is configured to perform path planning in a Frenet coordinate system based on an improved A-star algorithm to obtain a rough solution path; A second processing module, the second processing module is configured to smooth the rough solution path based on an adaptive pure tracking planning algorithm to obtain a smooth path; The third processing module is configured to determine whether the vehicle collides with the obstacle when passing through the smooth path. If not, the smooth path is used as the planned path; if so, the smooth path is adjusted based on a collision adjustment algorithm until the vehicle passes through the obstacle without colliding, and the adjusted smooth path is used as the planned path.

9. A terminal device, comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that: When the processor executes the computer program, the steps of the static obstacle avoidance method according to any one of claims 1 to 7 are implemented.

10. A computer-readable storage medium having a computer program stored thereon, characterized in that: When the computer program is executed by a processor, the steps of the static obstacle avoidance method according to any one of claims 1 to 7 are implemented.

Images