CN118192601A - Unstructured scene-oriented automatic driving tractor path planning method - Google Patents
Unstructured scene-oriented automatic driving tractor path planning method Download PDFInfo
- Publication number
- CN118192601A CN118192601A CN202410463807.2A CN202410463807A CN118192601A CN 118192601 A CN118192601 A CN 118192601A CN 202410463807 A CN202410463807 A CN 202410463807A CN 118192601 A CN118192601 A CN 118192601A
- Authority
- CN
- China
- Prior art keywords
- tractor
- path
- obstacle
- speed
- node
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
- 238000000034 method Methods 0.000 title claims abstract description 61
- 238000010586 diagram Methods 0.000 claims abstract description 39
- 230000003068 static effect Effects 0.000 claims abstract description 12
- 238000005457 optimization Methods 0.000 claims abstract description 10
- 238000006243 chemical reaction Methods 0.000 claims abstract description 9
- 230000008859 change Effects 0.000 claims description 17
- 230000001133 acceleration Effects 0.000 claims description 13
- 238000005070 sampling Methods 0.000 claims description 12
- 238000006073 displacement reaction Methods 0.000 claims description 11
- 230000008569 process Effects 0.000 claims description 11
- 238000005520 cutting process Methods 0.000 claims description 6
- 239000011159 matrix material Substances 0.000 claims description 6
- 239000000725 suspension Substances 0.000 claims description 6
- 230000001131 transforming effect Effects 0.000 claims description 5
- 230000007613 environmental effect Effects 0.000 claims description 4
- 238000009825 accumulation Methods 0.000 claims description 3
- 230000004888 barrier function Effects 0.000 claims description 3
- 238000010276 construction Methods 0.000 claims description 3
- 230000003247 decreasing effect Effects 0.000 claims description 3
- 230000000694 effects Effects 0.000 claims description 3
- 238000013178 mathematical model Methods 0.000 claims description 3
- 238000012216 screening Methods 0.000 claims description 3
- 230000009466 transformation Effects 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000001514 detection method Methods 0.000 description 1
- 235000013399 edible fruits Nutrition 0.000 description 1
- 238000004519 manufacturing process Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000009331 sowing Methods 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 230000007704 transition Effects 0.000 description 1
- 239000002699 waste material Substances 0.000 description 1
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/40—Control within particular dimensions
- G05D1/43—Control of position or course in two dimensions
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/20—Control system inputs
- G05D1/24—Arrangements for determining position or orientation
- G05D1/242—Means based on the reflection of waves generated by the vehicle
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/20—Control system inputs
- G05D1/24—Arrangements for determining position or orientation
- G05D1/243—Means capturing signals occurring naturally from the environment, e.g. ambient optical, acoustic, gravitational or magnetic signals
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/20—Control system inputs
- G05D1/24—Arrangements for determining position or orientation
- G05D1/246—Arrangements for determining position or orientation using environment maps, e.g. simultaneous localisation and mapping [SLAM]
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/60—Intended control result
- G05D1/617—Safety or protection, e.g. defining protection zones around obstacles or avoiding hazards
- G05D1/622—Obstacle avoidance
- G05D1/633—Dynamic obstacles
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/60—Intended control result
- G05D1/644—Optimisation of travel parameters, e.g. of energy consumption, journey time or distance
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/60—Intended control result
- G05D1/648—Performing a task within a working area or space, e.g. cleaning
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T10/00—Road transport of goods or passengers
- Y02T10/10—Internal combustion engine [ICE] based vehicles
- Y02T10/40—Engine management systems
Landscapes
- Engineering & Computer Science (AREA)
- Aviation & Aerospace Engineering (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
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
Technical Field
The invention belongs to the field of automatic driving vehicle path planning, and particularly relates to an unstructured scene-oriented automatic driving tractor path planning method.
Background
The traditional tractor adopts manual driving to complete various operation tasks such as sowing, fertilizing, fruit transportation and the like. However, various obstacles in the actual operation scene cannot be avoided, the phenomena of row leakage, row overlapping and the like are easy to occur in the obstacle avoidance process in the traditional operation mode, and the problems of low production efficiency, land resource waste and the like exist. In recent years, along with the gradual transition of agricultural machinery in China to intelligent transformation, the development of precise agriculture has been on the way. An important premise for implementing precise agriculture is to conduct path planning so as to guide agricultural machinery such as an automatic driving tractor to walk according to a planned working path, namely, to consider the position and layout of obstacles to determine an optimal path.
The autopilot tractor is primarily traveling in unstructured scenes. Because of the lack of clear road marking lines, complex and diverse road conditions, and difficult distinction between road and non-road areas, conventional path planning methods that rely on lane lines or clear road boundaries for sensing and positioning are not suitable. In addition, in unstructured scenes, if obstacle detection and environment sensing are performed by using sensor data such as a camera and a laser radar to determine a safe driving path, implementation is difficult and system cost is increased significantly. At present, a graph search-based method (such as an algorithm A) can be used for dispersing a state space into a graph, and a reference path in an unstructured scene is obtained by using an offline map modeling mode, so that excessive dependence on sensor data is avoided. However, such a scheme does not fully consider the kinematic constraint of the tractor (the path curvature may have abrupt change), and cannot cope with the situation that dynamic obstacles exist in a scene or the motion direction of the dynamic obstacles is not fixed. On the other hand, the online solution of the problems can be realized by means of a numerical optimization method (such as a dynamic programming algorithm). However, an excessive solution range can significantly increase the computational complexity. Therefore, it is necessary to develop an automatic driving tractor path planning method for unstructured scenes, so that the tractor efficiently bypasses the obstacle, and the operation efficiency is improved while the operation safety is ensured.
Disclosure of Invention
The invention aims to: aiming at the defects in the prior art, the invention provides a global reference path searching and local obstacle avoidance planning method based on a mixed A-based algorithm and a dynamic planning algorithm for an agricultural machinery operation scene without lane lines, clear road boundaries and dynamic obstacles, and aims to solve the problem of path planning of an automatic driving tractor for unstructured scenes. First, a global reference path satisfying the tractor kinematic constraint is determined based on a hybrid a-algorithm, and an optimal reference path search is completed with the tractor kinematic constraint and static obstacle taken into account. On the basis, the curvature of the reference path is smoothed by using a dynamic planning algorithm, and the speed planning of the dynamic obstacle is completed by combining the dynamic planning algorithm.
The technical scheme is as follows:
The invention provides an unstructured scene-oriented automatic driving tractor path planning method, which comprises the following steps:
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.
Further, 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.
Further, 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 1 (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,yn) is the coordinates of the current node n, (x g,yg) 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(h1(n,g),h2(n,g)) (4)
Wherein h 2 (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 1 (n) is added in the actual cost g (n):
g1(n)＝ωg1·|vn-vn-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-4 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.
Further, 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 1～M4 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 1N2…Nn 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 1 and f 2 of the point M i positioned outside the convex polygon N 1N2…Nn are obtained by using the formula (7), and the following obstacle avoidance constraint is established:
Wherein, gamma (N 1,…Nn) is a set of convex polygon vertexes, and χ (M 1,…M4) 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.
Further, 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.
Further, the specific method of 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 longitudinal 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,/>For longitudinal acceleration,/>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.
Further, the specific method of 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＝Jobs+Js+Jl (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,dmax 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 0 is the path planning start time, t s is the path planning end time, ω 1、ω2、ω3 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 10～a(N-1)5 represents the coefficient of the polynomial of fifth degree, τ(s) represents the set of the polynomial of fifth degree, s 5 represents the square of s, let s j,lj,l′j,l″j be the start point state information, s k,lk,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～ak5 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.
Further, 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 1S2 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 (s 0,l0), and the transverse and longitudinal speeds are respectively And/>(S t,lt) predicting a track for the obstacle at the 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 1 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 1;
to minimize the speed and speed change rate of the tractor, a comfort cost function J 2 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 3:
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＝J1+J2+J3(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.
The beneficial effects of the invention are as follows:
(1) And (3) providing a mixed A algorithm, and searching a global reference path aiming at a static obstacle to obtain an optimal path from a starting point to an end point, wherein the optimal path meets the kinematic constraint of the electric tractor.
(2) And (3) completing reference path optimization based on the S-L diagram, and completing speed planning based on the S-T diagram, so as to realize local obstacle avoidance optimization considering dynamic obstacles.
Drawings
FIG. 1 is an overall block diagram;
FIG. 2 is a grid map;
fig. 3 is a schematic view of a child node expansion of a mixed a algorithm;
FIG. 4 is a triangle area method;
FIG. 5 is a correspondence between Frenet and Cartesian coordinate systems;
fig. 6 is a projection of a dynamic obstacle onto an S-T map.
Detailed Description
The invention will be further described with reference to the drawings and the specific embodiments, but the scope of the invention is not limited thereto.
Step (1), establishing an environment map model; determining the specific location of a static obstacle
And (3) establishing a grid map, and discretizing the tractor working environment into a plurality of independent square grids, wherein each grid corresponds to a specific position in the real working environment. The corresponding grid is marked as occupied or idle depending on the obstacle situation in the real environment. All marked grids jointly form an environment map model.
In the grid map, the occupied grid is represented by black, and the corresponding value in the numerical matrix is 1; the free grid is represented by white, with a corresponding value of 0 in the numerical matrix, as shown in fig. 2. The mathematical model of the grid map can be expressed 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.
And (2) improving the heuristic function by utilizing a Reeds-Shepp curve model to obtain a cost function of the hybrid A-algorithm.
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 starting node to the current node, and h (n, g) is a heuristic function of the distance 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, nodes which are more likely to be guided to the target are prioritized, so that the searching efficiency is improved; the corresponding distance heuristic function h (n, g) uses Euclidean distance h 1 (n, g) to ignore tractor non-integrity constraints and consider obstacle avoidance environmental constraints:
Where (x n,yn) is the coordinates of the current node n and (x g,yg) is the coordinates of the path-target point.
The Reeds-Shepp curve model consists of two turns and a straight line, can determine the shortest path of a tractor or other moving bodies under given constraint conditions, and can effectively describe different types of movement and steering operations. The distance heuristic function commonly used by the A-algorithm and the Reeds-Shepp curve model are used for parallel setting, so that a hybrid A-algorithm can be obtained, namely, a larger value is selected as the heuristic function value for searching, and meanwhile, the incomplete kinematic constraint and the highest traffic efficiency of the tractor are met:
h(n,g)＝max(h1(n,g),h2(n,g)) (4)
H 2 (n, g) represents a distance heuristic function adopting a Reeds-Shepp curve model, the shortest path from the current node to the target position is obtained under the conditions of ignoring environmental barriers and considering the incomplete constraint of the tractor, and the heuristic function can ensure that the child node is constrained by the front wheel corner during expansion.
In the hybrid a algorithm, each node records the specific speed and front wheel steering angle that its parent node expands to the current node. To prevent frequent changes in vehicle speed v and front wheel steering angle δ f, an additional cost function g 1 (n) is added to the actual cost g (n):
g1(n)＝ωg1·|vn-vn-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.
Step (3), determining constraint conditions of the system, ensuring obstacle avoidance safety, adapting to the motion characteristics of the tractor,
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 1～M4 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 1N2…Nn 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 1 and f 2 of the point M i positioned outside the convex polygon N 1N2…Nn are obtained by using the formula (7), and the following obstacle avoidance constraint is established:
Wherein, gamma (N 1,…Nn) is a set of convex polygon vertexes, and χ (M 1,…M4) 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.
Step (4), sub-node expansion is carried out according to the cost function in step S2 and the system constraint in step S3, a global reference path is obtained, the kinematic constraint of the tractor is considered, and the node expansion mode is changed by adopting a mixed A algorithm: 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.
And (5) converting Frenet coordinates and Cartesian coordinates of the tractor pose information before the local obstacle avoidance optimization based on the dynamic programming algorithm is carried out.
The Frenet coordinate system uses curvature and tangential direction to represent the path, and can accurately describe the path curvature change and the tractor position offset relative to the reference path, and the road can be expressed as a function of the reference line and lateral offset. As shown in fig. 5, the two-dimensional path planning task is simplified into a one-dimensional problem based on an S-L graph, and the mutual positional relationship between the actual path and the reference path is represented by a longitudinal displacement S and a lateral displacement L.
In order to make path planning and tracking control efficient and easy to implement, road description is generally performed in the path planning stage using the Frenet coordinate system, and intuitive and simplified control is described in the tracking control stage using the Cartesian coordinate system. In the Cartesian coordinate system, the motion state of a tractor at a certain point can be expressed as [ x, y, v, ψ, a, κ ]; a is the longitudinal acceleration in cartesian's coordinate system, while in Frenet's coordinate system, the tractor motion state can be expressed asWherein/>For longitudinal speed of vehicle,/>For longitudinal acceleration,/>For transverse speed,/>For lateral acceleration, l 'is the derivative of l over s, l' is the pair/>Is a derivative of (a).
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:
Wherein, And/>Is longitudinal speed and longitudinal acceleration under Frenet coordinate system,/>And/>For lateral vehicle speed and lateral acceleration in Frenet coordinate system, l 'is the derivative of l with s, l' is/>Pair/>Is 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.
And (6) 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.
And the reference path is optimized by adopting a dynamic programming algorithm, and the solving process is shown in fig. 6. First, an S-L diagram in the Frenet coordinate system is constructed according to obstacle information in a scene, and a space is discretized by scattering points in the diagram and initializing. Secondly, a reasonable cost function is set, and a cost value is allocated to each node to describe the path selection relation between adjacent units. The method mainly considers the factors such as the position of the obstacle, the current position of the tractor, the target position and the like, and ensures that the minimum cost path can be found while collision is avoided. The cost function J is described by a safe distance cost function J obs of the path from the static obstacle, a smoothness cost function J s, and a degree of deviation from the reference path cost function J l:
J＝Jobs+Js+Jl (13)
The distance cost function J obs of the path from the static obstacle can be expressed as:
Wherein d max is the maximum collision distance, d min is the minimum collision distance, d min,dmax is the collision buffer zone, f risk is the monotonically decreasing collision risk function, and the closer to an obstacle, the larger the f risk value is, namely the easier the collision risk occurs.
The smoothness cost function J s can be expressed as:
wherein t 0 is the path planning start time, t s is the path planning end time, ω 1、ω2、ω3 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 (a).
The cost function J l of the degree of departure from the reference path can be expressed as:
Where ω l is the off-reference path degree weight coefficient and l r is the reference line function. The reference line may direct the direction of tractor travel, for a structured scene, the road centerline is typically taken as the reference line; for unstructured scenarios, a planned reference path may be selected as a reference line.
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, and storing path information for backtracking. And after all the nodes are calculated, screening to obtain the minimum connection cost of all the path sampling nodes. And finally, carrying out coordinate transformation on the node information of the path, and transforming the related coordinate information from an S-L diagram of a Frenet coordinate system to an X-Y diagram of a Cartesian coordinate system for tracking control.
And interpolating and fitting given nodes by using a penta-polynomial, and serializing discrete nodes to obtain a smooth path curve meeting the requirements. The fifth order polynomial curve can be expressed by the following formula:
Where t represents time, a 10～a(N-1)5 represents the coefficient of the penta-order polynomial, τ(s) represents the set of multi-segment penta-order polynomials, and s 5 represents the fifth power of s. Assuming that the state information of the starting point of the penta-degree polynomial of a certain segment is [ s j,lj,l′j,l″j ], the state information of the ending point is [ s k,lk,l′k,l″k ], the starting point and the state information of the ending point are brought into the formula (17), and the following can be obtained:
wherein a k0～ak5 represents the polynomial coefficient of each segment of fifth degree;
Solving the linear equation set of the equation (18) can determine all coefficients of the polynomial curve of the fifth degree, and the optimization of the reference path is realized.
Step (7) of speed planning based on S-T diagram
The S-T diagram may be used to describe the motion relationship between the tractor and the dynamic obstacle. The speed planning problem can be converted into a feasible node searching problem in the S-T diagram by adopting a dynamic planning algorithm. The speed planning at a certain moment is performed on the premise that the path planning at the certain moment is completed. For dynamic obstacle avoidance, the dynamic obstacle position and the predicted future position need to be projected into the S-T map under the Frenet coordinate system. As shown in fig. 6, the time of cutting into the dynamic obstacle is T in, the time of cutting into the dynamic obstacle is T out, and the length S 1S2 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 can be predicted by adopting a constant speed model:
wherein the initial position of the obstacle is (s 0,l0), and the transverse and longitudinal speeds are respectively And/>(S t,lt) is the obstacle prediction trajectory at time t.
The dynamic obstacle is projected onto the S-T map and sampled in the S-T map. In the sampling process, a series of sampling points, namely a plurality of solutions to be selected, are generated at each moment. In order to obtain the optimal solution, a cost function is defined to evaluate the quality of each solution to be selected. The cost function mainly considers factors such as safety, stability and expected speed.
Safety means that the tractor should keep a safe distance with the dynamic obstacle as much as possible in the speed planning process so as to ensure the safety of running. During the time interval when an obstacle exists, a safety cost function J 1 is introduced:
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 tractor movement. If the tractor is affected by multiple dynamic obstacles for a certain period of time, multiple safety cost functions J 1 may be accumulated.
Stability may be equivalently the smoothness of the speed planning curve, and the speed and speed change rate of the tractor should be minimized in the cost function. The sampling points are connected by a penta-order polynomial, and the acceleration and the change rate of the acceleration in each time interval can be conveniently analyzed and expressed. To minimize the speed and rate of change of speed of the tractor, a comfort cost function J 2 may be set:
where ω v and ω a represent weight coefficients of the speed and the speed change rate, respectively.
The desired speed v ref, irrespective of the moving obstacle effect, is determined by road speed limit, path curvature and other traffic rules, and a corresponding cost function J 3 can be defined:
Where ω r represents a weight coefficient deviating from the desired speed.
And integrating the cost function to obtain a cost function of speed planning:
J＝J1+J2+J3 (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 planning a solution according to the obtained optimal speed, and effectively avoiding dynamic obstacles.
The examples are preferred embodiments of the present invention, but the present invention is not limited to the above-described embodiments, and any obvious modifications, substitutions or variations that can be made by one skilled in the art without departing from the spirit of the present invention are within the 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 1 (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,yn) is the coordinates of the current node n, (x g,yg) 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(h1(n,g),h2(n,g)) (4)
Wherein h 2 (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 1 (n) is added in the actual cost g (n):
g1(n)＝ωg1·|vn-vn-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 1～M4 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 1N2…Nq 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 1 and f 2 of the point M i positioned outside the convex polygon N 1N2…Nn are obtained by using the formula (7), and the following obstacle avoidance constraint is established:
Wherein, gamma (N 1,…Nn) is a set of convex polygon vertexes, and χ (M 1,…M4) 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＝Jobs+Js+Jl (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,dmax 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 0 is the path planning start time, t s is the path planning end time, ω 1、ω2、ω3 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 10～a(N-1)5 represents the coefficient of the polynomial of fifth degree, τ(s) represents the set of the polynomial of fifth degree, s 5 represents the square of s, let s j,lj,l′j,l″j be the start point state information, s k,lk,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～ak5 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 1S2 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 0,l0),(st,lt) 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 1 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 1;
to minimize the speed and speed change rate of the tractor, a comfort cost function J 2 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 3:
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＝J1+J2+J3(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.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202410463807.2A CN118192601B (en) | 2024-04-17 | 2024-04-17 | Unstructured scene-oriented automatic driving tractor path planning method |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202410463807.2A CN118192601B (en) | 2024-04-17 | 2024-04-17 | Unstructured scene-oriented automatic driving tractor path planning method |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN118192601A true CN118192601A (en) | 2024-06-14 |
| CN118192601B CN118192601B (en) | 2024-10-25 |
Family
ID=91403626
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202410463807.2A Active CN118192601B (en) | 2024-04-17 | 2024-04-17 | Unstructured scene-oriented automatic driving tractor path planning method |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN118192601B (en) |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN118584961A (en) * | 2024-07-26 | 2024-09-03 | 浙江有鹿机器人科技有限公司 | Edge attaching method and system of sweeping machine vehicle, electronic equipment and sweeping machine vehicle |
Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN113932823A (en) * | 2021-09-23 | 2022-01-14 | 同济大学 | Unmanned multi-target-point track parallel planning method based on semantic road map |
| CN114779788A (en) * | 2022-05-27 | 2022-07-22 | 昆明理工大学 | Path planning method for improving A-algorithm |
| WO2022193584A1 (en) * | 2021-03-15 | 2022-09-22 | 西安交通大学 | Multi-scenario-oriented autonomous driving planning method and system |
| CN116674529A (en) * | 2023-06-16 | 2023-09-01 | 无锡学院 | Parking path planning and parking method for unstructured scene automatic driving vehicle |
| CN116952244A (en) * | 2023-07-21 | 2023-10-27 | 常州工学院 | Local path planning method and system combining driver cognitive risk and vehicle instability risk |
-
2024
- 2024-04-17 CN CN202410463807.2A patent/CN118192601B/en active Active
Patent Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| WO2022193584A1 (en) * | 2021-03-15 | 2022-09-22 | 西安交通大学 | Multi-scenario-oriented autonomous driving planning method and system |
| CN113932823A (en) * | 2021-09-23 | 2022-01-14 | 同济大学 | Unmanned multi-target-point track parallel planning method based on semantic road map |
| CN114779788A (en) * | 2022-05-27 | 2022-07-22 | 昆明理工大学 | Path planning method for improving A-algorithm |
| CN116674529A (en) * | 2023-06-16 | 2023-09-01 | 无锡学院 | Parking path planning and parking method for unstructured scene automatic driving vehicle |
| CN116952244A (en) * | 2023-07-21 | 2023-10-27 | 常州工学院 | Local path planning method and system combining driver cognitive risk and vehicle instability risk |
Non-Patent Citations (2)
| Title |
|---|
| 李环宇: "四轮转向车辆避障换道轨迹规划与跟踪研究", 中国知网, 21 March 2023 (2023-03-21), pages 27 - 29 * |
| 韩龙飞: "非结构化场景下自动驾驶汽车轨迹规划与运动控制算法研究", 中国优秀硕士学位论文全文数据库, no. 03, 15 March 2022 (2022-03-15), pages 13 - 16 * |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN118584961A (en) * | 2024-07-26 | 2024-09-03 | 浙江有鹿机器人科技有限公司 | Edge attaching method and system of sweeping machine vehicle, electronic equipment and sweeping machine vehicle |
Also Published As
| Publication number | Publication date |
|---|---|
| CN118192601B (en) | 2024-10-25 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN111426330B (en) | Path generation method and device, unmanned transportation system and storage medium | |
| CN107168305B (en) | Bezier and VFH-based unmanned vehicle track planning method under intersection scene | |
| CN109976329B (en) | Planning method for vehicle obstacle avoidance and lane change path | |
| CN113204236B (en) | Intelligent agent path tracking control method | |
| CN112284393B (en) | Global path planning method and system for intelligent mobile robot | |
| CN110954122B (en) | Automatic driving track generation method under high-speed scene | |
| WO2019042295A1 (en) | Path planning method, system, and device for autonomous driving | |
| CN116185014A (en) | Intelligent vehicle global optimal track planning method and system based on dynamic planning | |
| CN111060108A (en) | Path planning method and device and engineering vehicle | |
| CN118192601B (en) | Unstructured scene-oriented automatic driving tractor path planning method | |
| CN111896004A (en) | Narrow passage vehicle track planning method and system | |
| CN114488185B (en) | Robot navigation system method and system based on multi-line laser radar | |
| Kim et al. | A hierarchical motion planning framework for autonomous driving in structured highway environments | |
| Yu et al. | Hierarchical framework integrating rapidly-exploring random tree with deep reinforcement learning for autonomous vehicle | |
| CN114840001A (en) | Multi-vehicle collaborative track planning method in closed environment | |
| CN114889643A (en) | Three-element autonomous obstacle avoidance method for moving obstacle | |
| Chen et al. | A robust trajectory planning method based on historical information for autonomous vehicles | |
| CN117109620A (en) | Automatic driving path planning method based on interaction of vehicle behaviors and environment | |
| CN113815645B (en) | Automatic driving behavior decision system and motion planning method suitable for annular intersection | |
| CN116466708A (en) | Autonomous passenger parking track planning method oriented to complex unstructured scene | |
| CN114460933B (en) | Dynamic environment-oriented mobile robot local path planning algorithm | |
| Shi et al. | An autonomous valet parking algorithm for path planning and tracking | |
| Li et al. | An efficient sampling-based hybrid a* algorithm for intelligent vehicles | |
| Chang et al. | On-road trajectory planning with spatio-temporal informed RRT | |
| Zhang et al. | An NSGA-II-based multi-objective trajectory planning method for autonomous driving |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |