CN118192601A - Unstructured scene-oriented automatic driving tractor path planning method - Google Patents

Abstract

The invention discloses an unstructured scene-oriented automatic driving tractor path planning method. Firstly, establishing an environment map model; determining the specific position of the static obstacle; improving the heuristic function by utilizing Reeds-Shepp curve model to obtain a cost function of a mixed A algorithm, determining constraint conditions of the system, and expanding sub-nodes according to the cost function and step system constraint to obtain a global reference path; before the local obstacle avoidance optimization based on the dynamic programming algorithm is carried out, the Frenet coordinate and the Cartesian coordinate are converted on the tractor pose information; according to the global reference path and the coordinate conversion method, the global reference path is optimized by utilizing the S-L diagram, and a continuous and smooth expected path is obtained; and finally, carrying out speed planning by using the S-T diagram according to the coordinate conversion method and the expected path planning result, and determining the expected speed for effectively avoiding the dynamic obstacle.

Description

A path planning method for autonomous driving tractors in unstructured scenarios

Technical Field

The present invention belongs to the field of autonomous driving vehicle path planning, and in particular relates to an autonomous driving tractor path planning method for unstructured scenarios.

Background technique

Traditional tractors use manual driving to complete various tasks such as sowing, fertilizing, and fruit transportation. However, various obstacles are unavoidable in actual operation scenarios. Traditional operation methods are prone to missing rows and overlapping rows during obstacle avoidance, resulting in low production efficiency and waste of land resources. In recent years, with the gradual transformation of my country's agricultural machinery towards intelligence, the development of precision agriculture has been put on the agenda. An important prerequisite for implementing precision agriculture is path planning to guide agricultural machinery such as self-driving tractors to walk according to the planned operation path, that is, considering the location and layout of obstacles to determine the optimal path.

Self-driving tractors mainly travel in unstructured scenes. Due to the lack of clear road markings, complex and diverse road conditions, and the difficulty in distinguishing between road and non-road areas, traditional path planning methods that rely on lane lines or clear road boundaries for perception and positioning are not applicable. In addition, in unstructured scenes, if obstacle detection and environmental perception are used to determine a safe driving path using sensor data such as cameras and lidar, it is not only difficult to achieve but also significantly increases the system cost. At present, graph search-based methods (such as the A* algorithm) can discretize the state space into a graph and use offline map modeling to obtain a reference path in an unstructured scene to avoid over-reliance on sensor data. However, such schemes do not fully consider the kinematic constraints of the tractor (the path curvature may have sudden changes) and cannot handle the situation where there are dynamic obstacles in the scene or the movement direction of dynamic obstacles is not fixed. On the other hand, with the help of numerical optimization methods (such as dynamic programming algorithms), the above problems can also be solved online. However, too large a solution range will significantly increase the complexity of the calculation. Therefore, it is necessary to develop a path planning method for self-driving tractors for unstructured scenes, so that the tractor can efficiently bypass obstacles and improve the operation efficiency while ensuring the safety of the operation.

Summary of the invention

Purpose of the invention: In view of the shortcomings in the prior art, the present invention proposes a global reference path search and local obstacle avoidance planning method based on a hybrid A* algorithm and a dynamic programming algorithm for agricultural machinery operation scenarios without lane lines, clear road boundaries, and dynamic obstacles, aiming to solve the path planning problem of autonomous driving tractors for unstructured scenarios. First, a global reference path that satisfies the kinematic constraints of the tractor is determined based on the hybrid A* algorithm, and the optimal reference path search is completed while considering the tractor's dynamic constraints and static obstacles. On this basis, the curvature of the reference path is smoothed using the dynamic programming algorithm, and the speed planning for dynamic obstacles is completed in combination with the dynamic programming algorithm.

Technical solutions:

The present invention provides a path planning method for an automatic driving tractor in an unstructured scenario, the method comprising the following steps:

S1. Establish an environmental map model; determine the specific location of static obstacles;

S2. The heuristic function is improved by using the Reeds-Shepp curve model to obtain the cost function of the hybrid A* algorithm;

S3. Determine the constraints of the system to ensure obstacle avoidance safety and adapt to the tractor's motion characteristics;

S4. Expand the sub-nodes according to the cost function of step S2 and the system constraints of step S3 to obtain a global reference path;

S5. Before performing the local obstacle avoidance optimization based on the dynamic programming algorithm, the tractor posture information is converted into Frenet coordinates and Cartesian coordinates;

S6. According to the global reference path obtained in step S4 and the coordinate conversion method in step S5, the global reference path is optimized using the S-L graph to obtain a continuous and smooth desired path;

S7. Based on the coordinate conversion method of step S5 and the expected path planning result of step S6, the S-T diagram is used to perform speed planning to determine the expected speed for effectively avoiding dynamic obstacles.

Furthermore, the specific method of step S1 is:

Establish a grid map, discretize the tractor working environment into multiple independent square grids, each grid corresponds to a specific position in the real working environment, and mark the corresponding grid as occupied or idle according to the obstacles in the real environment. All marked grids together constitute the environmental map model;

In the grid map, the occupied grids are represented by black, and the corresponding value in the numerical matrix is 1; the idle grids are represented by white, and the corresponding value in the numerical matrix is 0. The mathematical model of the grid map is expressed as:

Wherein, A represents the entire environment map model, m and p are the number of rows and columns after the environment map is rasterized, respectively, _aij represents the grid located at (i, j), _aij = 0 indicates that the grid is idle, that is, there is no obstacle; _aij = 1 indicates that the grid is occupied, that is, there is an obstacle.

Furthermore, the specific method of step S2 is:

The core idea of the A* algorithm is to use the heuristic function to guide the direction of the algorithm traversal and evaluate each node of the network according to the cost function f(n), which can be expressed as:

f(n)＝g(n)+h(n,g)(2)

