CN116399363A - Track planning method and device for narrow road, electronic equipment and storage medium - Google Patents
Track planning method and device for narrow road, electronic equipment and storage medium Download PDFInfo
- Publication number
- CN116399363A CN116399363A CN202310540544.6A CN202310540544A CN116399363A CN 116399363 A CN116399363 A CN 116399363A CN 202310540544 A CN202310540544 A CN 202310540544A CN 116399363 A CN116399363 A CN 116399363A
- Authority
- CN
- China
- Prior art keywords
- point
- track
- spline curve
- straight line
- road
- 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.)
- Pending
Links
- 238000000034 method Methods 0.000 title claims abstract description 42
- 238000004422 calculation algorithm Methods 0.000 claims abstract description 23
- 238000004364 calculation method Methods 0.000 claims abstract description 9
- 230000011218 segmentation Effects 0.000 claims abstract description 4
- 230000006870 function Effects 0.000 claims description 28
- 238000004891 communication Methods 0.000 claims description 20
- 238000005070 sampling Methods 0.000 claims description 19
- 238000005457 optimization Methods 0.000 claims description 13
- 238000004590 computer program Methods 0.000 claims description 12
- 230000007613 environmental effect Effects 0.000 claims description 4
- 238000012545 processing Methods 0.000 claims description 2
- 230000002093 peripheral effect Effects 0.000 abstract description 2
- 238000010586 diagram Methods 0.000 description 2
- 230000000694 effects Effects 0.000 description 2
- 230000001788 irregular Effects 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 230000003287 optical effect Effects 0.000 description 2
- 238000007792 addition Methods 0.000 description 1
- 230000005540 biological transmission Effects 0.000 description 1
- 238000010276 construction Methods 0.000 description 1
- 230000001419 dependent effect Effects 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 239000000835 fiber Substances 0.000 description 1
- 239000004065 semiconductor Substances 0.000 description 1
- 239000007787 solid Substances 0.000 description 1
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
-
- 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)
- 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 track planning method, a track planning device, electronic equipment and a storage medium for a narrow road, which are used for modeling a narrow road environment in a segmentation boundary fitting mode, avoiding calculation difficulty caused by traversing the road boundary point by point, improving calculation efficiency and simultaneously using a mixed A * The method comprises the steps of searching to obtain an initial track, dynamically adjusting heuristic weights according to the size of a peripheral free space, and improving algorithm convergence speed while ensuring that a feasibility solution is searched to the greatest extent.
Description
Technical Field
The invention belongs to the technical field of automobile track planning, and particularly relates to a narrow road track planning method, a narrow road track planning device, electronic equipment and a storage medium.
Background
Narrow roads are typical complex environments in automatic driving, particularly widely exist in unstructured scenes such as rural small roads, surface mines and the like, and how to generate a safe and stable driving track in a scene with irregular road boundaries and strictly limited free space is a difficult problem faced by automatic driving track planning.
Autopilot trajectory planning suffers from a number of differences in handling structured/unstructured roads. In terms of coordinate system selection: in an unstructured scene, no obvious road guide line exists, so that a structured road track method based on a Frenet coordinate system is obviously not applicable, and the track planning problem in the unstructured scene needs to be processed in a Cartesian coordinate system. In terms of planning algorithms: the track planning algorithm based on the optimization method can obtain a good effect in the structured road scene under the condition of the original guide line with higher quality; however, unstructured roads usually have no guide lines, which means that the optimization algorithm usually has no initial solution, so that the subsequent planning problem cannot be solved; meanwhile, the trajectory planning based on the optimization method is seriously dependent on the quality of the initial solution; when an initial solution is not feasible, it often results in solution failure, which also requires that a given initial solution must be a feasible solution; especially when the road scene is an unstructured narrow road, the quality of the initial solution directly influences whether the solution can be successfully solved.
Therefore, in the automatic driving process of unstructured scenes, a track planning method capable of planning a safe and smooth path is needed.
Disclosure of Invention
In view of the above problems, a main objective of the present invention is to design a method, apparatus, electronic device and storage medium for planning a B-spline trajectory for unstructured narrow roads, which first models a narrow environmental road and then uses a hybrid a * The discretization search result of the method is used as an initial solution of track planning, and a B spline curve-based optimization method is adopted to construct the track planning problem, so that the technical problem that a safe and stable running track is not easy to generate in the narrow road running process is solved.
The invention adopts the following technical scheme for realizing the purposes:
a B spline track planning method facing unstructured narrow road comprises the following steps:
step 1: modeling is carried out aiming at a narrow road environment by adopting a segmentation boundary fitting mode, so as to obtain a road boundary set;
step 2: using mix A * Searching by algorithm to obtain initial track, and performing feasible solution calculation to obtain initial feasible trackA collection of trace points;
step 3: taking the initial feasible track points as control points, constructing a B spline curve, and obtaining the state of each track point;
step 4: constructing limiting conditions of a road starting point and a road ending point by taking the center of a rear axle of a vehicle body as a reference;
step 5: establishing an objective function;
step 6: and putting the limiting conditions and the objective function into an optimization solver for solving to obtain an optimized B-spline curve, and sampling on the optimized B-spline curve to obtain a track consisting of sampling points, namely a track of the center of the rear axle of the vehicle body.
As a further description of the present invention, in step 1, the narrow road environment modeling is as follows:
step 1.1: if the current boundary segment set is empty, the first 2 points P in the boundary point set P are taken i (i=0, 1) to form a straight line L j (j=0) and calculating the orientation of the straight line theta;
step 1.2: taking the next point P in the boundary point set P i+1 Solving for P i+1 And the straight line L in the step 1.1 j The straight line formed by the end points is oriented towards theta_new;
step 1.3: if abs (theta-theta new ) < theta_grid, then consider P i+1 Belonging to straight line L j Can be represented by the same straight line parameter; otherwise, will be in straight line L j End point and P i+1 Form a new straight line L j+1 ；
Wherein theta_grid is a set threshold value;
step 1.4: repeating the steps 1.2 and 1.3 until the boundary point set P is traversed, and forming a left boundary straight line set bound_left and a right boundary straight line set bound_right, wherein the expression of the ith section of boundary line is as follows: a is that i x+B i y+C i ＝0，i＝1...N；
As a further description of the invention, the road edge is rasterized, each grid represents a discretized node, topological association exists among the nodes, and all grids finally form a discretized map for mixing A in step 2 * Algorithm search use；
In step 2, mix A * The cost function in the algorithm is:
f(x)＝g(x)+h(x)
wherein g (x) represents the cost from the start point to the current point, and h (x) represents the cost from the current point to the end point;
when h (x) is larger or smaller, the cost function is adjusted, namely:
f(x)＝g(x)+N b *h(x)
wherein N is b Representing the number of nodes in the feasibility region near the current expansion node;
when N is b When the current node is larger, indicating that the current node is in an open area, and increasing the weight of h (x); when N is b If the node is smaller, the current narrow space is represented, and more expansion nodes are needed at the moment;
based on the algorithm, a series of discretized sequences consisting of nodes, namely an initial feasible solution set PC, is obtained i I.e., a set of initial feasible trajectory points, where i=1, 2 … N, N is the initial number of trajectory points.
As a further description of the present invention, in step 3, the manner of constructing the B-spline curve includes the steps of:
step 3.1: using the initial feasible track point PC as a control point, constructing a B spline curve, which is expressed as:
where u represents a node vector, PC i Represents the ith control point, N i,k (u) represents the k-order basis function coefficient corresponding to each control point of i at the time of u;
step 3.2: uniformly sampling on the B spline curve to obtain a sampling point set
Corresponding accumulated Length i I=1, 2 … Num; each state contains x, y, theta of the current point.
As a further description of the present invention, in step 4, the method for constructing the limit conditions of the start point and the end point includes the steps of:
step 4.1: calculating the projection of each line segment in the left boundary straight line set bound_left and the right boundary straight line set bound_right on the B spline curve to obtain projection ranges start_s and end_s of each line segment in the length S direction of the B spline, namely S values, and storing the projection ranges on the straight line segments;
step 4.2: taking straight line segments of bound_left and bound_right before and after the S value according to the S value of the current track point;
step 4.3: placing the center of a rear axle of the vehicle body at each track point to obtain a vehicle body profile which runs to the track point, and calculating the minimum distance between the vehicle body profile and the straight line segment in the step 4.2, wherein the minimum distance is used as the left-right condition constraint b_l and b_r of the track point;
step 4.4: calculating aiming at each track point to obtain left and right boundary condition constraints b_ls and b_rs of all track points to be optimized;
step 4.5: the constraints of the start point and the end point are: the limitation of the starting point is P 0 ＝[x 0 ,y 0 ,theta 0 ]The limit condition of the end point is P end ＝[x end ,y end ,theta end ]。
As a further description of the present invention, in step 5, a cost function cost=w is set 1 ∫BS′ 2 +w 2 ∫BS″ 2 +w 3 ∫BS″′ 2 ；
Wherein BS ' is the first derivative of the B-spline curve, BS ' is the second derivative of the B-spline curve, BS ' "is the third derivative of the B-spline curve, w 1 Weight coefficient corresponding to first derivative of B spline curve, w 2 Weight coefficient corresponding to second derivative of B spline curve, w 3 And the weight coefficient corresponding to the third derivative of the B spline curve.
As a further description of the present invention, in step 6, the constraint of step 4 and the cost function of step 5 are substituted into the solver to obtain an optimized B-spline curve function
Sampling on the optimized B-spline curve at equal intervals to obtain a series of pose points
i=1, 2 … N, and the track formed by all pose points is the track of the vehicle body running.
A vehicle trajectory planning device for a narrow road, the device comprising an acquisition module and a generation module;
the acquisition module is used for acquiring the boundary of the narrow road and the environmental information;
the generation module is used for obtaining an initial track, constructing a B-spline curve, constructing limiting conditions of a road starting point and a road end point, and establishing an objective function, further obtaining an optimized B-spline curve, and sampling on the optimized B-spline curve to obtain a track of vehicle body running.
An electronic device comprising a processor, a communication interface, a memory and a communication bus, wherein the processor, the communication interface and the memory complete communication with each other through the communication bus, and the memory is used for storing a computer program;
the processor is configured to execute the steps of the track planning method by executing the computer program stored on the memory.
A computer readable storage medium having a computer program stored therein, wherein the computer program when executed by a processor implements the steps of the trajectory planning method described above.
Compared with the prior art, the invention has the technical effects that:
the invention provides a method, a device, electronic equipment and a storage medium for planning a track of a B spline for an unstructured narrow road, which describe the boundary of the narrow road by using a multi-segment linear equation, avoid calculation difficulty caused by traversing the road boundary point by point, improve the calculation efficiency and simultaneously use a mixed A * Searching to obtain initial track and rootAccording to the size of the peripheral free space, heuristic weights are dynamically adjusted, the algorithm convergence speed is improved while the feasibility solution is searched to the greatest extent, in addition, the feasibility solution is taken as input, the vehicle kinematic model is taken as constraint, the track optimization solving model is established, a safe and smooth optimal path is obtained, and the optimization solving success rate and efficiency are effectively improved.
Drawings
FIG. 1 is a schematic overall flow chart of a track planning method according to the present invention;
FIG. 2 is a schematic diagram of modeling for a narrow road in the present invention;
FIG. 3 is a schematic illustration of calculated orientations in the context modeling of the present invention;
FIG. 4 is a schematic diagram of the construction of the road start and end limiting conditions according to the present invention.
Detailed Description
The invention is described in detail below with reference to the attached drawing figures:
in one embodiment of the present invention, a method for planning a B-spline trajectory for unstructured narrow roads is disclosed, and referring to fig. 1, the method comprises the following specific steps:
step 1: modeling is carried out aiming at a narrow road environment by adopting a segmentation boundary fitting mode, so as to obtain a road boundary set;
step 2: using mix A * Searching by an algorithm to obtain an initial track, and performing feasible solution calculation to obtain a set of initial feasible track points;
step 3: taking the initial feasible track points as control points, constructing a B spline curve, and obtaining the state of each track point;
step 4: constructing limiting conditions of a road starting point and a road ending point by taking the center of a rear axle of a vehicle body as a reference;
step 5: establishing an objective function;
step 6: and putting the limiting conditions and the objective function into an optimization solver for solving to obtain an optimized B-spline curve, and sampling on the optimized B-spline curve to obtain a track consisting of sampling points, namely a track of the center of the rear axle of the vehicle body.
Specifically, in this embodiment, the steps disclosed above are analyzed in detail, and the analysis contents are as follows:
(1) In step 1, the boundary of the unstructured narrow road is irregular and cannot be described by a section of curve, so that the track is planned by adopting a sectional boundary fitting mode in the embodiment.
Specifically, referring to fig. 2 and 3, the steps of modeling the narrow road environment are as follows:
step 1.1: if the current boundary segment set is empty, the first 2 points P in the boundary point set P are taken i (i=0, 1) to form a straight line L j (j=0) and calculates the orientation of the line theta, i.e., the angle between the line and horizontal in fig. 3;
theta＝(y i+1- y i )/(x i+1 -x i )
step 1.2: taking the next point P in the boundary point set P i+1 Solving for P i+1 And the straight line L in the step 1.1 j The straight line formed by the end points is oriented towards theta_new;
step 1.3: if abs (theta-theta new ) < theta_grid, then consider P i+1 Belonging to straight line L j Can be represented by the same straight line parameter; otherwise, will be in straight line L j End point and P i+1 Form a new straight line L j+1 ；
Wherein theta_grid is a set threshold value;
step 1.4: repeating the step 1.2 and the step 1.3 until the boundary point set P is traversed;
finally, a left boundary straight line set bound_left and a right boundary straight line set bound_right are formed, wherein the expression of the ith section of boundary line is as follows: a is that i x+B i y+C i ＝0，i＝1...N；
(2) In step 2, mix A * The algorithm is conventional A * On the basis, consideration factors such as vehicle dynamics constraint, obstacle avoidance constraint, gear shifting times and the like are increased, so that a node expansion mode is changed.
Specifically, the road edge is subjected to rasterization, each grid represents a discretized node, and all the nodes are storedIn topological association, all grids finally form a discretized map for mixing A in step 2 * Searching and using an algorithm; mix A * The cost function in the algorithm is:
f(x)＝g(x)+h(x)
wherein g (x) represents the cost from the start point to the current point, and h (x) represents the cost from the current point to the end point;
it should also be noted that when h (x) is smaller, the number of expansion nodes of the algorithm can be increased, and the number of feasible paths is correspondingly increased; when h (x) is larger, the number of expansion nodes of the algorithm can be reduced, and the searching speed is increased.
According to the requirements, the cost function is improved as follows:
f(x)＝g(x)+N b *h(x)
wherein N is b Representing the number of nodes in the feasibility region near the current expansion node; when N is b When the current node is larger, the current node is in an open area, and at the moment, the h (x) weight is increased to enable the search to be in a direction approaching the target more quickly; similarly, when N b Smaller, representing a currently narrow space, more expandable nodes are needed to ensure a viable path.
Based on the algorithm, a series of discretized sequences consisting of nodes, namely an initial feasible solution set PC, is obtained i I.e., a set of initial feasible trajectory points, where i=1, 2 … N, N is the initial number of trajectory points.
(3) In step 3, the method for constructing the B spline curve comprises the following steps:
step 3.1: constructing a B spline curve by taking an initial feasible track point PC as a control point, wherein the control point is also a variable for solving an optimization problem, namely:
where u represents a node vector, PC i Represents the ith control point, N i,k (u) represents the k-order basis function coefficient corresponding to each control point of i at the time of u;
step 3.2: uniformly sampling on the B spline curve according to a certain step length to obtain a sampling point set
Corresponding accumulated Length i I=1, 2 … Num; each state contains x, y, theta of the current point.
After the curve is obtained, substituting the curve into track points PC and N according to the formula in the step 3.1 to obtain sampling points (x and y); PC (personal computer) i The self is Cartesian coordinates (x, y), and the coefficient is multiplied to obtain the (x, y);
(4) In step 4, the method for constructing the limit conditions of the starting point and the end point comprises the following steps:
step 4.1: calculating the projection of each line segment in the left boundary straight line set bound_left and the right boundary straight line set bound_right on the B spline curve, wherein the projection points are the closest distance points from the head and tail points of the line segment to the curve, so as to obtain the projection ranges start_s and end_s (radial direction) of the length S of the B spline curve of each line segment, namely S values, and storing the projection ranges on the straight line segments;
step 4.2: taking straight line segments of bound_left and bound_right before and after the S value according to the S value of the current track point;
step 4.3: placing the center of a rear axle of the vehicle body at each track point to obtain a vehicle body contour travelling to the track point, and calculating the minimum distance between the vehicle body contour and the straight line segment in the step 4.2, wherein the minimum distance is used as the left and right condition constraint b_l and b_r of the track point, namely the left and right hard constraint of the track point as shown in fig. 4;
step 4.4: calculating aiming at each track point to obtain left and right boundary condition constraints b_ls and b_rs of all track points to be optimized, namely, left and right boundary hard constraints of all track points to be optimized;
step 4.5: the limiting conditions for setting the starting point and the end point are: the limitation of the starting point is P 0 ＝[x 0 ,y 0 ,theta 0 ]The limit condition of the end point is P end ＝[x end ,y end ,theta end ]。
(5) In step 5, a cost function cost=w is set 1 ∫BS′ 2 +w 2 ∫BS″ 2 +w 3 ∫BS″′ 2 ；
Wherein BS ' is the first derivative of the B-spline curve, BS ' is the second derivative of the B-spline curve, BS ' "is the third derivative of the B-spline curve, w 1 Weight coefficient corresponding to first derivative of B spline curve, w 2 Weight coefficient corresponding to second derivative of B spline curve, w 3 And the weight coefficient corresponding to the third derivative of the B spline curve.
(6) In step 6, substituting the limiting condition of step 4 and the cost function of step 5 into a solver to obtain an optimized B-spline curve function
Sampling on the optimized B-spline curve at equal intervals to obtain a series of pose points
i=1, 2 … N, and the track formed by all pose points is the track of the vehicle body running.
In another embodiment of the invention, a vehicle track planning apparatus for a narrow road is disclosed, the apparatus comprising an acquisition module and a generation module;
the acquisition module is used for acquiring the boundary of the narrow road and the environmental information;
the generation module is used for obtaining an initial track, constructing a B-spline curve, constructing limiting conditions of a road starting point and a road end point, and establishing an objective function, further obtaining an optimized B-spline curve, and sampling on the optimized B-spline curve to obtain a track of vehicle body running.
In another embodiment of the invention, an electronic device is disclosed that may include a processor and a memory storing instructions for a computer program.
In particular, in this embodiment, the processor may include a Central Processing Unit (CPU), or a specific integrated circuit, or may be configured as one or more integrated circuits of the embodiment; the above-described memory may include mass storage for data or instructions, for which memory includes, but is not limited to, hard Disk Drives (HDD), floppy Disk drives, flash memory, optical disks, magneto-optical disks, magnetic tape, or universal serial bus (Universal Serial Bus, USB) drives, or a combination of two or more of these; the memory may include removable or non-removable (or fixed) media, where appropriate; in a particular embodiment, the memory is a non-volatile solid state memory. In a particular embodiment, the memory includes Read Only Memory (ROM). The ROM may be mask-programmed ROM, programmable ROM (PROM), erasable PROM (EPROM), electrically Erasable PROM (EEPROM), electrically rewritable ROM (EAROM), or flash memory, or a combination of two or more of these, where appropriate.
The processor implements the steps of the disclosed B-spline track planning method of the present invention by reading and executing computer program instructions stored in a memory.
It should be further noted that the electronic device of the present embodiment may further include a communication interface and a communication bus. The processor, the memory and the communication interface are connected through a communication bus and complete communication with each other. The communication interface is mainly used for realizing the communication among the units, the modules, the devices or the equipment in the embodiment of the invention.
The communication bus described above includes hardware, software, or a combination of both that couple the components of the on-line data flow device to each other. The communication bus may include one or more buses, where appropriate.
In addition, in combination with the B-spline trajectory planning method disclosed in the above embodiment, an embodiment of the present invention may be implemented by providing a computer storage medium having computer program instructions stored thereon; the computer program instructions are executed by the processor to perform the B-spline track planning method disclosed above.
It should be clear that the present invention is not limited to the methods, systems, apparatuses disclosed above, but includes various changes, modifications and additions, or the order of steps between them, which are made by those skilled in the art based on the teachings of the present invention.
When implemented in hardware, the present invention may be an electronic circuit, an application specific integrated circuit, appropriate firmware, plug-in, function card, or the like; when implemented in software, the elements of the invention are the program or code segments that are used to perform the desired tasks, which may be stored in a machine readable medium or uploaded through a transmission medium or communication link by a data signal carried in a carrier wave, which may comprise any medium capable of storing or transmitting information such as: electronic circuitry, semiconductor memory devices, ROM, flash memory, erasable ROM (EROM), floppy disks, optical disks, hard disks, fiber optic media, radio frequency links, etc. The code segments may be downloaded via computer networks such as the internet, intranets, etc.
Compared with the technical scheme in the prior art, the track planning method disclosed by the invention has the following advantages:
1. according to the method, the unstructured narrow road is subjected to linear modeling, so that calculation difficulty caused by traversing the road boundary point by point is avoided, and the calculation efficiency is improved;
2. aiming at unstructured narrow roads, the method dynamically adjusts the weight of the mixed A heuristic term, and improves the convergence speed of an algorithm while ensuring that feasible solutions are searched to the greatest extent
3. The invention creatively combines the mixed A and B spline curve optimization algorithm, solves the problem that the optimization algorithm has no initial solution in the unstructured narrow road scene, and simultaneously solves the discretized optimal solution feasible solution due to the mixed A, thereby effectively improving the success rate and efficiency of the optimization solution.
The above embodiments are only for illustrating the technical solution of the present invention, but not for limiting, and other modifications and equivalents thereof by those skilled in the art should be included in the scope of the claims of the present invention without departing from the spirit and scope of the technical solution of the present invention.
Claims (10)
1. A B spline track planning method for unstructured narrow roads is characterized by comprising the following steps:
step 1: modeling is carried out aiming at a narrow road environment by adopting a segmentation boundary fitting mode, so as to obtain a road boundary set;
step 2: using mix A * Searching by an algorithm to obtain an initial track, and performing feasible solution calculation to obtain a set of initial feasible track points;
step 3: taking the initial feasible track points as control points, constructing a B spline curve, and obtaining the state of each track point;
step 4: constructing limiting conditions of a road starting point and a road ending point by taking the center of a rear axle of a vehicle body as a reference;
step 5: establishing an objective function;
step 6: and putting the limiting conditions and the objective function into an optimization solver for solving to obtain an optimized B-spline curve, and sampling on the optimized B-spline curve to obtain a track consisting of sampling points, namely a track of the center of the rear axle of the vehicle body.
2. The unstructured narrow road-oriented B-spline trajectory planning method of claim 1, wherein: in step 1, the narrow road environment modeling is as follows:
step 1.1: if the current boundary segment set is empty, the first 2 points P in the boundary point set P are taken i (i=0, 1) to form a straight line L j (j=0) and calculating the orientation of the straight line theta;
step 1.2: taking the next point P in the boundary point set P i+1 Solving for P i+1 And the straight line L in the step 1.1 j The straight line formed by the end points is oriented towards theta_new;
step 1.3: if abs (theta-theta new ) < theta_grid, then consider P i+1 Belonging to straight line L j Can be represented by the same straight line parameter; otherwise, will be in straight line L j End point and P i+1 Form a new straight line L j+1 ；
Wherein theta_grid is a set threshold value;
step 1.4: repeating steps 1.2 andstep 1.3, until the boundary point set P is traversed, forming a left boundary straight line set bound_left and a right boundary straight line set bound_right, wherein the expression of the ith section of boundary line is as follows: a is that i x+B i y+C i ＝0，i＝1...N。
3. The unstructured narrow road-oriented B-spline trajectory planning method of claim 2, wherein: carrying out rasterization processing on the road side line, wherein each grid represents a discretized node, topological association exists among the nodes, and all grids finally form a discretized map for mixing A in the step 2 * Searching and using an algorithm;
in step 2, mix A * The cost function in the algorithm is:
f(x)＝g(x)+h(x)
wherein g (x) represents the cost from the start point to the current point, and h (x) represents the cost from the current point to the end point;
when h (x) is larger or smaller, the cost function is adjusted, namely:
f(x)＝g(x)+N b *h(x)
wherein N is b Representing the number of nodes in the feasibility region near the current expansion node;
when N is b When the current node is larger, indicating that the current node is in an open area, and increasing the weight of h (x); when N is b If the node is smaller, the current narrow space is represented, and more expansion nodes are needed at the moment;
based on the algorithm, a series of discretized sequences consisting of nodes, namely an initial feasible solution set PC, is obtained i Where i=1, 2 … N, N is the initial number of trace points.
4. A method of B-spline trajectory planning for unstructured narrow roads according to claim 3, wherein: in step 3, the method for constructing the B-spline curve includes the following steps:
step 3.1: using the initial feasible track point PC as a control point, constructing a B spline curve, which is expressed as:
where u represents a node vector, PC i Represents the ith control point, N i,k (u) represents the k-order basis function coefficient corresponding to each control point of i at the time of u;
step 3.2: uniformly sampling on the B spline curve to obtain a sampling point set
Corresponding accumulated Length i I=1, 2 … Num; each state contains x, y, theta of the current point.
5. The unstructured narrow-road-oriented B-spline trajectory planning method of claim 4, wherein: in step 4, the method for constructing the limit conditions of the starting point and the end point comprises the following steps:
step 4.1: calculating the projection of each line segment in the left boundary straight line set bound_left and the right boundary straight line set bound_right on the B spline curve to obtain projection ranges start_s and end_s of each line segment in the length S direction of the B spline, namely S values, and storing the projection ranges on the straight line segments;
step 4.2: taking straight line segments of bound_left and bound_right before and after the S value according to the S value of the current track point;
step 4.3: placing the center of a rear axle of the vehicle body at each track point to obtain a vehicle body profile which runs to the track point, and calculating the minimum distance between the vehicle body profile and the straight line segment in the step 4.2, wherein the minimum distance is used as the left-right condition constraint b_l and b_r of the track point;
step 4.4: calculating aiming at each track point to obtain left and right boundary condition constraints b_ls and b_rs of all track points to be optimized;
step 4.5: the constraints of the start point and the end point are: the limitation of the starting point is P 0 ＝[x 0 ,y 0 ,theta 0 ]The limit condition of the end point is P end ＝[x end ,y end ,theta end ]。
6. The unstructured narrow-road-oriented B-spline trajectory planning method of claim 5, wherein: in step 5, a cost function cost=w is set 1 ∫BS′ 2 +w 2 ∫BS″ 2 +w 3 ∫BS″′ 2 ；
Wherein BS ' is the first derivative of the B-spline curve, BS ' is the second derivative of the B-spline curve, BS ' "is the third derivative of the B-spline curve, w 1 Weight coefficient corresponding to first derivative of B spline curve, w 2 Weight coefficient corresponding to second derivative of B spline curve, w 3 And the weight coefficient corresponding to the third derivative of the B spline curve.
7. The unstructured narrow road-oriented B-spline trajectory planning method of claim 6, wherein: in step 6, substituting the limiting condition of step 4 and the cost function of step 5 into a solver to obtain an optimized B-spline curve function
Sampling on the optimized B-spline curve at equal intervals to obtain a series of pose points
The track formed by all the pose points is the track of the vehicle body running.
8. A vehicle trajectory planning device for a narrow road, characterized by: the device comprises an acquisition module and a generation module;
the acquisition module is used for acquiring the boundary of the narrow road and the environmental information;
the generation module is used for obtaining an initial track, constructing a B-spline curve, constructing limiting conditions of a road starting point and a road end point, and establishing an objective function, further obtaining an optimized B-spline curve, and sampling on the optimized B-spline curve to obtain a track of vehicle body running.
9. An electronic device comprising a processor, a communication interface, a memory and a communication bus, wherein the processor, the communication interface and the memory complete communication with each other through the communication bus, characterized in that: the memory is used for storing a computer program;
the processor being adapted to perform the method steps of any of claims 1-7 by running the computer program stored on the memory.
10. A computer-readable storage medium, characterized in that the storage medium has stored therein a computer program, wherein the computer program, when executed by a processor, implements the method steps of any of claims 1 to 7.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310540544.6A CN116399363A (en) | 2023-05-15 | 2023-05-15 | Track planning method and device for narrow road, electronic equipment and storage medium |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310540544.6A CN116399363A (en) | 2023-05-15 | 2023-05-15 | Track planning method and device for narrow road, electronic equipment and storage medium |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN116399363A true CN116399363A (en) | 2023-07-07 |
Family
ID=87010700
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202310540544.6A Pending CN116399363A (en) | 2023-05-15 | 2023-05-15 | Track planning method and device for narrow road, electronic equipment and storage medium |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN116399363A (en) |
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116958316A (en) * | 2023-09-19 | 2023-10-27 | 北京集度科技有限公司 | Topology map generation method, device, computer equipment and storage medium |
| CN117742316A (en) * | 2023-11-28 | 2024-03-22 | 上海友道智途科技有限公司 | Optimal track planning method based on model with trailer |
-
2023
- 2023-05-15 CN CN202310540544.6A patent/CN116399363A/en active Pending
Cited By (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116958316A (en) * | 2023-09-19 | 2023-10-27 | 北京集度科技有限公司 | Topology map generation method, device, computer equipment and storage medium |
| CN116958316B (en) * | 2023-09-19 | 2023-12-08 | 北京集度科技有限公司 | Topology map generation method, device, computer equipment and storage medium |
| CN117742316A (en) * | 2023-11-28 | 2024-03-22 | 上海友道智途科技有限公司 | Optimal track planning method based on model with trailer |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN116399363A (en) | Track planning method and device for narrow road, electronic equipment and storage medium | |
| EP3805703B1 (en) | Movement track reconstruction method and device, storage medium, and electronic device | |
| CN110712680B (en) | Automatic lane changing method and device for vehicle | |
| CN112677995B (en) | Vehicle track planning method and device, storage medium and equipment | |
| CN111506058B (en) | Method and device for planning a short-term path for autopilot by means of information fusion | |
| CN106104656B (en) | Map information generating systems, method and program | |
| CN112810630A (en) | Method and system for planning track of automatic driving vehicle | |
| CN113819917A (en) | Automatic driving path planning method, device, equipment and storage medium | |
| CN111123947A (en) | Robot traveling control method and device, electronic device, medium, and robot | |
| CN115309170B (en) | Track planning method, device and system considering comfort constraint | |
| CN112977412A (en) | Vehicle control method, device and equipment and computer storage medium | |
| CN114764249A (en) | Real-time obstacle avoidance path planning method, system, device and medium | |
| CN114802214A (en) | Pure tracking automatic parking control method, performance evaluation tool and readable storage medium | |
| CN114620070A (en) | Driving track planning method, device, equipment and storage medium | |
| CN114620071A (en) | Detour trajectory planning method, device, equipment and storage medium | |
| CN111752294A (en) | Flight control method and related device | |
| CN114763133A (en) | Vehicle parking planning method, device, equipment and computer storage medium | |
| CN114167860B (en) | Automatic driving optimal track generation method and device | |
| CN116202522A (en) | Unmanned plane path planning method and device, electronic equipment and storage medium | |
| CN114625127A (en) | Speed planning method, device, equipment, readable storage medium and program product | |
| CN111752295B (en) | Unmanned aerial vehicle flight trajectory planning method and related device | |
| CN115704693A (en) | Vehicle driving path planning method, device, equipment and computer storage medium | |
| CN113189985A (en) | Partially observable driving planning method based on adaptive particle and belief filling | |
| JP6936673B2 (en) | Map data update system and map data update program | |
| CN116300478B (en) | Track generation method, medium and equipment based on variable separation differential dynamic programming |
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 |