CN116560362A - Automatic navigation path planning tracking method and system - Google Patents
Automatic navigation path planning tracking method and system Download PDFInfo
- Publication number
- CN116560362A CN116560362A CN202310454499.2A CN202310454499A CN116560362A CN 116560362 A CN116560362 A CN 116560362A CN 202310454499 A CN202310454499 A CN 202310454499A CN 116560362 A CN116560362 A CN 116560362A
- Authority
- CN
- China
- Prior art keywords
- path
- target vehicle
- working
- planning
- node
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Withdrawn
Links
- 238000000034 method Methods 0.000 title claims abstract description 31
- 230000007704 transition Effects 0.000 claims abstract description 62
- 238000004364 calculation method Methods 0.000 claims description 8
- 238000005457 optimization Methods 0.000 claims description 4
- 238000004422 calculation algorithm Methods 0.000 description 3
- 238000010586 diagram Methods 0.000 description 3
- 230000002068 genetic effect Effects 0.000 description 3
- 206010063385 Intellectualisation Diseases 0.000 description 2
- 238000005516 engineering process Methods 0.000 description 2
- 230000000694 effects Effects 0.000 description 1
- 230000006872 improvement Effects 0.000 description 1
- 230000008569 process Effects 0.000 description 1
- 230000000750 progressive effect Effects 0.000 description 1
- 230000001172 regenerating effect Effects 0.000 description 1
- 238000007616 round robin method Methods 0.000 description 1
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
- G05D1/0219—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory ensuring the processing of the whole working surface
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T10/00—Road transport of goods or passengers
- Y02T10/10—Internal combustion engine [ICE] based vehicles
- Y02T10/40—Engine management systems
Landscapes
- Engineering & Computer Science (AREA)
- Aviation & Aerospace Engineering (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
The invention provides an automatic navigation path planning and tracking method and system, and relates to the field of automatic control of agricultural machinery. The planning tracking method comprises the following steps: acquiring the turning area information, the operation area information, the starting point coordinates, the end point coordinates, the operation tilling width and the turning radius of the target vehicle according to the grid map; determining an initial planning path according to the region information of the transition zone, the starting point coordinates and the end point coordinates; solving the initial planning path by taking the shortest time passing through the transition area as a target to obtain an optimal transition path; determining a working path of the target vehicle in the working area according to the working tilling width of the target vehicle, the turning radius of the target vehicle and the working area information; determining a driving path of the target vehicle according to the optimal transition path and the working path; controlling the target vehicle to run along the running path and tracking the running state of the target vehicle; the invention can realize automatic planning and tracking of the path, thereby improving the working efficiency.
Description
Technical Field
The invention relates to the field of automatic control of agricultural machinery, in particular to an automatic navigation path planning and tracking method and system.
Background
With the improvement of modern technology level and the modern times requirements of countries on agricultural rural modernization, the modernization and the intellectualization of agricultural machinery are one of important development directions. And the automatic navigation of the agricultural machine is one of key technologies of the automation and the intellectualization of the modern agricultural machine. In the navigation process of agricultural machinery, operation path planning and tracking are key factors affecting the working efficiency. In the prior art, when the agricultural machine is actually applied, the condition of deviating from a set track can occur in the reciprocating motion during working, so that the working of the agricultural machine is deviated. In addition, the prior art has the problem that the formulated navigation route is deviated, so that the navigation route is not an optimal route, and the driving route of the agricultural machine is yawed. At this time, the machine is required to be stopped for re-debugging, and the working efficiency of the agricultural machine is low.
Disclosure of Invention
The invention aims to provide an automatic navigation path planning and tracking method and system, so as to realize automatic planning and tracking of paths and improve working efficiency.
In order to achieve the above object, the present invention provides the following solutions:
an automatic navigation path planning tracking method, the planning tracking method comprising:
acquiring the turning area information, the operation area information, the starting point coordinates, the end point coordinates, the operation tilling width and the turning radius of the target vehicle according to the grid map; the region information of the turning region comprises coordinates of corresponding positions of roads in the grid map in the region of the turning region; the operation area information comprises coordinates of positions of an entry point and a boundary of the operation area corresponding to the position in the grid map;
determining an initial planning path according to the region information of the transfer field, the starting point coordinates and the end point coordinates;
solving the initial planning path by taking the shortest time passing through the transition area as a target to obtain an optimal transition path; the starting point of the optimal transition path is the starting point coordinate, and the end point of the optimal transition path is the entry point;
determining a working path of the target vehicle in the working area according to the working tilling width of the target vehicle, the turning radius of the target vehicle and the boundary in the working area information; the starting point of the working path is the entry point; the end point of the working path is the end point coordinate;
determining a driving path of the target vehicle according to the optimal transition path and the working path;
and controlling the target vehicle to run along the running path, and tracking the running state of the target vehicle.
Optionally, the solving the initial planning path with the goal of minimizing the duration of the transition area to obtain an optimal transition path specifically includes:
taking the starting point coordinates of the target vehicle as a starting node, and selecting any position point connected with the starting node as a next node;
judging whether the coordinate of the next node is the terminal point coordinate of the target vehicle or not to obtain a first judging result;
if the first judgment result is negative, the next node is used as the current starting node;
if the first judgment result is yes, obtaining an initial planning path;
and calculating the time length of the target vehicle passing through each initial planning path, and determining the initial planning path with the shortest time length as the optimal transition path.
Optionally, the calculating the duration of the target vehicle passing through each initial planned path specifically includes:
and calculating the time length of the target vehicle passing through each initial planning path by adopting a fitness function according to an optimization principle.
Optionally, the fitness function has a specific calculation formula:
wherein x is ij Path length in the j node for the i initial planned path; v ij The method comprises the steps of (1) planning a path for the ith initial planning path and driving speed in a jth node; x is x i An ith initial planning path is provided; f (x) i ) And (5) the fitness function of the ith initial planning path.
Optionally, the determining the working path of the target vehicle in the working area according to the working tilling width of the target vehicle, the turning radius of the target vehicle and the boundary in the working area information specifically includes:
determining at least one parallel path according to the working tilling width of the target vehicle, the turning radius of the target vehicle and the boundary;
determining a working path according to the parallel path, the working node and the position of the target vehicle; the working node is the intersection of the parallel path and the boundary.
Optionally, the controlling the target vehicle to travel along the travel path and tracking the travel state of the target vehicle specifically includes:
acquiring a running angle value of the target vehicle at the current moment; the travel angle value includes: steering angle and included angle; the included angle is an included angle between the transverse direction and the radial direction of the target vehicle;
judging whether the running angle value meets a set condition or not to obtain a second judging result; the setting condition is that the steering angle is a first expected angle value and the included angle is a second expected angle value;
and if the second judging result is negative, adjusting the running angle value so that the target vehicle runs along the running path.
Optionally, the calculation formula of the steering angle is:
wherein, theta is the steering angle,direction vector for target vehicle and mth working node,/->Is the vector of the wheel direction of the target vehicle, ω m Is the mth working node.
Optionally, the calculation formula of the included angle is:
where P is the current position of the target vehicle,is omega n Vector to the current position of the target vehicle, +.>Is the direction vector of the working path, alpha is the included angle, omega n Is the nth working node.
An automatic navigation path planning tracking system, the planning tracking system comprising:
the data acquisition module is used for acquiring the turning area information, the operation area information, the starting point coordinates, the end point coordinates, the operation tilling width and the turning radius of the target vehicle according to the grid map; the region information of the turning region comprises coordinates of corresponding positions of roads in the grid map in the region of the turning region; the operation area information comprises coordinates of positions of an entry point and a boundary of the operation area corresponding to the position in the grid map;
an initial planning path determining module, configured to determine an initial planning path from the region information of the turning field, the start point coordinate and the end point coordinate;
the optimal transition path determining module is used for solving the initial planning path by taking the shortest time passing through the transition area as a target to obtain an optimal transition path; the starting point of the optimal transition path is the starting point coordinate, and the end point of the optimal transition path is the entry point;
a working path determining module, configured to determine a working path of the target vehicle in the working area according to a working range of the target vehicle, a turning radius of the target vehicle, and the boundary in the working area information; the starting point of the working path is the entry point; the end point of the working path is the end point coordinate;
the driving path determining module is used for determining the driving path of the target vehicle according to the optimal transition path and the working path;
and the tracking module is used for controlling the target vehicle to run along the running path and tracking the running state of the target vehicle.
According to the specific embodiment provided by the invention, the invention discloses the following technical effects:
acquiring the turning area information, the operation area information, the starting point coordinates, the end point coordinates, the operation tilling width and the turning radius of the target vehicle according to the grid map; determining an initial planning path according to the region information of the transition zone, the starting point coordinates and the end point coordinates; solving the initial planning path by taking the shortest time passing through the transition area as a target to obtain an optimal transition path; due to the determination of the optimal transition path, the target vehicle can reach the working area in the shortest time, so that the path time is shortened, and the target vehicle can enter the working area as early as possible; determining a working path of the target vehicle in the working area according to the working tilling width of the target vehicle, the turning radius of the target vehicle and the working area information; determining a running path of the target vehicle according to the optimal transition path and the working path, then controlling the target vehicle to run along the running path, and tracking the running state of the target vehicle; because the running state of the target vehicle is tracked, the target vehicle moves according to the set track, if deviation occurs, the target vehicle is adjusted timely, the stop is not needed for debugging again, the time is shortened, and the working efficiency is improved; therefore, the invention can realize automatic planning and tracking of the path, thereby improving the working efficiency.
Drawings
In order to more clearly illustrate the embodiments of the present invention or the technical solutions of the prior art, the drawings that are needed in the embodiments will be briefly described below, it being obvious that the drawings in the following description are only some embodiments of the present invention, and that other drawings may be obtained according to these drawings without inventive effort for a person skilled in the art.
FIG. 1 is a flowchart of a path planning and tracking method for automatic navigation according to an embodiment of the present invention;
FIG. 2 is a block diagram of an automatic navigation path planning tracking system according to an embodiment of the present invention;
FIG. 3 is a schematic diagram of a path tracking method according to an embodiment of the present invention;
FIG. 4 is a flow chart of planning an optimal transition path according to an embodiment of the present invention;
FIG. 5 is a flow chart of a working path planning provided by an embodiment of the present invention;
fig. 6 is a flowchart of path tracking according to an embodiment of the present invention.
Symbol description:
the system comprises a data acquisition module-1, an initial planning path determination module-2, an optimal transition path determination module-3, a working path determination module-4, a driving path determination module-5 and a tracking module-6.
Detailed Description
The following description of the embodiments of the present invention will be made clearly and completely with reference to the accompanying drawings, in which it is apparent that the embodiments described are only some embodiments of the present invention, but not all embodiments. All other embodiments, which can be made by those skilled in the art based on the embodiments of the invention without making any inventive effort, are intended to be within the scope of the invention.
The invention aims to provide an automatic navigation path planning and tracking method and system, so as to realize automatic planning and tracking of paths and improve working efficiency.
In order that the above-recited objects, features and advantages of the present invention will become more readily apparent, a more particular description of the invention will be rendered by reference to the appended drawings and appended detailed description.
Example 1
As shown in fig. 1, an embodiment of the present invention provides an automatic navigation path planning and tracking method, where the planning and tracking method includes:
step 100: acquiring the turning area information, the operation area information, the starting point coordinates, the end point coordinates, the operation tilling width and the turning radius of the target vehicle according to the grid map; the transition area information comprises coordinates of corresponding positions of roads in the grid map in the transition area; the work area information includes coordinates of positions of entry points and boundaries of the work area corresponding in the grid map.
Step 200: an initial planned path is determined from the transition region information, the start point coordinates, and the end point coordinates.
Step 300: solving the initial planning path by taking the shortest time passing through the transition area as a target to obtain an optimal transition path; the starting point of the optimal transition path is the starting point coordinate, and the end point of the optimal transition path is the entry point.
Specifically, the starting point coordinates of the target vehicle are used as the starting node, and any position point connected with the starting node is selected as the next node.
And judging whether the coordinate of the next node is the terminal point coordinate of the target vehicle or not, and obtaining a first judging result.
If the first judgment result is negative, the next node is used as the current starting node.
If the first judgment result is yes, an initial planning path is obtained.
And calculating the time length of the target vehicle passing through each initial planning path, and determining the initial planning path with the shortest time length as the optimal transition path.
Further, through an optimization principle, the length of time for the target vehicle to pass through each initial planning path is calculated by adopting a fitness function.
The fitness function comprises the following specific calculation formula:
wherein x is ij Path length in the j node for the i initial planned path; v ij The method comprises the steps of (1) planning a path for the ith initial planning path and driving speed in a jth node; x is x i An ith initial planning path is provided; f (x) i ) And (5) the fitness function of the ith initial planning path.
Step 400: determining a working path of the target vehicle in the working area according to the working tilling width of the target vehicle, the turning radius of the target vehicle and the boundary in the working area information; the starting point of the working path is an entry point; the end point of the working path is the end point coordinates.
Specifically, at least one parallel path is determined based on the work tilling width of the target vehicle, the turning radius of the target vehicle, and the boundary.
Determining a working path according to the parallel path, the working node and the position of the target vehicle; the working node is the intersection of the parallel path and the boundary.
Step 500: and determining the driving path of the target vehicle according to the optimal transition path and the working path.
Step 600: the control target vehicle runs along the running path and tracks the running state of the target vehicle.
Specifically, a running angle value of a target vehicle at the current moment is obtained; the travel angle value includes: steering angle and included angle. The included angle is the included angle between the transverse direction and the radial direction of the target vehicle.
And judging whether the running angle value meets the set condition or not to obtain a second judging result. The setting condition is that the steering angle is a first expected angle value and the included angle is a second expected angle value.
And if the second judging result is negative, adjusting the running angle value so that the target vehicle runs along the running path.
Further, the calculation formula of the steering angle is:
wherein, theta is the steering angle,direction vector for target vehicle and mth working node,/->Is the vector of the wheel direction of the target vehicle, ω m Is the mth working node.
The calculation formula of the included angle is as follows:
where P is the current position of the target vehicle,is omega n Vector to the current position of the target vehicle, +.>Is the direction vector of the working path, alpha is the included angle, omega n Is the nth working node.
Example 2
As shown in fig. 2, an embodiment of the present invention provides an automatic navigation path planning tracking system, including: a data acquisition module 1, an initial planning path determination module 2, an optimal transition path determination module 3, a working path determination module 4, a driving path determination module 5 and a tracking module 6.
The data acquisition module 1 is used for acquiring the turning area information, the operation area information, the starting point coordinates, the end point coordinates, the operation tilling width and the turning radius of the target vehicle according to the grid map; the transition area information comprises coordinates of corresponding positions of roads in the grid map in the transition area; the work area information includes coordinates of positions of entry points and boundaries of the work area corresponding in the grid map.
An initial planning path determining module 2, configured to determine an initial planning path from the region information of the rotating field, the start point coordinates and the end point coordinates.
The optimal transition path determining module 3 is used for solving the initial planning path with the shortest time passing through the transition area as a target to obtain an optimal transition path; the starting point of the optimal transition path is the starting point coordinate, and the end point of the optimal transition path is the entry point.
A working path determining module 4 for determining a working path of the target vehicle in the working area according to the working tilling width of the target vehicle, the turning radius of the target vehicle and the boundary in the working area information; the starting point of the working path is an entry point; the end point of the working path is the end point coordinates.
And the driving path determining module 5 is used for determining the driving path of the target vehicle according to the optimal transition path and the working path.
And the tracking module 6 is used for controlling the target vehicle to travel along the travel path and tracking the travel state of the target vehicle.
The specific implementation steps of the invention in practical application can be as follows:
for path planning, as shown in fig. 4 and 5. FIG. 4 is a plan flow chart of an optimal transition path; path node data is first imported and then path distance is converted into fitness functions. Setting an initial population; and selecting, mutating, crossing and deleting the optimized population obtained by the operation, and then outputting an optimal path. Fig. 5 is a flow chart of the planning of a working path. Firstly, importing operation area data; then determining a reference edge; regenerating parallel paths; and determining a working path node and finally outputting a working path.
Specifically, the planning operation for the path is as follows:
1. and acquiring the operation area information, the turning area information, the operation tilling width of the target vehicle and the minimum turning radius in the grid map.
2. And selecting an optimal transition path from the starting point position, namely the starting point coordinate to the working area according to the genetic algorithm.
The starting point coordinates, the ending point coordinates, the distance data of feasible nodes and arcs of the target vehicle are determined in the map, the nodes correspond to the intersections of the roads, and the arcs correspond to the paths among the nodes. Converting the distance data of each arc into a time cost function to obtain the dynamic running time of each arc; setting an initial planning path, optimizing the initial population by using an improved genetic algorithm to obtain an optimized population based on a time shortest principle, and selecting an optimal transition path from the optimized population.
Specifically, two sets M, N are set, set M is defined as an unselected path node set, set N is a selected path node set, after one path node is selected in one path, the node is moved from set M to set N, it is defined that only the node in set M can be selected as a new path node, and all the refreshing is marked after each initial path selection is completed. Starting from the starting point, randomly selecting one point directly connected with the starting point as the next node, and repeating the steps until the end point is found, wherein the path is the initial population. All nodes in the M sets are selected through the operations of selecting, mutating, crossing and deleting by a round-robin method, and the fitness function under the principle of optimal optimization of the set time is as follows:
wherein x is ij Path length in the j node for the i initial planned path; v ij The method comprises the steps of (1) planning a path for the ith initial planning path and driving speed in a jth node; x is x i An ith initial planning path is provided; f (x) i ) And (5) the fitness function of the ith initial planning path.
Finally, the optimized population based on the shortest time principle is obtained.
3. Planning a working path of a target vehicle
And generating a plurality of parallel paths in the direction of the operation area by taking an entry point of the operation area as a starting point, taking one side edge of the entry point as a reference edge, taking the reference edge as a first edge and taking the operation cultivation width as a path interval, and calibrating boundary crossing points of the paths and the operation area as working nodes to circularly reciprocate so that the reference edge, the parallel paths and the working nodes form a working path. A plurality of equally spaced path points are interposed between each node in the working path and the transition path.
The specific implementation steps of the path tracking are as follows:
fig. 3 is a schematic diagram of a path tracking method. UTM (Universal transverse Mercartor grid system) is a universal transverse ink card grid system. The abscissa UTM-E represents the projected distance from the central meridian of the longitude zone, and the ordinate UTM-N represents the projected distance from the equator.
The data available inside the target vehicle are the current target vehicle working direction and the current position coordinates P (a, b) of the target vehicle. The invention can automatically track the path according to the path information, and the target vehicle can automatically travel to the set path from the current position coordinates P (a, b) to complete navigation tracking at any position.
A specific flow chart is shown in fig. 6. The specific operation steps are as follows:
working path setting and recording
Before automatic operation, setting a path on a map, and representing node information A generated by path planning as:
A＝{ω i |ω i ∈E 2 ,0＜i＜N}；
ω i ＝(x,y)；
ω i the longitude and latitude of each node are included, and N is the total number of working nodes in the working area. Meanwhile, the data which can be acquired by the target vehicle are the current heading phi of the target vehicle and the current position P, P E of the machine 2 . Wherein E is 2 Refers to a two-dimensional spatial vector.
Searching the nearest point of the target vehicle
After the target vehicle starts working, the stored position point omega is searched i ，ω i Is the ith working node; according to the distance linear equation:
calculating the shortest distance between the current position P of the target vehicle and the set working path, and finding the nearest point omega n ，ω n Is the nth working node.
Third, calculate the steering angle θ of the target vehicle
Continue searching for omega i The forward viewing distance LA is introduced, which is related to the traveling speed of the target vehicle, with a larger LA value for the speed.
I.e. ω on the travel path n To omega m Is a sum of the polyline distances of (a). Omega n Forward search for omega m Point, calibrated omega n ω m The straight line is:
Ax+By+C＝0；
the position P to omega of the target vehicle m Is the vector of the direction of (2)The direction vector of the target vehicle and the wheels is +.>The steering angle θ of the target vehicle can be thus found:
LA is the forward looking distance, θ is the steering angle, ω i For the i-th working node,direction vector for target vehicle and mth working node,/->Is the vector of the wheel direction of the target vehicle, n is the total number of working nodes omega i+1 The (i+1) th working node, m is the sequence number of the working node, omega m Omega for the mth working node n Is the nth working node.
Fourth, the target vehicle turns and runs.
Fifth, judge whether the target vehicle passes omega n And (5) a dot.
During the driving of the target vehicle, omega is judged n Vector to the position P where the target vehicle is locatedDirection of the pathIncluded angle of (2):
where P is the current position of the target vehicle,is omega n Vector to the current position of the target vehicle, +.>Is the direction vector of the working path, and alpha is the included angle.
If alpha is less than or equal to 90 degrees, the current target vehicle is passed through the point omega n The method comprises the steps of carrying out a first treatment on the surface of the Otherwise, the boundary is not crossed.
If the target vehicle passes over the point omega n In omega n+1 And (5) repeating the third step to the fifth step as the datum point. I.e. search forward to find a new ω m Calculating the steering angle of the target vehicle and the targetSteering the vehicle and judging whether to pass omega n+1 . And (5) circulating until all the position information points, namely nodes, are completed, and realizing automatic path tracking operation of the target vehicle.
The path planning is divided into transition path planning and working path planning, can be effectively used for realizing automatic navigation of the agricultural operation machine when the agricultural intelligent machine automatically operates in the field, and can realize shortest path planning, continuous connection of break points of the agricultural machine and automatic operation according to the path planned automatically. Setting the path information of a starting point, an entrance point of a working area and a feasible node of a target vehicle in an electronic map (grid map), and finding a path from the starting point to the entrance point of the working area, which is shortest in time, through a genetic algorithm; and taking an entry point of the working area as a starting point, taking one side edge of the entry point as a reference edge, taking the reference edge as a first edge, taking the working cultivation width as a path interval, and generating a plurality of parallel paths in the direction of the working area to finally obtain the working path. The target vehicle is enabled to automatically track to a set path from any point, and autonomous navigation of the agricultural machinery is achieved.
In the present specification, each embodiment is described in a progressive manner, and each embodiment is mainly described in a different point from other embodiments, and identical and similar parts between the embodiments are all enough to refer to each other. For the system disclosed in the embodiment, since it corresponds to the method disclosed in the embodiment, the description is relatively simple, and the relevant points refer to the description of the method section.
The principles and embodiments of the present invention have been described herein with reference to specific examples, the description of which is intended only to assist in understanding the methods of the present invention and the core ideas thereof; also, it is within the scope of the present invention to be modified by those of ordinary skill in the art in light of the present teachings. In view of the foregoing, this description should not be construed as limiting the invention.
Claims (9)
1. An automatic navigation path planning and tracking method, which is characterized by comprising the following steps:
acquiring the turning area information, the operation area information, the starting point coordinates, the end point coordinates, the operation tilling width and the turning radius of the target vehicle according to the grid map; the region information of the turning region comprises coordinates of corresponding positions of roads in the grid map in the region of the turning region; the operation area information comprises coordinates of positions of an entry point and a boundary of the operation area corresponding to the position in the grid map;
determining an initial planning path according to the region information of the transfer field, the starting point coordinates and the end point coordinates;
solving the initial planning path by taking the shortest time passing through the transition area as a target to obtain an optimal transition path; the starting point of the optimal transition path is the starting point coordinate, and the end point of the optimal transition path is the entry point;
determining a working path of the target vehicle in the working area according to the working tilling width of the target vehicle, the turning radius of the target vehicle and the boundary in the working area information; the starting point of the working path is the entry point; the end point of the working path is the end point coordinate;
determining a driving path of the target vehicle according to the optimal transition path and the working path;
and controlling the target vehicle to run along the running path, and tracking the running state of the target vehicle.
2. The automatic navigation path planning and tracking method according to claim 1, wherein the solving the initial planned path with the goal of minimizing the duration of the transition area to obtain an optimal transition path specifically includes:
taking the starting point coordinates of the target vehicle as a starting node, and selecting any position point connected with the starting node as a next node;
judging whether the coordinate of the next node is the terminal point coordinate of the target vehicle or not to obtain a first judging result;
if the first judgment result is negative, the next node is used as the current starting node;
if the first judgment result is yes, obtaining an initial planning path;
and calculating the time length of the target vehicle passing through each initial planning path, and determining the initial planning path with the shortest time length as the optimal transition path.
3. The method of automatically navigating route planning and tracking according to claim 2, wherein said calculating the length of time for the target vehicle to traverse each of the initially planned routes comprises:
and calculating the time length of the target vehicle passing through each initial planning path by adopting a fitness function according to an optimization principle.
4. The path planning tracking method of automatic navigation according to claim 3, wherein the fitness function is specifically calculated as:
wherein x is ij Path length in the j node for the i initial planned path; v ij The method comprises the steps of (1) planning a path for the ith initial planning path and driving speed in a jth node; x is x i An ith initial planning path is provided; f (x) i ) And (5) the fitness function of the ith initial planning path.
5. The automatic navigation path planning and tracking method according to claim 1, wherein the determining the working path of the target vehicle in the working area according to the working tilling width of the target vehicle, the turning radius of the target vehicle and the boundary in the working area information specifically comprises:
determining at least one parallel path according to the working tilling width of the target vehicle, the turning radius of the target vehicle and the boundary;
determining a working path according to the parallel path, the working node and the position of the target vehicle; the working node is the intersection of the parallel path and the boundary.
6. The automatic navigation path planning tracking method according to claim 1, characterized in that the controlling the target vehicle to travel along the travel path and tracking the travel state of the target vehicle specifically includes:
acquiring a running angle value of the target vehicle at the current moment; the travel angle value includes: steering angle and included angle; the included angle is an included angle between the transverse direction and the radial direction of the target vehicle;
judging whether the running angle value meets a set condition or not to obtain a second judging result; the setting condition is that the steering angle is a first expected angle value and the included angle is a second expected angle value;
and if the second judging result is negative, adjusting the running angle value so that the target vehicle runs along the running path.
7. The automatic navigation path planning tracking method according to claim 6, wherein the calculation formula of the steering angle is:
wherein, theta is the steering angle,direction vector for target vehicle and mth working node,/->Is the vector of the wheel direction of the target vehicle, ω m Is the mth working node.
8. The automatic navigation path planning tracking method according to claim 7, wherein the calculation formula of the included angle is:
where P is the current position of the target vehicle,is omega n Vector to the current position of the target vehicle, +.>Is the direction vector of the working path, alpha is the included angle, omega n Is the nth working node.
9. An automatic navigation path planning tracking system, the planning tracking system comprising:
the data acquisition module is used for acquiring the turning area information, the operation area information, the starting point coordinates, the end point coordinates, the operation tilling width and the turning radius of the target vehicle according to the grid map; the region information of the turning region comprises coordinates of corresponding positions of roads in the grid map in the region of the turning region; the operation area information comprises coordinates of positions of an entry point and a boundary of the operation area corresponding to the position in the grid map;
an initial planning path determining module, configured to determine an initial planning path from the region information of the turning field, the start point coordinate and the end point coordinate;
the optimal transition path determining module is used for solving the initial planning path by taking the shortest time passing through the transition area as a target to obtain an optimal transition path; the starting point of the optimal transition path is the starting point coordinate, and the end point of the optimal transition path is the entry point;
a working path determining module, configured to determine a working path of the target vehicle in the working area according to a working range of the target vehicle, a turning radius of the target vehicle, and the boundary in the working area information; the starting point of the working path is the entry point; the end point of the working path is the end point coordinate;
the driving path determining module is used for determining the driving path of the target vehicle according to the optimal transition path and the working path;
and the tracking module is used for controlling the target vehicle to run along the running path and tracking the running state of the target vehicle.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310454499.2A CN116560362A (en) | 2023-04-25 | 2023-04-25 | Automatic navigation path planning tracking method and system |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310454499.2A CN116560362A (en) | 2023-04-25 | 2023-04-25 | Automatic navigation path planning tracking method and system |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN116560362A true CN116560362A (en) | 2023-08-08 |
Family
ID=87501063
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202310454499.2A Withdrawn CN116560362A (en) | 2023-04-25 | 2023-04-25 | Automatic navigation path planning tracking method and system |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN116560362A (en) |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117170386A (en) * | 2023-10-11 | 2023-12-05 | 苏州欣和智达能源科技有限公司 | Operation control method and system for unmanned freight car |
-
2023
- 2023-04-25 CN CN202310454499.2A patent/CN116560362A/en not_active Withdrawn
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117170386A (en) * | 2023-10-11 | 2023-12-05 | 苏州欣和智达能源科技有限公司 | Operation control method and system for unmanned freight car |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN104571112B (en) | Pilotless automobile lateral control method based on turning curvature estimation | |
| CN113405558B (en) | Automatic driving map construction method and related device | |
| CN111830979B (en) | Track optimization method and device | |
| CN108628324B (en) | Unmanned vehicle navigation method, device, equipment and storage medium based on vector map | |
| Zhao et al. | Dynamic motion planning for autonomous vehicle in unknown environments | |
| CN108609014A (en) | The crosswise joint method and device of intelligent vehicle | |
| CN111006667B (en) | Automatic driving track generation system under high-speed scene | |
| CN110481551A (en) | A kind of control method, device and the vehicle of automatic lane-change | |
| CN107664993A (en) | A kind of paths planning method | |
| CN105955257A (en) | Bus automatic driving system based on fixed route and driving method thereof | |
| CN109579854A (en) | Unmanned vehicle barrier-avoiding method based on Quick Extended random tree | |
| CN109916421B (en) | Path planning method and device | |
| CN112947406A (en) | FLOYD and Astar-based hybrid path planning method | |
| CN116560362A (en) | Automatic navigation path planning tracking method and system | |
| CN112965481A (en) | Orchard operation robot unmanned driving method based on point cloud map | |
| CN113515111B (en) | Vehicle obstacle avoidance path planning method and device | |
| CN114815853B (en) | Path planning method and system considering road surface obstacle characteristics | |
| CN114593739B (en) | Vehicle global positioning method and device based on visual detection and reference line matching | |
| CN110031015B (en) | Method for approximately representing lane plane curve by using continuous derivative function | |
| CN114370874B (en) | Vehicle, vehicle path planning method and device | |
| CN111310919B (en) | Driving control strategy training method based on scene segmentation and local path planning | |
| CN116989808A (en) | Parallel parking path planning method for all-wheel steering vehicle considering uncertainty in positioning | |
| CN110069066A (en) | A kind of method and terminal that unmanned vehicle tracks | |
| CN114397890A (en) | Vehicle dynamic path planning method and device, electronic equipment and readable storage medium | |
| CN113390431A (en) | Method and device for dynamically generating reference line, computer equipment and storage medium |
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 | ||
| WW01 | Invention patent application withdrawn after publication | ||
| WW01 | Invention patent application withdrawn after publication |
Application publication date: 20230808 |