Among them, g(n) is the actual cost from the starting node to the current node, h(n,g) is the distance heuristic function from the current node n to the target node g, which is used to evaluate the predicted distance cost from the current node n to the target node g;

During the search process, the corresponding distance heuristic function h(n,g) uses the Euclidean distance h ₁ (n,g) to ignore the tractor's non-integrity constraints and consider the obstacle avoidance environment constraints:

Among them, (x _n ,y _n ) is the coordinate of the current node n, (x _g ,y _g ) is the coordinate of the path target point;

The Reeds-Shepp curve model consists of two turns and a straight line, which is used to determine the shortest path of a tractor or other moving body under given constraints. The distance heuristic function commonly used in the A* algorithm and the Reeds-Shepp curve model are set in parallel to obtain a hybrid A* algorithm, that is, the larger value is selected as the heuristic function value for the search, while satisfying the non-holonomic kinematic constraints of the tractor and the highest travel efficiency:

h(n,g)＝max(h ₁ (n,g),h ₂ (n,g)) (4)

Wherein, h ₂ (n,g) represents the distance heuristic function using the Reeds-Shepp curve model, which obtains the shortest path from the current node to the target location under the conditions of ignoring environmental obstacles and considering the non-holonomic constraints of the tractor;

In the hybrid A* algorithm, each node records the specific speed and front wheel angle of its parent node extending to the current node. To prevent the vehicle speed v and the front wheel angle δ _f from changing frequently, an additional cost function g ₁ (n) is added to the actual cost g(n):

_g1 (n)＝ _ωg1 ·| _vn - _vn-1 |+ _ωg2 ·| _δfn - _δfn-1 | (5)

Among them, ω _g1 is the weight coefficient when the car's driving state changes; ω _g2 is the weight coefficient when the car's driving direction changes; v _n is the speed of the car when it drives to the current node n, v _n-4 is the speed of the car when it drives to the previous node n-1, δ _{fn is} the front wheel turning angle when the car drives to the current node n, and δ _fn-1 is the front wheel turning angle when the car drives to the previous node n-1.

Furthermore, the specific method of step S3 is: select a rectangle and a polygon as the outline of the tractor and the obstacle, regard the tractor as a rectangle, use the tractor boundary function to describe the size and shape of the tractor, and the horizontal and vertical coordinates of the corresponding four vertices _M1 to _M4 are expressed as:

Among them, (x, y) is the tractor position coordinate, f is the tractor front overhang distance, r is the tractor rear overhang distance, L is the tractor wheelbase, w is the body width tractor size parameter, and Ψ is the tractor heading angle;

To determine whether the tractor collides with an obstacle, it is necessary to determine whether the tractor boundary and the obstacle outline coincide. The triangle area method is used to check the positional relationship between the tractor vertex coordinates and the convex polygon N ₁ N ₂ …N _n . Let one of the tractor's vertex coordinates be _Mi , and connect point _Mi with every two adjacent vertices of the convex polygon to form a triangle. The areas of these triangles are accumulated. If the sum of the accumulated areas is greater than the total area of the convex polygon, it is determined that point _Mi is located outside the convex polygon; if the sum of the areas is less than or equal to the total area of the convex polygon, point _Mi may be located inside the polygon, as shown in formula (7):

Among them, S represents the area of each polygon;

Area of convex polygon The problem can be solved by using several triangles. On a two-dimensional plane, the collision will definitely occur at the vertex. Each vertex of the vehicle body rectangle is always constrained to be outside the obstacle polygon. At the same time, each vertex of the polygonal obstacle is also constrained to be outside the vehicle body rectangle. Using equation (7), we can obtain the constraints f ₁ and f ₂ that point _Mi is outside the convex polygon N ₁ N ₂ …N _n , and establish the following obstacle avoidance constraints:

Among them, γ(N ₁ ,…N _n ) is the set of convex polygon vertices, χ(M ₁ ,…M ₄ ) is the set of four vertices of the body rectangle; according to the kinematic characteristics of the tractor, it can be known that the curvature κ of the tractor's travel path is only related to the front wheel turning angle δ _f :

Too large a front wheel steering angle will increase the path curvature, which will reduce driving comfort and increase mechanical wear of the tractor. Therefore, the curvature of the planned path is constrained by limiting the tractor's front wheel steering angle:

|δ _f |≤arctan(Lκ _max ) (10)

Finally, the obstacle avoidance constraints and front wheel turning angle constraints solved by the hybrid A* algorithm are determined to ensure obstacle avoidance safety and adapt to the tractor's motion characteristics.

Furthermore, the specific method of step S4 is: considering the kinematic constraints of the tractor, using the hybrid A* algorithm to change the expansion mode of the node: each node contains the state information (x, y, Ψ) of the tractor, where x and y represent the position coordinates of the tractor, Ψ represents the heading angle of the tractor, and each node represents the parent node with the center point. The number of child nodes of the hybrid A* algorithm is 6, which are generated from any position in the grid. The nodes are connected in a three-dimensional kinematic state, and the searched path can meet the non-integrity constraints of the tractor, that is, the global reference path is obtained by planning and solving using the hybrid A* algorithm.

Furthermore, the specific method of step S5 is: converting the road description using the Cartesian coordinate system in steps S1-S5 into the description using the Frenet coordinate system, where the motion state of the tractor at a certain point is expressed as [x, y, v, Ψ, a, κ]; a is the longitudinal acceleration in the Cartesian coordinate system, and in the Frenet coordinate system, the motion state of the tractor is expressed as Where s is the longitudinal displacement, /> is the longitudinal speed, /> is the longitudinal acceleration, /> is the lateral speed, /> is the lateral acceleration, l' is the derivative of l with respect to s, and l" is / > Yes/> The derivative of

The formula for converting Cartesian coordinate system to Frenet coordinate system is:

The formula for converting Frenet coordinate system to Cartesian coordinate system is:

