CN110749333B - Unmanned vehicle motion planning method based on multi-objective optimization - Google Patents
Unmanned vehicle motion planning method based on multi-objective optimization Download PDFInfo
- Publication number
- CN110749333B CN110749333B CN201911081063.3A CN201911081063A CN110749333B CN 110749333 B CN110749333 B CN 110749333B CN 201911081063 A CN201911081063 A CN 201911081063A CN 110749333 B CN110749333 B CN 110749333B
- Authority
- CN
- China
- Prior art keywords
- path
- unmanned vehicle
- vehicle
- individual
- cost
- 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.)
- Active
Links
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
- G01C21/34—Route searching; Route guidance
- G01C21/3446—Details of route searching algorithms, e.g. Dijkstra, A*, arc-flags, using precalculated routes
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
The invention discloses a multi-objective optimization-based unmanned vehicle motion planning method, which comprises the steps of mapping a vehicle and an environment from a Cartesian coordinate system to a Frenet coordinate system; establishing a mathematical model of the unmanned vehicle multi-target path planning problem; planning a path by using a linear dynamic planning method; describing a track by using a piecewise fifth-order polynomial, respectively taking a path nearest obtained by linear dynamic planning of track slope minimum, curvature minimum, riding comfort and distance as an optimization target, taking positions of connecting points of the piecewise fifth-order polynomial, first derivative, second derivative and third derivative as equality constraints, and taking a road natural boundary constraint and an obstacle boundary constraint as inequality constraints, and establishing a mathematical model of a multi-target track generation problem of the unmanned vehicle; and obtaining the optimal solution of the unmanned vehicle multi-target track generation problem. The invention solves the problem that the path obtained by the path planning method based on random point scattering is difficult to conform to the kinematic constraint of the vehicle.
Description
Technical Field
The invention belongs to the technical field of intelligent driving and planning thereof, and particularly relates to a multi-objective optimization-based unmanned vehicle motion planning method.
Background
Since more than one hundred times of the introduction of 1886, the automobile field has rapidly developed, and the technical development and the popularity of the automobile field become important marks for measuring the modernization degree and the civilization degree of a country or a region. However, with the popularity and exponential growth of automobiles, people are also suffering from increasingly severe traffic congestion and serious annoyances resulting from accidents. Therefore, intelligent driving technology has gained attention from people. Urban road environment, the road conditions of traveling are simple relatively, have the higher map of precision, consequently change and realize intelligent driving more easily. The national key research and development plan of 2017 clearly lists the automatic driving of the new energy electric automobile as a major common key technology.
Motion planning is a key technology of intelligent driving, and can be understood as follows in a broad sense: in order to realize safe and collision-free intelligent driving in a dynamic environment, the intelligent vehicle needs to predict the motion of the environmental vehicle so as to plan the future track of the intelligent vehicle. The motion planning includes path planning and trajectory generation. In the aspect of path planning, the search for the optimal path is mainly divided into an incremental search mode and a local search mode. Incremental search refers to a technique that does not fully specify the search configuration or state and uses a priori information to increase the search speed, and in recent years, research on incremental search has focused on two techniques: (1) fast-searching Random Trees (RRT); (2) lattice plans (Lattice planers, LP). In terms of trajectory generation, the trajectory has various expressions, including arcs, clothoids, Nelson polynomials, helical polynomials, splines, bezier curves, and the like. The automatic driving is a necessary product for the development of the intelligent era and the internet era, and the popularization of the automatic driving is a breakthrough in vehicle. On one hand, the development of public transportation and automatic driving is greatly promoted by national policies; on the other hand, automatic driving is easier to realize due to the fixed vehicle line. However, in the traditional path planning method based on random scattering points, such as RRT, the obtained path does not meet the vehicle kinematics and dynamics constraints; according to the traditional trajectory generation method based on curve fitting, multi-objective weighting in the multi-objective optimization problem is converted into the multi-objective optimization problem, and the weight is difficult to determine. Therefore, the path and trajectory generation method of the urban unmanned vehicle is in urgent need of further research.
Disclosure of Invention
The invention aims to solve the technical problem that aiming at the defects of the prior art, the invention provides the unmanned vehicle motion planning method based on multi-objective optimization, so that the generated path and track are more in line with the actual road constraint. In order to solve the technical problems, the technical scheme adopted by the invention is as follows: a multi-objective optimization-based unmanned vehicle motion planning method comprises the following steps:
1) mapping the vehicle and the environment from a Cartesian coordinate system to a Frenet coordinate system;
2) under a Frenet coordinate system, taking the weighted sum of the smooth cost, the barrier cost and the reference line cost as an evaluation index, and establishing a mathematical model of the unmanned vehicle multi-target path planning problem;
3) performing path planning by using a linear dynamic planning method, and solving a mathematical model of the multi-target path planning problem to obtain a path;
4) describing a track by using a piecewise fifth-order polynomial, respectively taking the path closest to linear dynamic programming, the track slope minimum, the curvature minimum and the riding experience comfort as optimization targets, taking the position, the first derivative, the second derivative and the third derivative at a connecting point of the piecewise fifth-order polynomial as equality constraints, and taking a road natural boundary constraint and an obstacle boundary constraint as inequality constraints, and establishing a mathematical model of the unmanned vehicle multi-target track generation problem;
5) and solving the mathematical model of the multi-target track generation problem by using the NSGA II to obtain the optimal solution of the multi-target track generation problem of the unmanned vehicle.
In step 1), the coordinates (s, l, l', l ") of the obstacle or vehicle in the Frenet coordinate system are calculated using the following formula:
wherein, (x, y, theta, kappa, v, a) is the position, heading, curvature, speed, acceleration of the obstacle or vehicle in the cartesian coordinate system; (s, l, l') is the coordinate (x, y, θ, κ, v, a) mapped to the Frenet coordinate system; (s, l, l', l ") is the distance along the reference line, the lateral distance from the reference line, the derivative of the lateral distance, the second derivative; Δ θ ═ θ - θr；(xr,yr,θr,κr) Is the closest point(s) to the obstacle or vehicle (x, y, theta, kappa, v, a)r,lr) A corresponding cartesian coordinate system.
In step 2), in the mathematical model of the multi-objective path planning problem, the cost function expression is as follows: ctotal(f(s))＝Csmooth(f)+Cobs(f)+Cguidance(f) (ii) a Wherein f(s) is a fifth order polynomial, Csmooth(f) For smoothing costs, Cobs(f) At a cost of obstacles, Cguidance(f) Is the reference line cost.
The expression of the smoothing cost is as follows:
wherein, w1，w2，w3Cost weights, s, of first, second and third derivatives, respectively0、seRespectively as the abscissa values of the starting point and the end point of the path;
the expression for the obstacle cost is as follows:
where d is the distance between the vehicle and the known obstacle, dnTo an absolute safety distance, dcAs a dangerous distance, Cnudge、CcollisionIs a constant;
the reference line cost expression is as follows:
where g(s) is a fifth order polynomial representing the reference line.
In step 3), the specific implementation process of performing path planning by using the linear dynamic planning method includes:
1) calculating the total cost from the starting point to each point by using columns as stages, and recording the father node of each point;
2) and finding out the path point with the minimum cost in the last column, reversely searching along the father path point until the current position of the vehicle, wherein the obtained group of path points and the quintic polynomial among the path points are the results of path planning by using a linear dynamic planning method.
In step 4), the mathematical model of the unmanned vehicle multi-target track generation problem comprises an optimization target, equality constraint and inequality constraint, and the establishment process of the mathematical model comprises the following steps:
the path nearest obtained by the minimum track slope, the minimum curvature, the comfortable riding experience and the linear dynamic programming is respectively taken as an optimization target:
wherein h(s) is a fifth order polynomial representing the trajectory;
taking the position, the first derivative, the second derivative and the third derivative at the connecting point of the piecewise fifth-order polynomial as equality constraint:
wherein s isieRepresents the abscissa, s, of the end of the i-th segment(i+1)0Represents the abscissa of the starting point of the (i + 1) th segment;
determining constraints of the road natural boundary and the barrier boundary during trajectory planning by taking the constraint of the road natural boundary and the constraint of the barrier boundary as inequality constraints; wherein the left road boundary or obstacle boundary l is formed when the vehicle is runningleft corner boundSatisfies the following conditions: h(s)1)+sin(θ)l+w/2≤h(s1)+h'(s1)l+w/2≤lleft corner bound(ii) a Longitudinal coordinate l of left front vertex of vehicleleft front corner＝h(s1) + sin (θ) l + w/2; wherein, h(s)1)、h'(s1) Each represents s ═ s1The ordinate and the slope of the time trajectory, theta is the included angle between the vehicle and the reference line, and l and w are the vehicle length and the vehicle width respectively.
In the step 5), the specific implementation process for obtaining the optimal solution of the unmanned vehicle multi-target track generation problem comprises the following steps:
1) determining a fifth-order polynomial track coefficient and coding;
2) randomly initializing an initial population with the scale of N, wherein each individual in the population conforms to equality constraint and inequality constraint of a multi-target track generation problem mathematical model of the unmanned vehicle;
3) calculating the objective function value S of each individual in the initial population1、S2、S3、S4；
4) Any given two decision variables Bi、BjIf satisfied with
All have Sk(Bi)≤Sk(Bj) Is established, and
so that Sk(Bi)＜Sk(Bj) Then call BiDominating Bj(ii) a Calculating the dominated number N of each individual p in the initial population NpAnd a set S of decision variables governed by the individualp；1≤i,j≤N；
5) For each objective function value SkK belongs to 1,2,3 and 4, the individuals in each hierarchy are sorted by the objective function value, two individual crowdedness degrees with the maximum and minimum objective function values after sorting are set as infinity, and the distance between the rest individual crowdedness degrees is set as infinity
Wherein idIndicates the crowdedness distance of the ith individual,
the k-th objective function value of the (i + 1) -th individual after sorting,
the kth objective function value of the i-1 th individual after sorting;
6) selecting, crossing and mutating the decision variable population to generate a child population with the size of N, wherein each individual in the population conforms to equality constraint and inequality constraint of a mathematical model for generating a problem by the multi-target track of the unmanned vehicle;
7) judging whether the iteration times reach the specified times, if so, saving the optimized individuals and ending; otherwise, entering step 8);
8) merging the parent offspring population to generate a new population with the scale of 2N; calculating the objective function value of each individual in the new population; performing rapid non-dominated sorting layering and congestion degree distance calculation; and selecting N individuals with lower layering or same layering and larger crowding distance to form a new population, and returning to the step 4). The specific implementation process of the step 4) comprises the following steps:
A) initializing i to 1;
B) find all n in the populationpThe individual is 0, namely the ith layer;
C) for each individual in the ith layer, traversing each individual q in the decision variable set, and executing n* q＝nq-1；
D) Add 1 to the value of i and return to step 2) until the entire population is stratified.
Compared with the prior art, the invention has the beneficial effects that:
1. the invention converts the vehicle, the barrier and the like from a Cartesian coordinate system to a Frenet coordinate system, so that the generated path and the generated track are more in line with the actual road constraint.
2. And carrying out horizontal and longitudinal sampling along the road direction, connecting by using multiple sections of fifth-order polynomials, and obtaining a path which accords with the kinematic constraint of the vehicle by using linear dynamic programming. And the path obtained by the traditional path planning method based on random point scattering is difficult to conform to the kinematic constraint of the vehicle.
3. According to the method, multiple targets such as safety, curvature, comfort and the like are directly considered, and the NSGA II method is used for solving the track generation model; the problem that the weight is difficult to determine when a weighted multi-objective optimization method is adopted is solved.
Drawings
FIG. 1 is a flow chart of the method of the present invention;
FIG. 2 is a vehicle layout view;
FIG. 3 is a vehicle path plan;
FIG. 4 is a linearized vehicle model;
FIG. 5 is a NSGA II algorithm solving flow.
Detailed Description
The intelligent vehicle modified by the bus with the length of 12m and the width of 2.5m is provided with an infrared receiving and transmitting device, a laser radar, a millimeter wave radar, a camera and a GPS/IMU system.
An NSGA II algorithm-based unmanned vehicle motion planning method based on multi-objective optimization under an urban road environment comprises the following steps:
the method comprises the following steps: mapping the vehicle and the environment from a Cartesian coordinate system to a Frenet coordinate system, and performing the following path planning and track generation under the coordinate system;
step two: taking the weighted sum of the smooth cost, the barrier cost and the reference line cost as an evaluation index, and establishing a mathematical model of the unmanned vehicle multi-target path planning problem;
step three: performing path planning by using a linear dynamic planning method, and solving a mathematical model of a multi-target path planning problem to obtain a path;
step four: describing a track by using a piecewise fifth-order polynomial, respectively taking a path nearest obtained by linear dynamic planning of track slope minimum, curvature minimum, riding comfort and distance as an optimization target, taking positions of connecting points of the piecewise fifth-order polynomial, first derivative, second derivative and third derivative as equality constraints, and taking a road natural boundary constraint and an obstacle boundary constraint as inequality constraints, and establishing a mathematical model of a multi-target track generation problem of the unmanned vehicle;
step five: and solving the multi-target track generation problem mathematical model by using the NSGA II to obtain the optimal solution of the unmanned vehicle multi-target track generation problem.
In the first step, the specific process of mapping the vehicle and the environment from the cartesian coordinate system to the Frenet coordinate system is as follows: the Frenet coordinate system enables the scene to be easier to understand, and the motion relation of different objects on the road can be more intuitively reflected. In a cartesian coordinate system, an obstacle or a self-vehicle can be described as a combination of position, heading, curvature, speed, acceleration (x, y, θ, κ, v, a). Mapped to the Frenet coordinate system as (s, l, l', l "), which represents the distance along the reference line, the lateral distance from the reference line, and the derivative, second derivative, of the lateral distance, respectively. The reference line is generally a road middle line or a global path collected in advance.
First, a point(s) closest to an obstacle or a host vehicle (x, y, θ, κ, v, a) is found on a reference liner,lr) Corresponding to the Cartesian coordinate System (x)r,yr,θr,κr). Calculating coordinates (s, l, l') of the obstacle or the vehicle in a Frenet coordinate system:
s＝sr
l'＝(1-κrl)tanΔθ
wherein Δ θ is θ - θr。
In the second step, the mathematical model of the unmanned vehicle multi-target path planning problem is specifically established as follows: (1) describing paths by piecewise fifth-order polynomials
In the Frenet coordinate system, the path points are sampled at equal intervals in the horizontal and vertical directions. And the sampling points of adjacent columns are connected by a fifth-order polynomial. The ith segment of the path may be denoted as fi(s)＝ai0+ai1s+ai2s2+ai3s3+ai4s4+ai5s5Wherein a isi0-ai5Is the coefficient of the ith segment of the path function.
(2) Taking the weighted sum of the smooth cost, the obstacle cost and the reference line cost as an evaluation index
The position, the first derivative and the second derivative are required to be equal at the joint. The method comprehensively considers the vehicle kinematic constraint and the traffic rule, and the cost function consists of three parts:
Ctotal(f(s))＝Csmooth(f)+Cobs(f)+Cguidance(f)
wherein f(s) is a fifth order polynomial, Csmooth(f) For smoothing costs, Cobs(f) At a cost of obstacles, Cguidance(f) Is the reference line cost.
The smoothing cost is composed of a first derivative, a second derivative and a third derivative:
wherein, w1，w2，w3The cost weights of the first derivative, the second derivative and the third derivative are respectively obtained according to experience, and the range 5000 is more than or equal to w1≥10000、1≥w2≥10、0.1≥w3Not less than 1, in this example, w is the value1＝8000、w2＝5、w3＝0.5。s0、seRespectively, the abscissa values of the starting point and the ending point of the path.
The obstacle cost is as follows:
where d is the distance between the vehicle and the known obstacle, dnTo an absolute safety distance, dcAs a dangerous distance, Cnudge、CcollisionIs a constant, the empirical value d is taken in this examplen＝10、dc＝0.5、Cnudge＝20、Ccollision100000000. When d > dnThe vehicle is safe, and the barrier cost is 0; when d isc＜d＜dnThe barrier cost decreases with increasing distance; when d < dcWhen the collision happens, the obstacle cost is very large constant Ccollision。
The reference line cost is as follows:
where g(s) is a fifth order polynomial representing the reference line. The cost is the distance cost of the planned path from the reference line. When there is no obstacle, the planned path should be close to the reference line, which is typically the road center line.
In the third step, the specific steps of planning the path by using the linear dynamic programming method are as follows:
dynamic programming is one approach, one method to solve the optimization problem. Linear dynamic programming algorithms are typically used to solve problems with some optimal nature. In such problems, there may be many possible solutions. Each solution corresponds to a value in order to find the solution with the optimal value. The basic idea of the linear dynamic programming algorithm is to decompose a problem to be solved into a plurality of sub-problems, the sub-problems obtained through decomposition are often not mutually independent, the sub-problems are solved first, and then the solution of the original problem is obtained through the solution of the sub-problems. By saving the answers of the solved subproblems and finding out the obtained answers when needed, a large amount of repeated calculation is avoided, and the time is saved.
Firstly, taking a path point as a decision object; then, since the total cost from the starting point to all the points in the i-1 column needs to be utilized when calculating the total cost from the starting point to the ith column point, the total cost calculation from the starting point to the scattered point of each column path is taken as a stage; taking the total cost from the path point to the starting point as a state variable of each stage; and finally, establishing the transfer process of the state variables of each stage. The method comprises the following specific steps:
first, the total cost from the starting point to each point is calculated in stages by columns, and the parent node of each point is recorded. With Ci,jRepresents the total cost from the starting point to the point of the ith (0. ltoreq. i.ltoreq.M) column j (0. ltoreq. j.ltoreq.N) row. Ci,j＝min(Ci-1,k+Ctotal(f (s)) where k is 0. ltoreq. N, f(s) is a fifth order polynomial between the path point (i-1, k) and the path point (i, j), Ctotal(f (s)) is the cost of the fifth order polynomial. If k is m, Ci,jTake the minimum value, thenThe parent waypoint of the waypoint (i, j) is set to (i-1, m).
And then, finding out the path point with the minimum cost in the last column, reversely searching along the father path point until the current position of the vehicle, wherein the obtained group of path points and the quintic polynomial among the path points are the results of path planning by using a linear dynamic planning method.
In the fourth step, the mathematical model of the unmanned vehicle multi-target trajectory generation problem comprises an optimization target, equality constraints and inequality constraints, and the mathematical model is specifically established by the following steps:
(1) the path nearest obtained by the minimum track slope, the minimum curvature, the comfortable riding experience and the linear dynamic programming is respectively taken as an optimization target:
where h(s) is a fifth order polynomial representing the trajectory. The first three terms represent the degree of path smoothness and the last term represents the distance cost from the path.
(2) Taking the position, the first derivative, the second derivative and the third derivative at the connecting point of the piecewise fifth-order polynomial as equality constraint:
wherein s isieIndicates the i-th section transverse endCoordinate, s(i+1)0Represents the abscissa of the start of the i +1 th segment.
(3) Using natural boundary constraint of road and boundary constraint of obstacle as inequality constraint
And adding a semicircle at the front and the back of the vehicle to convert the nonlinear constraint into linear constraint. If the midpoint of the tail of the vehicle is used as a positioning point, when the included angle between the vehicle and the reference line is positive, the vertical coordinate of the vertex of the left front of the vehicle can be expressed as follows:
lleft front corner＝h(s1)+sin(θ)l+w/2
wherein, h(s)1) When s is equal to s1And a time track ordinate, theta is an included angle between the vehicle and the reference line, and l and w are the vehicle length and the vehicle width respectively. When θ is small, sin (θ) can be approximated by h'(s)1) Represents:
h(s1)+sin(θ)l+w/2≤h(s1)+h'(s1)l+w/2≤lleft corner bound
wherein lleft corner boundIndicating the left road boundary or obstacle boundary as the vehicle travels. The equation represents the constraints of the natural boundary and the obstacle boundary of the road in the trajectory planning.
Similarly, the constraints of other three angles can be obtained.
In the fifth step, the NSGA II is used for solving the mathematical model, and the step of obtaining the optimal solution of the unmanned vehicle multi-target track generation problem is as follows:
aiming at the above described problem of generating the multi-target track of the unmanned vehicle, an NSGA II algorithm is designed for calculation and solution: using trajectory quintic polynomial coefficient vector B ═ B0,b1,b2,b3,b4,b5]As decision variables, the decision variables are independent of one another; the method comprises the steps of taking the path closest obtained by the minimum track slope, the minimum curvature, the comfortable riding experience and the linear dynamic planning as an independent optimization target, taking the position of a piecewise fifth-degree polynomial connecting point, first derivative, second derivative and third derivative as equality constraints, and taking the road natural boundary constraint and the obstacle boundary constraint as inequality constraints. Using floating-point number coding, using arithmeticA number cross operator and a gaussian mutation operator.
The method comprises the following steps: determining parameters to be optimized, namely fifth-order polynomial trajectory coefficients and encoding;
step two: randomly initializing an initial population with the scale of N, wherein each individual in the population conforms to equality constraint and inequality constraint of a multi-target track generation problem mathematical model of the unmanned vehicle;
step three: calculating the objective function value S of each individual in the population1、S2、S3、S4；
Step four: fast non-dominated ordering hierarchy: any given two decision variables Bi、Bj(1. ltoreq. i, j. ltoreq. N), if satisfied for
All have Sk(Bi)≤Sk(Bj) Is established, and
so that Sk(Bi)＜Sk(Bj) Then call BiDominating Bj. Calculating the dominated number N of each individual p in the population NpAnd a set S of decision variables governed by the individualp。
Layering: (1) the initialization i is 1. (2) Find all n in the populationpThe i-th layer is the individual 0. (3) For each individual in the ith layer, traversing each individual q in the decision variable set, and executing nq＝nq-1. (4) i ═ i + 1. Repeating the steps (2), (3) and (4) until the whole population is stratified.
Step five: and (3) calculating the crowding degree distance: for each target Sk(k ∈ 1,2,3,4), sorting the objective function values of the individuals in each hierarchy, setting the two individual congestion degrees with the largest and the smallest objective function values after sorting to ∞, and setting the remaining individual congestion degree distances to infinity
Wherein idIndicates the crowdedness distance of the ith individual,
the k-th objective function value of the (i + 1) -th individual after sorting,
the kth objective function value of the i-1 th individual after sorting;
step six: genetic operation, including selecting, crossing and mutating the decision variable population, generating the offspring population with the size of N, wherein each individual in the population conforms to the equality constraint and the inequality constraint of the unmanned vehicle multi-target track generation problem mathematical model;
step seven: and judging whether the iteration times reach the specified times. If yes, saving the optimized individuals and finishing; otherwise, entering step eight;
step eight: an elite strategy comprising: merging the parent offspring population to generate a new population with the scale of 2N; calculating the objective function value of each individual in the new population; performing rapid non-dominated sorting layering and congestion degree distance calculation; and selecting N individuals with lower layering or same layering and larger crowding distance to form a new population. And entering the step four.
Claims (8)
1. A multi-objective optimization-based unmanned vehicle motion planning method is characterized by comprising the following steps:
1) mapping the vehicle and the environment from a Cartesian coordinate system to a Frenet coordinate system;
2) under a Frenet coordinate system, taking the weighted sum of the smooth cost, the barrier cost and the reference line cost as an evaluation index, and establishing a mathematical model of the unmanned vehicle multi-target path planning problem;
3) performing path planning by using a linear dynamic planning method, and solving a mathematical model of the multi-target path planning problem to obtain a path;
4) describing a track by using a piecewise fifth-order polynomial, respectively taking the path closest to linear dynamic programming, the track slope minimum, the curvature minimum and the riding experience comfort as optimization targets, taking the position, the first derivative, the second derivative and the third derivative at a connecting point of the piecewise fifth-order polynomial as equality constraints, and taking a road natural boundary constraint and an obstacle boundary constraint as inequality constraints, and establishing a mathematical model of the unmanned vehicle multi-target track generation problem;
5) and solving the mathematical model of the multi-target track generation problem by using the NSGA II to obtain the optimal solution of the multi-target track generation problem of the unmanned vehicle.
2. The method for multi-objective optimization-based unmanned vehicle motion planning according to claim 1, wherein in step 1), coordinates (s, l, l', l ") of the obstacle or vehicle in the Frenet coordinate system are calculated using the following formula:
wherein, (x, y, theta, kappa, v, a) is the position, heading, curvature, speed, acceleration of the obstacle or vehicle in the cartesian coordinate system; (s, l, l') is the coordinate (x, y, θ, κ, v, a) mapped to the Frenet coordinate system; (s, l, l', l ") is the distance along the reference line, the lateral distance from the reference line, the derivative of the lateral distance, the second derivative; Δ θ ═ θ - θr；(xr,yr,θr,κr) Is the closest point(s) to the obstacle or vehicle (x, y, theta, kappa, v, a)r,lr) A corresponding cartesian coordinate system.
3. The multi-objective optimization-based unmanned vehicle motion planning method according to claim 1, wherein in the step 2), in the mathematical model of the multi-objective path planning problem, the cost function expression is as follows: ctotal(f(s))＝Csmooth(f)+Cobs(f)+Cguidance(f) (ii) a Wherein f(s) is a fifth order polynomial, Csmooth(f) For smoothing costs, Cobs(f) At a cost of obstacles, Cguidance(f) Is the reference line cost.
4. The multi-objective optimization-based unmanned vehicle motion planning method of claim 3, wherein the smoothing cost is expressed as follows:
wherein, w1，w2，w3Cost weights, s, of first, second and third derivatives, respectively0、seRespectively as the abscissa values of the starting point and the end point of the path;
the expression for the obstacle cost is as follows:
where d is the distance between the vehicle and the known obstacle, dnTo an absolute safety distance, dcAs a dangerous distance, Cnudge、CcollisionIs a constant;
the reference line cost expression is as follows:
where g(s) is a fifth order polynomial representing the reference line.
5. The unmanned vehicle motion planning method based on multi-objective optimization of claim 3, wherein the specific implementation process of performing path planning by using the linear dynamic planning method in the step 3) comprises:
1) calculating the total cost from the starting point to each point by using columns as stages, and recording the father node of each point;
2) and finding out the path point with the minimum cost in the last column, reversely searching along the father path point until the current position of the vehicle, wherein the obtained group of path points and the quintic polynomial among the path points are the results of path planning by using a linear dynamic planning method.
6. The unmanned vehicle motion planning method based on multi-objective optimization of claim 4, wherein in the step 4), the mathematical model of the unmanned vehicle multi-objective trajectory generation problem comprises an optimization objective, an equality constraint and an inequality constraint, and the establishment process of the mathematical model comprises:
the path nearest obtained by the minimum track slope, the minimum curvature, the comfortable riding experience and the linear dynamic programming is respectively taken as an optimization target:
wherein h(s) is a fifth order polynomial representing the trajectory;
taking the position, the first derivative, the second derivative and the third derivative at the connecting point of the piecewise fifth-order polynomial as equality constraint:
wherein s isieRepresents the abscissa, s, of the end of the i-th segment(i+1)0Represents the abscissa of the starting point of the (i + 1) th segment;
determining constraints of the road natural boundary and the barrier boundary during trajectory planning by taking the constraint of the road natural boundary and the constraint of the barrier boundary as inequality constraints; wherein the left road boundary or obstacle boundary l is formed when the vehicle is runningleftcornerboundSatisfies the following conditions: h(s)1)+sin(θ)l+w/2≤h(s1)+h'(s1)l+w/2≤lleftcornerbound(ii) a Longitudinal coordinate l of left front vertex of vehicleleftfrontcorner＝h(s1) + sin (θ) l + w/2; wherein, h(s)1)、h'(s1) Each represents s ═ s1The ordinate and the slope of the time trajectory, theta is the included angle between the vehicle and the reference line, and l and w are the vehicle length and the vehicle width respectively.
7. The unmanned vehicle motion planning method based on multi-objective optimization of claim 1, wherein in the step 5), the specific implementation process for obtaining the optimal solution of the unmanned vehicle multi-objective trajectory generation problem comprises:
1) determining a fifth-order polynomial track coefficient and coding;
2) randomly initializing an initial population with the scale of N, wherein each individual in the population conforms to equality constraint and inequality constraint of a multi-target track generation problem mathematical model of the unmanned vehicle;
3) calculating the objective function value S of each individual in the initial population1、S2、S3、S4；
4) Any given two decision variables Bi、BjIf satisfied with
All have Sk(Bi)≤Sk(Bj) Is established, and
so that Sk(Bi)＜Sk(Bj) Then call BiDominating Bj(ii) a Calculating the dominated number N of each individual p in the initial population NpAnd a set S of decision variables governed by the individualp；1≤i,j≤N；
5) For each objective function value SkK belongs to 1,2,3 and 4, the individuals in each hierarchy are sorted by the objective function value, two individual crowdedness degrees with the maximum and minimum objective function values after sorting are set as infinity, and the distance between the rest individual crowdedness degrees is set as infinity
Wherein idIndicates the crowdedness distance of the ith individual,
to sequence inThe (i + 1) th individual k-th objective function value,
the kth objective function value of the i-1 th individual after sorting;
6) selecting, crossing and mutating the decision variable population to generate a child population with the size of N, wherein each individual in the population conforms to equality constraint and inequality constraint of a mathematical model for generating a problem by the multi-target track of the unmanned vehicle;
7) judging whether the iteration times reach the specified times, if so, saving the optimized individuals and ending; otherwise, entering step 8);
8) merging the parent offspring population to generate a new population with the scale of 2N; calculating the objective function value of each individual in the new population; performing rapid non-dominated sorting layering and congestion degree distance calculation; and selecting N individuals with lower layering or same layering and larger crowding distance to form a new population, and returning to the step 4).
8. The multi-objective optimization-based unmanned vehicle motion planning method according to claim 7, wherein the specific implementation process of the step 4) comprises the following steps:
A) initializing i to 1;
B) find all n in the populationpThe individual is 0, namely the ith layer;
C) for each individual in the ith layer, traversing each individual q in the decision variable set, and executing n* q＝nq-1；
D) Add 1 to the value of i and return to step 2) until the entire population is stratified.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201911081063.3A CN110749333B (en) | 2019-11-07 | 2019-11-07 | Unmanned vehicle motion planning method based on multi-objective optimization |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201911081063.3A CN110749333B (en) | 2019-11-07 | 2019-11-07 | Unmanned vehicle motion planning method based on multi-objective optimization |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN110749333A CN110749333A (en) | 2020-02-04 |
| CN110749333B true CN110749333B (en) | 2022-02-22 |
Family
ID=69282545
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201911081063.3A Active CN110749333B (en) | 2019-11-07 | 2019-11-07 | Unmanned vehicle motion planning method based on multi-objective optimization |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN110749333B (en) |
Families Citing this family (44)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN113361152A (en) * | 2020-03-05 | 2021-09-07 | 北京京东乾石科技有限公司 | Trajectory planning method, trajectory planning device, storage medium, and electronic apparatus |
| CN111627204B (en) * | 2020-03-10 | 2021-08-17 | 蘑菇车联信息科技有限公司 | Path determining method and device, electronic equipment and storage medium |
| CN111580493B (en) * | 2020-04-14 | 2022-08-30 | 吉利汽车研究院（宁波）有限公司 | Automatic driving simulation method, system and medium |
| CN111552284A (en) * | 2020-04-20 | 2020-08-18 | 宁波吉利汽车研究开发有限公司 | Method, device, equipment and medium for planning local path of unmanned vehicle |
| CN111750887B (en) * | 2020-06-11 | 2023-11-21 | 上海交通大学 | Unmanned vehicle track planning method and system for reducing accident severity |
| CN111678523B (en) * | 2020-06-30 | 2022-05-17 | 中南大学 | Rapid BI _ RRT obstacle avoidance trajectory planning method based on STAR algorithm optimization |
| US20220011775A1 (en) * | 2020-07-13 | 2022-01-13 | Baidu Usa Llc | Random shift based path centering system for autonomous vehicles |
| CN112068545B (en) * | 2020-07-23 | 2022-12-27 | 哈尔滨工业大学（深圳） | Method and system for planning running track of unmanned vehicle at crossroad and storage medium |
| CN111964682B (en) * | 2020-08-10 | 2022-03-08 | 北京轩宇空间科技有限公司 | Fast path planning method and device adapting to unknown dynamic space and storage medium |
| CN113804207B (en) * | 2020-09-14 | 2024-06-18 | 北京京东乾石科技有限公司 | Vehicle path planning method, system, equipment and storage medium |
| CN113778071A (en) * | 2020-09-17 | 2021-12-10 | 北京京东乾石科技有限公司 | Unmanned vehicle path planning method and device, electronic equipment, unmanned vehicle and medium |
| CN113804196B (en) * | 2020-09-17 | 2024-04-12 | 北京京东乾石科技有限公司 | Unmanned vehicle path planning method and related equipment |
| CN113804208B (en) * | 2020-09-18 | 2024-05-17 | 北京京东乾石科技有限公司 | Unmanned vehicle path optimization method and related equipment |
| CN112146667B (en) * | 2020-09-29 | 2022-10-14 | 广州小鹏自动驾驶科技有限公司 | Method and device for generating vehicle transition track |
| CN112182893B (en) * | 2020-09-30 | 2023-07-18 | 湘潭大学 | Roadway plane movable type stereo garage parking space allocation optimization method |
| CN112362074B (en) * | 2020-10-30 | 2024-03-19 | 重庆邮电大学 | Intelligent vehicle local path planning method under structured environment |
| CN112363504B (en) * | 2020-11-06 | 2024-04-12 | 黑龙江惠达科技发展有限公司 | Unmanned turning method for agricultural machinery |
| CN113008222B (en) * | 2021-02-20 | 2023-03-31 | 西北工业大学 | Track constraint target tracking method based on continuous time track function |
| CN113805578A (en) * | 2021-02-25 | 2021-12-17 | 京东鲲鹏(江苏)科技有限公司 | Unmanned vehicle path optimization method and related equipment |
| CN113031592A (en) * | 2021-02-25 | 2021-06-25 | 杭州国辰机器人科技有限公司 | Robot path smoothing method and system based on fifth-order Bezier curve |
| CN113044029B (en) * | 2021-03-19 | 2022-03-15 | 北京理工大学 | Motion planning method for ensuring safe driving of unmanned vehicle on three-dimensional terrain |
| CN113104049B (en) * | 2021-03-25 | 2022-07-01 | 浙江大学 | Vehicle motion planning system and method using frequency shaping |
| CN113147841A (en) * | 2021-05-13 | 2021-07-23 | 中车长春轨道客车股份有限公司 | Rail vehicle capacity management and energy-saving auxiliary driving method and related device |
| CN113119929A (en) * | 2021-05-24 | 2021-07-16 | 前海七剑科技(深圳)有限公司 | Curve brake control method, curve brake control system, electronic device and storage medium |
| CN113432618A (en) * | 2021-06-16 | 2021-09-24 | 深圳市道通智能汽车有限公司 | Trajectory generation method and apparatus, computing device and computer-readable storage medium |
| CN113483776A (en) * | 2021-07-01 | 2021-10-08 | 广州小鹏自动驾驶科技有限公司 | Path planning method and device and automobile |
| CN113419534B (en) * | 2021-07-01 | 2022-03-08 | 湖南大学 | Bezier curve-based steering road section path planning method |
| CN113654556A (en) * | 2021-07-05 | 2021-11-16 | 的卢技术有限公司 | Local path planning method, medium and equipment based on improved EM algorithm |
| CN113467457A (en) * | 2021-07-08 | 2021-10-01 | 无锡太机脑智能科技有限公司 | Graph optimization path planning method for edge-pasting sweeping of unmanned sanitation vehicle |
| CN113552839B (en) * | 2021-07-23 | 2022-12-13 | 广州小鹏自动驾驶科技有限公司 | Speed optimization method, device and equipment |
| CN113551679A (en) * | 2021-07-23 | 2021-10-26 | 杭州海康威视数字技术股份有限公司 | Map information construction method and device in teaching process |
| CN113485383B (en) * | 2021-09-02 | 2021-12-31 | 北京三快在线科技有限公司 | Control method and control device of unmanned equipment |
| CN115230731A (en) * | 2021-09-13 | 2022-10-25 | 上海仙途智能科技有限公司 | Travel route determination method, travel route determination device, travel route determination terminal, and travel route determination medium |
| CN114001739B (en) * | 2021-10-11 | 2024-04-16 | 广州小鹏自动驾驶科技有限公司 | Path planning method, device, vehicle and storage medium |
| CN114200926B (en) * | 2021-11-12 | 2023-04-07 | 河南工业大学 | Local path planning method and system for unmanned vehicle |
| CN114923496A (en) * | 2022-03-29 | 2022-08-19 | 武汉路特斯汽车有限公司 | Path planning method and device, electronic equipment and storage medium |
| CN114771551B (en) * | 2022-04-29 | 2023-08-11 | 阿波罗智能技术(北京)有限公司 | Automatic driving vehicle track planning method and device and automatic driving vehicle |
| CN114676939B (en) * | 2022-05-26 | 2022-09-02 | 之江实验室 | Multi-vehicle-type parameter self-adaptive reference line smoothing method and system |
| CN115416656B (en) * | 2022-08-23 | 2024-06-04 | 华南理工大学 | Automatic driving lane changing method, device and medium based on multi-target track planning |
| CN115127576B (en) * | 2022-09-02 | 2022-12-13 | 青岛慧拓智能机器有限公司 | Path planning method, device, chip, terminal, electronic equipment and storage medium |
| CN115489548B (en) * | 2022-09-20 | 2024-06-04 | 重庆大学 | Intelligent automobile park road path planning method |
| CN115371685B (en) * | 2022-10-24 | 2023-03-24 | 成都市以太节点科技有限公司 | Method and device for planning dominant path of unmanned equipment in industrial control scene and storage medium |
| CN116147654B (en) * | 2023-04-19 | 2023-07-25 | 广东工业大学 | Path planning method based on offline path library |
| CN117308964B (en) * | 2023-11-24 | 2024-02-13 | 陕西欧卡电子智能科技有限公司 | Path planning method and device for intelligent pleasure boat, unmanned boat and medium |
Family Cites Families (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20050216182A1 (en) * | 2004-03-24 | 2005-09-29 | Hussain Talib S | Vehicle routing and path planning |
| CN104634343B (en) * | 2015-01-27 | 2017-09-26 | 杭州格文数字技术有限公司 | A kind of scenic spot route automatic planning based on multiple-objection optimization |
| CN108931243B (en) * | 2018-04-17 | 2020-12-22 | 哈尔滨工程大学 | UUV path planning method based on energy consumption and sampling amount multi-objective optimization under influence of complex marine environment |
| CN109764882B (en) * | 2018-12-27 | 2022-06-07 | 华侨大学 | Multi-target vehicle path planning method based on self-adaptive local search chain |
| CN109631905B (en) * | 2019-01-18 | 2022-09-06 | 大连理工大学 | NSGA III path planning method based on dynamic reference point |
| CN109813328B (en) * | 2019-02-22 | 2021-04-30 | 百度在线网络技术（北京）有限公司 | Driving path planning method and device and vehicle |
-
2019
- 2019-11-07 CN CN201911081063.3A patent/CN110749333B/en active Active
Also Published As
| Publication number | Publication date |
|---|---|
| CN110749333A (en) | 2020-02-04 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN110749333B (en) | Unmanned vehicle motion planning method based on multi-objective optimization | |
| Sharma et al. | Recent advances in motion and behavior planning techniques for software architecture of autonomous vehicles: A state-of-the-art survey | |
| CN107702716B (en) | Unmanned driving path planning method, system and device | |
| CN113932823A (en) | Unmanned multi-target-point track parallel planning method based on semantic road map | |
| Huang et al. | Conditional predictive behavior planning with inverse reinforcement learning for human-like autonomous driving | |
| CN106371439B (en) | Unified automatic driving transverse planning method and system | |
| CN110928297A (en) | Intelligent bus route planning method based on multi-objective dynamic particle swarm optimization | |
| CN114005280A (en) | Vehicle track prediction method based on uncertainty estimation | |
| Li et al. | Semantic-level maneuver sampling and trajectory planning for on-road autonomous driving in dynamic scenarios | |
| CN116185014A (en) | Intelligent vehicle global optimal track planning method and system based on dynamic planning | |
| Ma et al. | Autorvo: Local navigation with dynamic constraints in dense heterogeneous traffic | |
| Wei et al. | Game theoretic merging behavior control for autonomous vehicle at highway on-ramp | |
| Bertolazzi et al. | Efficient re-planning for robotic cars | |
| Zhang et al. | Structured road-oriented motion planning and tracking framework for active collision avoidance of autonomous vehicles | |
| CN115140096A (en) | Spline curve and polynomial curve-based automatic driving track planning method | |
| Zhang et al. | Lexicographic actor-critic deep reinforcement learning for urban autonomous driving | |
| Xing et al. | Vehicle motion planning with joint cartesian-frenét mpc | |
| Sun et al. | Interactive left-turning of autonomous vehicles at uncontrolled intersections | |
| Nagasaka et al. | Towards safe, smooth, and stable path planning for on-road autonomous driving under uncertainty | |
| Yu et al. | Hierarchical framework integrating rapidly-exploring random tree with deep reinforcement learning for autonomous vehicle | |
| CN112050805B (en) | Path planning method and device, electronic equipment and storage medium | |
| Smit et al. | Informed sampling-based trajectory planner for automated driving in dynamic urban environments | |
| Garrido et al. | A two-stage real-time path planning: Application to the overtaking manuever | |
| Wang et al. | Dynamic path planning algorithm for autonomous vehicles in cluttered environments | |
| Zhao et al. | On-road trajectory planning of connected and automated vehicles in complex traffic settings: a hierarchical framework of trajectory refinement |
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 |