Where s _r is the desired longitudinal displacement, x _r and y _r are the desired lateral and longitudinal positions in the Cartesian coordinate system, κ _r is the reference path curvature, κ′ _r is the derivative of the reference path curvature with respect to s, ΔΨ＝Ψ-Ψ _r , and Ψ _r is the reference heading angle.

Furthermore, the specific method of step S6 is: first, construct a relationship diagram representing the road and obstacles in the Frenet coordinate system according to the obstacle information in the scene, referred to as the SL diagram, and discretize the space by scattering points in the SL diagram and initializing it; second, set a cost function, assign a cost value to each node to describe the path selection relationship between adjacent units; consider the position of the obstacle, the current position of the tractor and the target position, and use a dynamic programming algorithm to optimize the reference path to ensure that the minimum cost path can be found while avoiding collisions. The cost function J is described by the safety distance cost function J _obs between the path and the static obstacle, the smoothness cost function J _s , and the deviation from the reference path cost function J _l :

J＝J _obs +J _s +J _l (13)

The safety distance cost function J _obs between the path and the static obstacle is expressed as:

Where d _max is the maximum collision distance, d _min is the minimum collision distance, [d _min ,d _max ] is the collision buffer zone, and f _risk is a monotonically decreasing collision risk function. The closer to the obstacle, the larger the f _risk value, which means the more likely it is to have a collision risk.

The smoothness cost function J _s is expressed as:

Where t ₀ is the start time of path planning, t _s is the end time of path planning, ω ₁ , ω ₂ , ω ₃ are smoothness weight coefficients, representing the path lateral displacement, path curvature, and the rate of change of path curvature, respectively, and l″′ is Yes/> The derivative of

The cost function J _l of the degree of deviation from the reference path can be expressed as:

Among them, ω _l is the weight coefficient of the degree of deviation from the reference path, l _r is the reference line function;

A recursive method is used to search from the starting point to the end point. The cost value of the current node is updated by calculating the minimum cost value from each node to the end point, and the path information is saved for backtracking. After all nodes are calculated, the minimum connection cost of all path sampling nodes is screened out;

Then, the given nodes are interpolated and fitted using the quintic polynomial, and the discrete nodes are made continuous to obtain a smooth path curve that meets the requirements. The quintic polynomial curve can be expressed by the following formula:

Where t represents time, a ₁₀ ～a _{(N-1) 5} represent the coefficients of the quintic polynomial, τ(s) represents the set of multiple quintic polynomials, s ⁵ represents the fifth power of s, and assuming that the starting state information is [s _j ,l _j ,l′ _j ,l″ _j ] and the end state information is [s _k ,l _k ,l′ _k ,l″ _k ], substitute the starting and end state information into formula (17), and we get:

Among them, a _k0 ~ a _k5 represent the coefficients of each section of the quintic polynomial;

By solving the linear equations of equation (18), all coefficients of the quintic polynomial curve can be determined, and the reference path can be optimized. Finally, the node information of the path is transformed, and the relevant position coordinate information is transformed from the S-L diagram of the Frenet coordinate system to the X-Y diagram of the Cartesian coordinate system to obtain the desired path for tracking control.

Furthermore, the specific method of step S7 is: the moment of the dynamic obstacle entering is T _in , the moment of the dynamic obstacle exiting is T _out , the lengths S ₁ and S ₂ are related to the width of the dynamic obstacle, the construction of the ST graph depends on the prediction of the trajectory of the dynamic obstacle, and the trajectory of the dynamic obstacle is predicted using a constant speed model:

In the formula, the initial position of the obstacle is (s ₀ ,l ₀ ), and the lateral and longitudinal velocities are and/> (s _t ,l _t ) is the predicted trajectory of the obstacle at time t;

Dynamic obstacles are projected onto the ST graph and sampled in the ST graph. During the sampling process, a series of sampling points are generated at each moment, that is, multiple candidate solutions. In order to obtain the optimal solution, a cost function needs to be defined to evaluate the pros and cons of each candidate solution. Safety means that the tractor should keep a safe distance from dynamic obstacles as much as possible during the speed planning process to ensure driving safety. In the time interval where obstacles exist, a safety cost function J ₁ is introduced:

Where ω _obs is the obstacle weight coefficient, the positive number k _b indicates the degree to which the tractor is away from the dynamic obstacle, S _obs indicates the boundary formed by the straight line fitting the dynamic obstacle, and S _b indicates the boundary of the tractor movement; if the tractor is affected by multiple dynamic obstacles within a certain period of time, multiple safety cost functions J ₁ are accumulated;

In order to minimize the tractor's speed and speed change rate, the comfort cost function J ₂ is set as follows:

Among them, ω _v and ω _a represent the weight coefficients of speed and speed change rate respectively;

The expected speed v _ref does not consider the impact of moving obstacles and is determined by the road speed limit, path curvature and other traffic rules. The expected speed cost function J ₃ is defined as:

Where ω _r represents the weight coefficient of deviation from the expected speed;

Integrating the above cost functions, we get the cost function of speed planning:

J＝J ₁ +J ₂ +J ₃ (23)

Finally, a dynamic programming algorithm is used to solve a speed curve with the lowest cost in the S-T diagram to determine the speed decision-making method of the tractor when facing dynamic obstacles; the relevant position and speed information is transformed from the S-T diagram of the Frenet coordinate system to the X-Y diagram of the Cartesian coordinate system to obtain the expected speed for effectively avoiding dynamic obstacles.

The beneficial effects of the present invention are:

(1) A hybrid A* algorithm is proposed to perform a global reference path search for static obstacles and obtain the optimal path from the starting point to the end point that satisfies the kinematic constraints of the electric tractor.

(2) The reference path is optimized based on the S-L graph, and the speed planning is completed based on the S-T graph, thus realizing local obstacle avoidance optimization considering dynamic obstacles.

BRIEF DESCRIPTION OF THE DRAWINGS

Figure 1 is an overall block diagram;

Figure 2 is a grid map;

FIG3 is a schematic diagram of the sub-node expansion of the A* and hybrid A* algorithms;

Figure 4 shows the triangle area method;

Figure 5 shows the correspondence between the Frenet coordinate system and the Cartesian coordinate system;

Figure 6 shows the projection of the dynamic obstacle on the S-T diagram.

Detailed ways

The present invention is further described below in conjunction with the accompanying drawings and specific embodiments, but the protection scope of the present invention is not limited thereto.

Step (1), establish an environmental map model; determine the specific location of static obstacles

A grid map is established to discretize the tractor working environment into multiple independent square grids, each of which corresponds to a specific location in the real working environment. According to the obstacles in the real environment, the corresponding grids are marked as occupied or idle. All marked grids together constitute the environmental map model.

In the grid map, the occupied grids are represented by black, and the corresponding value in the numerical matrix is 1; the idle grids are represented by white, and the corresponding value in the numerical matrix is 0, as shown in Figure 2. The mathematical model of the grid map can be expressed as:

Wherein, A represents the entire environment map model, m and p are the number of rows and columns after the environment map is rasterized, respectively, _aij represents the grid located at (i, j), _aij = 0 indicates that the grid is idle, that is, there is no obstacle; _aij = 1 indicates that the grid is occupied, that is, there is an obstacle.

Step (2), using the Reeds-Shepp curve model to improve the heuristic function, and obtain the cost function of the hybrid A* algorithm.

The core idea of the A* algorithm is to use the heuristic function to guide the direction of the algorithm traversal and evaluate each node of the network according to the cost function f(n), which can be expressed as:

f(n)＝g(n)+h(n,g) (2)

Among them, g(n) is the actual cost from the starting node to the current node, and h(n,g) is the distance heuristic function from the current node n to the target node g, which is used to evaluate the predicted distance cost from the current node n to the target node g. During the search process, nodes that are more likely to lead to the target should be given priority to improve the search efficiency; the corresponding distance heuristic function h(n,g) uses the Euclidean distance h ₁ (n,g) to ignore the tractor non-integrity constraints and consider the obstacle avoidance environment constraints:

Among them, (x _n , y _n ) are the coordinates of the current node n, and (x _g , y _g ) are the coordinates of the path target point.

The Reeds-Shepp curve model consists of two turns and a straight line. It can determine the shortest path of a tractor or other moving body under given constraints and can effectively describe different types of movement and steering operations. By using the distance heuristic function commonly used in the A* algorithm and the Reeds-Shepp curve model for parallel settings, a hybrid A* algorithm can be obtained, that is, the larger value is selected as the heuristic function value for the search, while satisfying the non-holonomic kinematic constraints of the tractor and the highest travel efficiency:

h(n,g)＝max(h ₁ (n,g),h ₂ (n,g)) (4)

Among them, h ₂ (n,g) represents the distance heuristic function using the Reeds-Shepp curve model, which obtains the shortest path from the current node to the target position under the condition of ignoring environmental obstacles and considering the non-integrity constraints of the tractor. This heuristic function can ensure that the child node is constrained by the front wheel angle when expanding.

In the hybrid A* algorithm, each node records the specific speed and front wheel angle of its parent node to the current node. To prevent the vehicle speed v and the front wheel angle δ _f from changing frequently, an additional cost function g ₁ (n) is added to the actual cost g(n):

_g1 (n)＝ _ωg1 ·| _vn - _vn-1 |+ _ωg2 ·| _δfn - _δfn-1 | (5)

Among them, ω _g1 is the weight coefficient when the car's driving state changes; ω _g2 is the weight coefficient when the car's driving direction changes; v _n is the speed of the car when it drives to the current node n, v _n-1 is the speed of the car when it drives to the previous node n-1, δ _{fn is} the front wheel turning angle when the car drives to the current node n, and δ _fn-1 is the front wheel turning angle when the car drives to the previous node n-1.

Step (3) determines the constraints of the system to ensure obstacle avoidance safety and adapt to the tractor's motion characteristics.

Select rectangles and polygons as the outlines of the tractor and the obstacle, regard the tractor as a rectangle, use the tractor boundary function to describe the size and shape of the tractor, and the horizontal and vertical coordinates of the corresponding four vertices M ₁ ~M ₄ are expressed as:

Among them, (x, y) is the tractor position coordinate, f is the tractor front overhang distance, r is the tractor rear overhang distance, L is the tractor wheelbase, w is the body width tractor size parameter, and Ψ is the tractor heading angle;

To determine whether the tractor collides with an obstacle, it is necessary to determine whether the tractor boundary and the obstacle outline coincide. The triangle area method is used to check the positional relationship between the tractor vertex coordinates and the convex polygon N ₁ N ₂ …N _n . Let one of the tractor's vertex coordinates be _Mi , and connect point _Mi with every two adjacent vertices of the convex polygon to form a triangle. The areas of these triangles are accumulated. If the sum of the accumulated areas is greater than the total area of the convex polygon, it is determined that point _Mi is located outside the convex polygon; if the sum of the areas is less than or equal to the total area of the convex polygon, point _Mi may be located inside the polygon, as shown in formula (7):

Among them, S represents the area of each polygon;

Area of convex polygon The problem can be solved by using several triangles. On a two-dimensional plane, the collision will definitely occur at the vertex. Each vertex of the vehicle body rectangle is always constrained to be outside the obstacle polygon. At the same time, each vertex of the polygonal obstacle is also constrained to be outside the vehicle body rectangle. Using equation (7), we can obtain the constraints f ₁ and f ₂ that point _Mi is outside the convex polygon N ₁ N ₂ …N _n , and establish the following obstacle avoidance constraints:

Among them, γ(N ₁ ,…N _n ) is the set of vertices of the convex polygon, χ(M ₁ ,…M ₄ ) is the set of four vertices of the body rectangle;

According to the kinematic characteristics of the tractor, it can be seen that the curvature κ of the tractor's travel path is only related to the front wheel turning angle _δf :

Too large a front wheel steering angle will increase the path curvature, which will reduce driving comfort and increase mechanical wear of the tractor. Therefore, the curvature of the planned path is constrained by limiting the tractor's front wheel steering angle:

|δ _f |≤arctan(Lκ _max ) (10)

Finally, the obstacle avoidance constraints and front wheel turning angle constraints solved by the hybrid A* algorithm are determined to ensure obstacle avoidance safety and adapt to the tractor's motion characteristics.

Step (4), expand the child nodes according to the cost function of step S2 and the system constraints of step S3 to obtain the global reference path, consider the kinematic constraints of the tractor, and use the hybrid A* algorithm to change the node expansion method: each node contains the state information of the tractor (x, y, Ψ), where x and y represent the position coordinates of the tractor, Ψ represents the heading angle of the tractor, and each node represents the parent node with the center point. The number of child nodes of the hybrid A* algorithm is 6, which are generated from any position in the grid. The nodes are connected by three-dimensional kinematic states, and the searched path can meet the non-integrity constraints of the tractor, that is, the global reference path is obtained by planning and solving using the hybrid A* algorithm.

In step (5), before performing local obstacle avoidance optimization based on the dynamic programming algorithm, the tractor posture information is converted into Frenet coordinates and Cartesian coordinates.

The Frenet coordinate system uses curvature and tangent direction to represent the path, which can accurately describe the change in path curvature and the position offset of the tractor relative to the reference path. The road can be represented as a function of the reference line and the lateral offset. As shown in Figure 5, the two-dimensional path planning task is simplified to a one-dimensional problem based on the S-L diagram, and the longitudinal displacement s and lateral displacement l are used to represent the relative position relationship between the actual path and the reference path.

In order to make path planning and tracking control efficient and easy to implement, the Frenet coordinate system is generally used to describe the road in the path planning stage, and the Cartesian coordinate system is used to describe the intuitive and simplified control in the tracking control stage. In the Cartesian coordinate system, the motion state of the tractor at a certain point can be expressed as [x, y, v, Ψ, a, κ]; a is the longitudinal acceleration in the Cartesian coordinate system, while in the Frenet coordinate system, the motion state of the tractor can be expressed as Among them,/> is the longitudinal speed, /> is the longitudinal acceleration, /> is the lateral speed, /> is the lateral acceleration, l' is the derivative of l with respect to s, l" is the derivative of l' with respect to /> The derivative of .

The formula for converting Cartesian coordinate system to Frenet coordinate system is:

The formula for converting Frenet coordinate system to Cartesian coordinate system is:

in, and/> are the longitudinal speed and longitudinal acceleration in the Frenet coordinate system,/> and/> is the lateral vehicle speed and lateral acceleration in the Frenet coordinate system, l′ is the derivative of l with respect to s, and l″ is /> Yes/> , s _r is the desired longitudinal displacement, x _r and y _r are the desired lateral position and longitudinal position in the Cartesian coordinate system, κ _r is the reference path curvature, κ′ _r is the derivative of the reference path curvature with respect to s, ΔΨ＝Ψ-Ψ _r , Ψ _r is the reference heading angle.

Step (6), based on the global reference path obtained in step S4 and the coordinate transformation method in step S5, the global reference path is optimized using the S-L graph to obtain a continuous and smooth desired path.

The reference path is optimized using a dynamic programming algorithm, and the solution process is shown in Figure 6. First, the SL graph in the Frenet coordinate system is constructed based on the obstacle information in the scene, and the space is discretized by scattering points in the graph and initializing it. Secondly, a reasonable cost function is set to assign a cost value to each node to describe the path selection relationship between adjacent units. The main factors considered are the location of the obstacle, the current location of the tractor, and the target location, to ensure that the minimum cost path can be found while avoiding collisions. The cost function J is described by the safety distance cost function J _obs between the path and the static obstacle, the smoothness cost function J _s , and the deviation cost function J _l from the reference path:

J＝J _obs +J _s +J _l (13)

The distance cost function J _obs between the path and the static obstacle can be expressed as:

Among them, d _max is the maximum collision distance, d _min is the minimum collision distance, [d _min ,d _max ] is the collision buffer zone, and f _risk is a monotonically decreasing collision risk function. The closer to the obstacle, the larger the f _risk value, which means the greater the risk of collision.

The smoothness cost function J _s can be expressed as:

Where t ₀ is the start time of path planning, t _s is the end time of path planning, ω ₁ , ω ₂ , ω ₃ are smoothness weight coefficients, representing the path lateral displacement, path curvature, and the rate of change of path curvature, respectively, and l″′ is Yes/> The derivative of .

The cost function J _l of the degree of deviation from the reference path can be expressed as:

Among them, ω _l is the weight coefficient of the degree of deviation from the reference path, and l _r is the reference line function. The reference line can guide the direction of the tractor. For structured scenes, the center line of the road is usually used as the reference line; for unstructured scenes, the planned reference path can be selected as the reference line.

A recursive method is used to search from the starting point to the end point. The cost value of the current node is updated by calculating the minimum cost value from each node to the end point, and the path information is saved for backtracking. After all nodes are calculated, the minimum connection cost of all path sampling nodes is screened. Finally, the node information of the path is transformed from the S-L diagram of the Frenet coordinate system to the X-Y diagram of the Cartesian coordinate system for tracking control.

Use the quintic polynomial to interpolate and fit the given nodes, make the discrete nodes continuous, and obtain a smooth path curve that meets the requirements. The quintic polynomial curve can be expressed by the following formula:

Where t represents time, a ₁₀ ～a _(N-1)5 represent the coefficients of the quintic polynomial, τ(s) represents the set of multiple quintic polynomials, and s ⁵ represents the fifth power of s. Assuming that the starting state information of a quintic polynomial is [s _j ,l _j ,l′ _j ,l″ _j ], and the end state information is [s _k ,l _k ,l′ _k ,l″ _k ], substituting the starting and end state information into equation (17), we can obtain:

Among them, a _k0 ~ a _k5 represent the coefficients of each section of the quintic polynomial;

By solving the linear equations of equation (18), all coefficients of the quintic polynomial curve can be determined, thus optimizing the reference path.

Step (7): Speed planning based on S-T graph

The ST graph can be used to describe the motion relationship between the tractor and the dynamic obstacle. The speed planning problem can be transformed into a feasible node search problem in the ST graph by using the dynamic programming algorithm. The speed planning at a certain moment is performed on the premise that the path planning at that moment has been completed. To perform dynamic obstacle avoidance, the dynamic obstacle position and the predicted future position need to be projected into the ST graph in the Frenet coordinate system. As shown in Figure 6, the moment when the dynamic obstacle cuts in is T _in , and the moment when it cuts out is T _out . The length S ₁ S ₂ is related to the width of the dynamic obstacle.

The construction of the S-T diagram depends on the prediction of the trajectory of dynamic obstacles. The constant velocity model can be used to predict the trajectory of dynamic obstacles:

In the formula, the initial position of the obstacle is (s ₀ ,l ₀ ), and the lateral and longitudinal velocities are and/> (s _t ,l _t ) is the predicted trajectory of the obstacle at time t.

Dynamic obstacles are projected onto the S-T graph and sampled in the S-T graph. During the sampling process, a series of sampling points, i.e., multiple candidate solutions, are generated at each moment. In order to obtain the optimal solution, a cost function needs to be defined to evaluate the pros and cons of each candidate solution. The cost function mainly considers factors such as safety, stability, and expected speed.

Safety means that the tractor should keep a safe distance from dynamic obstacles as much as possible during speed planning to ensure driving safety. In the time interval where obstacles exist, a safety cost function J ₁ is introduced:

Where ω _obs is the obstacle weight coefficient, the positive number k _b indicates the degree to which the tractor is away from the dynamic obstacle, S _obs indicates the boundary formed by the straight line fitting the dynamic obstacle, and S _b indicates the boundary of the tractor movement. If the tractor is affected by multiple dynamic obstacles within a certain period of time, multiple safety cost functions J ₁ can be accumulated.

Stability can be equivalent to the smoothness of the speed planning curve. In the cost function, the tractor speed and speed change rate should be minimized. The sampling points are connected by a fifth-order polynomial, and the acceleration and its change rate in each time interval can be easily expressed analytically. In order to minimize the tractor speed and speed change rate, the comfort cost function J ₂ can be set:

Among them, ω _v and ω _a represent the weight coefficients of speed and speed change rate respectively.

The expected speed v _ref does not consider the impact of moving obstacles and is determined by the road speed limit, path curvature and other traffic rules. The corresponding cost function J ₃ can be defined as:

Where ω _r represents the weight coefficient of deviation from the expected speed.

Integrating the above cost functions, we get the cost function of speed planning:

J＝J ₁ +J ₂ +J ₃ (23)

Finally, a dynamic programming algorithm is used to solve a speed curve with the lowest cost in the S-T diagram to determine the speed decision-making method of the tractor when facing dynamic obstacles; based on the obtained optimal speed planning solution, dynamic obstacles can be effectively avoided.

The embodiments are preferred implementations of the present invention, but the present invention is not limited to the above-mentioned implementations. Any obvious improvements, substitutions or modifications that can be made by those skilled in the art without departing from the essential content of the present invention belong to the protection scope of the present invention.

Claims (8)

1. An unstructured scene-oriented automatic driving tractor path planning method is characterized by comprising the following steps of:

S1, establishing an environment map model; determining the specific position of the static obstacle;

S2, improving the heuristic function by utilizing Reeds-Shepp curve model to obtain a cost function of the mixed A-algorithm;

s3, determining constraint conditions of the system, ensuring obstacle avoidance safety and adapting to the motion characteristics of the tractor;

s4, expanding child nodes according to the cost function in the step S2 and the system constraint in the step S3, and acquiring a global reference path;

S5, before the local obstacle avoidance optimization based on the dynamic programming algorithm is carried out, converting Frenet coordinates and Cartesian coordinates of the tractor pose information;

S6, optimizing the global reference path by using the S-L diagram according to the global reference path obtained in the step S4 and the coordinate conversion method in the step S5, and obtaining a continuous and smooth expected path;

S7, carrying out speed planning by using the coordinate conversion method in the step S5 and the expected path planning result in the step S6, and determining the expected speed for effectively avoiding the dynamic obstacle.

2. The method for planning the path of the automatic driving tractor facing the unstructured scene according to claim 1, wherein the specific method of the step S1 is as follows:

Establishing a grid map, discretizing the working environment of the tractor into a plurality of independent square grids, wherein each grid corresponds to a specific position in the real working environment, marking the corresponding grid as occupying or idle according to the obstacle condition in the real environment, and forming an environment map model by all marked grids;

In the grid map, the occupied grid is represented by black, and the corresponding value in the numerical matrix is 1; the empty grid is represented by white, the corresponding value in the numerical matrix is 0, and the mathematical model of the grid map is represented as:

Wherein a represents the whole environment map model, m and p are the number of rows and the number of columns after the environment map is rasterized, a _ij represents a grid at (i, j), a _ij =0 represents that the grid is in an idle state, i.e. no obstacle exists; a _ij =1 indicates the grid occupied state, i.e., the presence of an obstacle.

3. The method for planning the path of the automatic driving tractor facing the unstructured scene according to claim 1, wherein the specific method of the step S2 is as follows:

The core idea of the algorithm is to guide the traversing direction of the algorithm by using a heuristic function, and evaluate each node of the network according to a cost function f (n), which can be expressed as:

f(n)＝g(n)+h(n,g)(2)

Wherein g (n) is the actual cost from the initial node to the current node, h (n, g) is the distance heuristic function from the current node n to the target node g, and is used for evaluating the predicted distance cost from the current node n to the target node g;

In the searching process, the Euclidean distance h ₁ (n, g) is adopted as the corresponding distance heuristic function h (n, g) so as to neglect the non-integrity constraint of the tractor and consider the obstacle avoidance environment constraint:

Wherein, (x _n,y_n) is the coordinates of the current node n, (x _g,y_g) is the coordinates of the path-target point;

The Reeds-Shepp curve model consists of two turns and a section of straight line, is used for determining the shortest path of a tractor or other moving bodies under a given constraint condition, and is arranged in parallel by utilizing a distance heuristic function commonly used by an A-algorithm and the Reeds-Shepp curve model to obtain a mixed A-algorithm, namely, a larger value is selected as a heuristic function value for searching, and meanwhile, the incomplete kinematic constraint and the highest passing efficiency of the tractor are met:

h(n,g)＝max(h₁(n,g),h₂(n,g)) (4)

Wherein h ₂ (n, g) represents a distance heuristic function adopting a Reeds-Shepp curve model, and obtaining the shortest path from the current node to the target position under the condition of ignoring environmental barriers and considering the non-integrity constraint of the tractor;

In the hybrid a algorithm, each node records the specific speed and front wheel rotation angle of the parent node expanding to the current node, and in order to prevent the frequent change of the vehicle speed v and the front wheel rotation angle delta _f, an additional cost function g ₁ (n) is added in the actual cost g (n):

g₁(n)＝ω_g1·|v_n-v_n-1|+ω_g2·|δ_fn-δ_fn-1| (5)

Wherein omega _g1 is a weight coefficient when the running state of the automobile is changed; omega _g2 is the weight coefficient when the running direction of the automobile changes; v _n is the speed of the vehicle when the vehicle is driving to the current node n, v _n-1 is the speed of the vehicle when the vehicle is driving to the previous node n-1, delta _fn is the front wheel angle when the vehicle is driving to the current node n, delta _fn-1 is the front wheel angle when the vehicle is driving to the previous node n-1.

4. The method for planning the path of the automatic driving tractor facing the unstructured scene according to claim 1, wherein the specific method of the step S3 is as follows: rectangular and polygonal shapes are chosen as the contours of the tractor and the obstacle, the tractor is considered as rectangular, the tractor size and shape are described by the tractor boundary function, and the corresponding four vertices M ₁～M₄ are represented as the abscissa and the ordinate:

Wherein, (x, y) is the position coordinates of the tractor, f is the front suspension distance of the tractor, r is the rear suspension distance of the tractor, L is the wheelbase of the tractor, w is the dimension parameter of the tractor with the width of the vehicle body, and ψ is the course angle of the tractor;

Judging whether the tractor collides with an obstacle or not, determining whether the tractor boundary and the obstacle outline coincide or not, checking the position relation between the coordinates of the tractor vertexes and the convex polygon N ₁N₂…N_q by adopting a triangle area method, enabling one vertex coordinate of the tractor to be M _i, enabling a connection point M _i and every two adjacent vertexes of the convex polygon to form a triangle, accumulating the areas of the triangles, and determining that a point M _i is positioned outside the convex polygon if the sum of the areas obtained by accumulation is larger than the total area of the convex polygon; if the sum of the areas is less than or equal to the total area of the convex polygon, the point M _i may be located inside the polygon, as shown in formula (7):

Wherein S represents the area of each polygon;

Area of convex polygon The method can solve a plurality of triangles, collision can be necessarily generated at the vertex on a two-dimensional plane, each vertex of the vehicle body rectangle is always restrained outside the obstacle polygon, each vertex of the obstacle polygon is restrained to be positioned outside the vehicle body rectangle, constraint conditions f ₁ and f ₂ of the point M _i positioned outside the convex polygon N ₁N₂…N_n are obtained by using the formula (7), and the following obstacle avoidance constraint is established:

Wherein, gamma (N ₁,…N_n) is a set of convex polygon vertexes, and χ (M ₁,…M₄) is a set of four vertexes of the rectangle of the vehicle body;

from the kinematic characteristics of the tractor, it is known that the curvature κ of the path travelled by the tractor is only related to the front wheel angle δ _f:

excessive front wheel turning angle increases the curvature of the path, while reducing the driving comfort, and at the same time aggravates the mechanical wear of the tractor, so by limiting the tractor front wheel turning angle, the curvature of the planned path is constrained:

|δ_f|≤arctan(Lκ_max) (10)

And finally, determining obstacle avoidance constraint and front wheel steering angle constraint solved by the mixed A algorithm, ensuring the safety of obstacle avoidance, and adapting to the motion characteristics of the tractor.

5. The method for planning the path of the automatic driving tractor facing the unstructured scene according to claim 1, wherein the specific method of the step S4 is as follows: considering the kinematic constraint of the tractor, adopting a hybrid A algorithm to change the expansion mode of the nodes: each node contains state information (x, y, ψ) of the tractor, wherein x and y represent position coordinates of the tractor, ψ represents a course angle of the tractor, each node represents a parent node by a central point, the number of child nodes of the hybrid a-algorithm is 6, the nodes are generated from any position in a grid, the nodes are connected by three-dimensional kinematic states, and the searched paths can meet the non-integrity constraint of the tractor, namely, the global reference paths are obtained by planning and solving by using the hybrid a-algorithm.

6. The method for planning the path of the automatic driving tractor facing the unstructured scene according to claim 1, wherein the specific method of the step S5 is as follows: converting road description in the steps S1-S5 by using a Cartesian coordinate system into description by using a Frenet coordinate system, wherein the tractor motion state of a certain point is expressed as [ x, y, v, ψ, a, kappa ]; a is the vehicle acceleration in cartesian's coordinate system, while in Frenet's coordinate system, the tractor motion state is expressed asWherein s is longitudinal displacement,/>For longitudinal speed of vehicle,/>Is longitudinal acceleration, l is transverse displacement,/>For transverse speed,/>For lateral acceleration, l 'is the derivative of l with s, l' is/>Pair/>Is a derivative of (2);

the formula for converting the Cartesian coordinate system into the Frenet coordinate system is as follows:

the formula for converting the Frenet coordinate system into the Cartesian coordinate system is as follows:

Where s _r is the desired longitudinal displacement, x _r and y _r are the desired lateral and longitudinal positions in the Cartesian coordinate system, κ _r is the reference path curvature, κ' _r is the derivative of the reference path curvature with s, Δψ=ψ - ψ _r,Ψ_r is the reference heading angle.

7. The method for planning the path of the automatic driving tractor facing the unstructured scene according to claim 6, wherein the specific method in the step S6 is as follows: firstly, constructing a relation diagram representing a road and an obstacle under a Frenet coordinate system according to obstacle information in a scene, namely an S-L diagram, and discretizing a space by scattering points in the S-L diagram and initializing; secondly, setting a cost function, and distributing a cost value for each node to describe the path selection relation between adjacent units; taking the position of an obstacle, the current position of a tractor and the target position into consideration, optimizing a reference path by adopting a dynamic programming algorithm, ensuring that the minimum cost path can be found while collision is avoided, and describing a cost function J by a safe distance cost function J _obs, a smoothness cost function J _s and a deviation reference path degree cost function J _l of the path and the static obstacle:

J＝J_obs+J_s+J_l (13)

The safe distance cost function J _obs for a path to a static obstacle is expressed as:

Wherein d _max is the maximum collision distance, d _min is the minimum collision distance, d _min,d_max is the collision buffer zone, f _risk is a monotonically decreasing collision risk function, and the closer to an obstacle, the larger the f _risk value is, namely the easier the collision risk is generated;

The smoothness cost function J _s is expressed as:

wherein t ₀ is the path planning start time, t _s is the path planning end time, ω ₁、ω₂、ω₃ is the smoothness weight coefficient, and l' "is the rate of change of the path lateral displacement, the path curvature and the path curvature Pair/>Is a derivative of (2);

The cost function J _l of the degree of departure from the reference path can be expressed as:

Wherein ω _l is a deviation reference path degree weight coefficient, and l _r is a reference line function;

Searching from a starting point to a terminal point by adopting a recursive method, updating the cost value of the current node by calculating the minimum cost value from each node to the terminal point, storing path information for backtracking, and screening to obtain the minimum connection cost of all path sampling nodes after all nodes are calculated;

Then, interpolation and fitting are carried out on given nodes by using a fifth-order polynomial, discrete nodes are continuous, and a smooth path curve meeting the requirements is obtained, wherein the fifth-order polynomial curve can be expressed by the following formula:

Where t represents time, a ₁₀～a_(N-1)5 represents the coefficient of the polynomial of fifth degree, τ(s) represents the set of the polynomial of fifth degree, s ⁵ represents the square of s, let s _j,l_j,l′_j,l″_j be the start point state information, s _k,l_k,l′_k,l″_k be the end point state information, and let the start point and end point state information be the formula (17) to obtain:

wherein a _k0～a_k5 represents the polynomial coefficient of each segment of fifth degree;

Solving a linear equation set of the equation (18), and determining all coefficients of a quintic polynomial curve to realize optimization of a reference path; and finally, carrying out coordinate conversion on the node information of the path, and transforming the related position coordinate information from an S-L diagram of a Frenet coordinate system to an X-Y diagram of a Cartesian coordinate system to acquire the expected path for tracking control.

8. The method for planning the path of the automatic driving tractor facing the unstructured scene according to claim 7, wherein the specific method of the step S7 is as follows: the moment of cutting in the dynamic obstacle is recorded as T _in, the moment of cutting in the dynamic obstacle is recorded as T _out, the length S ₁S₂ is related to the width of the dynamic obstacle, the construction of the S-T diagram depends on the prediction of the track of the dynamic obstacle, and the track of the dynamic obstacle is predicted by adopting a constant speed model:

Wherein the initial position of the obstacle is an obstacle prediction track with the (s ₀,l₀),(s_t,l_t) time t;

The dynamic obstacle is projected onto an S-T diagram, sampling is carried out in the S-T diagram, a series of sampling points, namely a plurality of solutions to be selected, are generated at each moment in the sampling process, a cost function is required to be defined to evaluate the advantages and disadvantages of the solutions to be selected, the safety refers to that the tractor keeps a safety distance with the dynamic obstacle as far as possible in the speed planning process so as to ensure the driving safety, and a safety cost function J ₁ is introduced in the time interval when the obstacle exists:

wherein ω _obs is an obstacle weight coefficient, positive number k _b represents the degree to which the tractor is far away from the dynamic obstacle, S _obs represents the boundary formed by fitting a straight line to the dynamic obstacle, and S _b represents the boundary of the movement of the tractor; if the tractor is affected by a plurality of dynamic obstacles in a certain time period, accumulating a plurality of safety cost functions J ₁;

to minimize the speed and speed change rate of the tractor, a comfort cost function J ₂ is set:

Wherein ω _v and ω _a represent weight coefficients of the speed and the speed change rate, respectively;

The desired speed v _ref is determined by road speed limit, path curvature and other traffic rules without considering the effect of moving obstacles, and defines a desired speed cost function J ₃:

Wherein ω _r represents a weight coefficient deviating from the desired speed;

and integrating the cost function to obtain a cost function of speed planning:

J＝J₁+J₂+J₃(23)

Finally, solving a speed curve with the lowest cost in the S-T diagram by utilizing a dynamic programming algorithm so as to determine a speed decision mode of the tractor when facing the dynamic obstacle; and transforming the relevant position and speed information from an S-T diagram of the Frenet coordinate system to an X-Y diagram of the Cartesian coordinate system, and obtaining the expected speed for effectively avoiding the dynamic obstacle.