CN113778071A - Unmanned vehicle path planning method and device, electronic equipment, unmanned vehicle and medium - Google Patents
Unmanned vehicle path planning method and device, electronic equipment, unmanned vehicle and medium Download PDFInfo
- Publication number
- CN113778071A CN113778071A CN202010979321.6A CN202010979321A CN113778071A CN 113778071 A CN113778071 A CN 113778071A CN 202010979321 A CN202010979321 A CN 202010979321A CN 113778071 A CN113778071 A CN 113778071A
- Authority
- CN
- China
- Prior art keywords
- unmanned vehicle
- center line
- path
- coordinate system
- coordinate
- 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 36
- 230000008859 change Effects 0.000 claims description 13
- 230000007246 mechanism Effects 0.000 claims description 8
- 238000006243 chemical reaction Methods 0.000 claims description 6
- 230000003247 decreasing effect Effects 0.000 claims description 5
- 238000010586 diagram Methods 0.000 description 8
- 230000006870 function Effects 0.000 description 3
- 238000005516 engineering process Methods 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000013500 data storage Methods 0.000 description 1
- 238000010295 mobile communication Methods 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 230000008569 process Effects 0.000 description 1
- 230000008707 rearrangement Effects 0.000 description 1
- 239000007787 solid Substances 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
- 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
Abstract
The embodiment of the invention discloses a method and a device for planning a route of an unmanned vehicle, electronic equipment, the unmanned vehicle and a medium, wherein the method comprises the following steps: when the target path planning failure in the first coordinate system is detected, determining a second position coordinate of the unmanned vehicle in a second coordinate system according to a first position coordinate of the unmanned vehicle in the first coordinate system, wherein the second coordinate system comprises coordinate axes which are respectively along a road center line and are perpendicular to the road center line; in the second coordinate system, generating an alternative path connecting the second position coordinate and the road center line, wherein the alternative path is a monotonic curve; and controlling the unmanned vehicle to travel from the second position coordinate to the road center line along the alternative path and to travel to the destination along the current target path. The problem of current unmanned vehicle technique path planning success rate under the complicated scene is lower is solved.
Description
Technical Field
The embodiment of the invention relates to the field of unmanned vehicle control, in particular to an unmanned vehicle path planning method and device, electronic equipment, an unmanned vehicle and a medium.
Background
At present, the mobile robot technology is developed rapidly, and with the increasing application scenes and modes of the robot, various mobile robots are needed to be grown far away, and an unmanned vehicle is one of the mobile robots. Under normal conditions, the unmanned vehicle path planning module can output a safe feasible path, but in a scene with a complex environment, path planning failure can also occur.
In summary, in the process of implementing the present invention, the inventors found that at least the following technical problems exist in the prior art: when an existing unmanned vehicle encounters a path planning failure, a strategy adopted is to multiplex a path planned by a previous frame, but the success rate of the strategy in a complex scene is still low.
Disclosure of Invention
The embodiment of the invention provides a method and a device for planning a path of an unmanned vehicle, electronic equipment, the unmanned vehicle and a medium, and solves the problem of low success rate of path planning in a complex scene in the existing unmanned vehicle technology.
In a first aspect, an embodiment of the present invention provides an unmanned vehicle path planning method, including:
when the target path planning failure in the first coordinate system is detected, determining a second position coordinate of the unmanned vehicle in a second coordinate system according to a first position coordinate of the unmanned vehicle in the first coordinate system, wherein the second coordinate system comprises coordinate axes which are respectively along a road center line and are perpendicular to the road center line;
generating an alternative path connecting the second position coordinate and the road center line in the second coordinate system, wherein the alternative path is a monotonic curve;
and controlling the unmanned vehicle to travel from the second position coordinate to the road center line along the alternative path and to travel to the destination along the current target path.
In a second aspect, an embodiment of the present invention further provides an unmanned vehicle path planning apparatus, including:
the coordinate conversion module is used for determining a second position coordinate of the unmanned vehicle in a second coordinate system according to a first position coordinate of the unmanned vehicle in the first coordinate system when the target path in the first coordinate system fails, wherein the second coordinate system comprises coordinate axes which are respectively along a road center line and are perpendicular to the road center line;
the alternative path module is used for generating an alternative path for connecting the second position coordinate with the road center line in a second coordinate system; wherein the alternative path is a monotonic curve;
and the driving module is used for controlling the unmanned vehicle to drive from the second position coordinate to the road center line along the alternative path and drive to the destination along the current target path.
In a third aspect, an embodiment of the present invention further provides an electronic device, including:
one or more processors;
a memory for storing one or more programs;
when executed by the one or more processors, cause the one or more processors to implement the method of unmanned vehicle path planning as described in any of the embodiments.
In a fourth aspect, an embodiment of the present invention further provides an unmanned vehicle, including:
a vehicle body;
the running mechanism is arranged on the vehicle body and used for driving the vehicle body to run;
the processor is used for determining a second position coordinate of the unmanned vehicle in a second coordinate system according to a first position coordinate of the unmanned vehicle in a first coordinate system when the target path planning is detected to fail, wherein the second coordinate system comprises coordinate axes which are respectively along a road center line and are perpendicular to the road center line; in a second coordinate system, generating an alternative path connecting the second position coordinate and the road center line; wherein the alternative path is a monotonic curve; and controlling a running mechanism to drive the vehicle body to run from the second position coordinate to the road center line along the alternative path and to run to the destination along the current target path.
In a fifth aspect, embodiments of the present invention further provide a storage medium containing computer-executable instructions, which when executed by a computer processor, are configured to perform the unmanned vehicle path planning method according to any of the embodiments.
According to the technical scheme of the unmanned vehicle path planning method, when the target path planning failure of the unmanned vehicle in the first coordinate system is detected, the second position coordinate of the unmanned vehicle in the second coordinate system is determined according to the first position coordinate of the unmanned vehicle in the first coordinate system; generating an alternative path with the second position coordinate as a starting point in a second coordinate system, wherein the alternative path is a monotonic curve and can guide the unmanned vehicle to drive from the second position coordinate to the central line of the road, so that the unmanned vehicle can be quickly separated from the current complex scene; the unmanned vehicle returning to the center line of the road runs to the destination along the current target road, and the destination can be accurately and safely reached.
Drawings
In order to more clearly illustrate the technical solutions in the embodiments of the present invention, the drawings needed to be used in the description of the embodiments are briefly introduced below, and it is obvious that the drawings in the following description are some embodiments of the present invention, and it is obvious for those skilled in the art to obtain other drawings based on these drawings without creative efforts.
Fig. 1 is a flowchart of a method for planning a route of an unmanned aerial vehicle according to an embodiment of the present invention;
FIG. 2 is a schematic diagram of a road under both Cartesian and frient coordinate systems according to an embodiment of the present invention;
FIG. 3 is a schematic diagram of an alternative path provided by the first embodiment of the present invention;
fig. 4 is a block diagram of the unmanned vehicle route planning apparatus according to the second embodiment of the present invention;
fig. 5 is a block diagram of an electronic device according to a third embodiment of the present invention;
fig. 6 is a block diagram of an unmanned vehicle according to a fourth embodiment of the present invention.
Detailed Description
In order to make the objects, technical solutions and advantages of the present invention clearer, the technical solutions of the present invention will be clearly and completely described through embodiments with reference to the accompanying drawings in the embodiments of the present invention, and it is obvious that the described embodiments are some, but not all, embodiments of the present invention. All other embodiments, which can be derived by a person skilled in the art from the embodiments given herein without making any creative effort, shall fall within the protection scope of the present invention.
Example one
Fig. 1 is a flowchart of an unmanned vehicle route planning method according to an embodiment of the present invention. The technical scheme of the embodiment is suitable for the situation that the unmanned vehicle can quickly return to the central line of the road through the alternative path when the target path planning fails. The method can be executed by the unmanned vehicle path planning device provided by the embodiment of the invention, and the device can be realized in a software and/or hardware mode and is configured in a controller of the unmanned vehicle for application. The method specifically comprises the following steps:
s101, when failure of planning of a target path under a first coordinate system is detected, determining a second position coordinate of the unmanned vehicle in a second coordinate system according to a first position coordinate of the unmanned vehicle in the first coordinate system, wherein the second coordinate system comprises coordinate axes which are respectively along a road center line and are perpendicular to the road center line.
The target path planning failure comprises that the unmanned vehicle deviates from the center line of the road, and the distance between the unmanned vehicle and the edge of the road is within a preset early warning range. The first coordinate System includes, but is not limited to, a cartesian coordinate System, a UTM (Universal Transverse Mercator Grid System, UTM for short) coordinate System. The second coordinate system is preferably, but not limited to, a fresene coordinate system as long as it includes coordinate axes along and perpendicular to the center line of the roadway, respectively.
Take the first coordinate system as the Cartesian coordinate system and the second coordinate system as the friends coordinate system. If the road is complex, for example, the road is particularly curved, it is difficult to quickly plan a target path completely along the center line of the road in a cartesian coordinate system. If the target path is not completely along the center line of the road, the unmanned vehicle deviates from the center line of the road and drives to the edge of the road when driving along the target path. Once this condition occurs, and is not solved in time, the unmanned vehicle can rush out of the road. For this reason, the present embodiment determines the second position coordinate of the unmanned vehicle in the frient coordinate system according to the first position coordinate of the unmanned vehicle in the cartesian coordinate system, that is, quickly switches from the current cartesian coordinate system to the frient coordinate system.
In one embodiment, the Cartesian coordinate system is associated with a freshThe switching method of the et coordinate system comprises the following steps: as shown in fig. 2, the cartesian coordinate system is yMx, and the fresent coordinate system is SL, in which the S axis is along the direction of the center line 11 of the roadway and the L axis is along the direction perpendicular to the center line 11 of the roadway. The road centerline is composed of a series of discrete points, and the coordinates of each discrete point in a Cartesian coordinate system can be represented as ri(xi,yi,θi,kappai,dkappai) 1,2, n, wherein xiRepresenting the abscissa, y, of the discrete point in a Cartesian coordinate systemiRepresenting the ordinate value, theta, of the discrete point in a Cartesian coordinate systemiRepresenting the orientation angle of the discrete points in a cartesian coordinate system; kappa denotes the corresponding curvature of the discrete point in the cartesian coordinate system, and dkappa denotes the corresponding rate of change of curvature of the discrete point in the cartesian coordinate system. Setting the coordinate of a discrete point p in a Cartesian coordinate system as p (x)p,yp,θp,kappap,dkappap) The coordinate of the point p in the family of the friends coordinate system is(s)p,lp,lp',lp"). Determining two discrete points s (x) nearest to the discrete point p on the central line of the roads,ys,θs,kappas,dkappas) And e (x)e,ye,θe,kappae,dkappae) Setting the coordinate of the discrete point s in the freset coordinate system as(s)s0.0,0.0,0.0), the coordinate of the discrete point e in the freset coordinate system is(s)e0.0,0.0,0.0), then:
the vector of discrete points s pointing to discrete points p can be represented as:
the vector of discrete points e to discrete points p can be represented as:
calculating the vector by the following formula
And vector
A conversion coefficient between;
thus spCan be expressed as:
lpcan be expressed as:
the coordinate of the projection point of the discrete point p on the road center line in the Cartesian coordinate system is set as r (x)r,yr,θr,kappar,dkappar) Then, we can get:
from equations (6) and (7), we can derive:
in summary, the first position coordinate in the cartesian coordinate system can be transformed to the second position coordinate in the friends coordinate system according to the formulas (5), (6), (8) and (9).
And S102, generating an alternative path connecting the second position coordinate and the road center line in a second coordinate system, wherein the alternative path is a monotonic curve.
Because the S axis of the fresent coordinate system always follows the center line of the road, the center line of the road can be quickly positioned through the S axis of the fresent coordinate system along the center line of the road. Then, an alternative path 2 (see fig. 3) connecting the second position coordinates and the road center line 11 is planned according to the distribution of the second position coordinates and the road center line, and the alternative path 2 is a monotonic curve.
In some embodiments, referring to fig. 3, in the direction from the second position coordinate to the intersection of the alternative path and the road centerline, the coordinate values of the discrete points of the alternative path in the S axis (road centerline direction) are uniformly increased, the distance between the discrete points of the alternative path and the road centerline is uniformly decreased, and the curvature change rate of the discrete points are respectively uniformly decreased.
For example, each discrete point on the alternative path may be represented as:
where i is the index number of the discrete point on the alternative path, siFor the position of the discrete point with index number i on the alternative path in the direction parallel to the road centerline, liIs the position l 'of a discrete point with index number i in the direction vertical to the center line of the road on the alternative path'iThe curvature, l ″, of the alternative path corresponding to the discrete point with index number i on the alternative pathiThe curvature change rate(s) of the alternative path corresponding to the coordinate point with index number i on the alternative pathp，lp，l'p，l″p) Is the coordinate of the second position coordinate, spIs the value of the second position coordinate on the S axis, lpIs the value of the second position coordinate on the l axis l'pIs the curvature corresponding to the second position coordinate, l ″)pThe curvature change rate corresponding to the second position coordinate; n is the total number of discrete points on the alternative path, and the index number is gradually increased along the direction from the second position coordinate to the alternative path and the road center line.
As can be seen from equation (10) and FIG. 3, the alternative path
The road surface is a smooth path gradually advancing from the second position coordinate to the center line of the road in appearance, and the direction of the arc center of each arc part included in the road surface is consistent with the direction of the arc center of each arc part corresponding to the center line of the road. The alternative path in fig. 3 comprises two curves, each having a direction of the arc center coinciding with the direction of the arc center of the corresponding road center line. It will be appreciated that the alternative path has a tendency to coincide with the tendency of the centre line of the road. When the center line of the road is a straight line, the alternative path is also a straight line; when the center line of the road near the unmanned vehicle has only one curve part, the alternative path also has only one curve part; when the road center line near the unmanned vehicle consists of two arc parts, namely the unmanned vehicle currently corresponds to one arc part of the road center line, but the operation of returning to the road center line cannot be completed near the arc part, when the next arc part enters the road center line, the alternative path also has two arc parts, and the arc center direction of each arc part of the alternative path is consistent with the arc center direction of the arc part corresponding to the road center line.
And S103, controlling the unmanned vehicle to run from the second position coordinate to the road center line along the alternative path and to run to the destination along the current target path.
Taking the alternative path as the current path, referring to fig. 3, the unmanned vehicle 3 is controlled to travel from the second position coordinate to the road center line 11 along the alternative path 2 so as to depart from the current complex situation, and thus, the unmanned vehicle can travel to the destination along the current target path. Fig. 3 also shows that the orientation angle of the unmanned vehicle 3 is the same as the heading of the road center line 11 when the unmanned vehicle 3 travels along the alternative path onto the road center line 11.
In some embodiments, the road traveled by the unmanned vehicle is only heavily curved near the second location coordinates and the alternate path. When the unmanned vehicle enters the center line of the road along the alternative path, the unmanned vehicle can be considered to return to the original target path, and the original target path is still used as the current target path at the moment, and the unmanned vehicle is controlled to continue to run along the original target path; or replanning the target path in a Cartesian coordinate system or a frient coordinate system by taking the intersection point of the alternative path and the road center line as a starting point to update the target path, and controlling the unmanned vehicle to continuously drive to the destination along the updated target path, so that the destination can be accurately and safely reached.
In some embodiments, if there are a large number of curved road segments on the road traveled by the unmanned vehicle, the target path is re-planned in the fresent coordinate system, starting from the intersection of the alternative path and the road centerline, and the target path is always along the road centerline. And when the unmanned vehicle runs to the road center line from the alternative path, taking the newly planned target path as the current target path, and controlling the unmanned vehicle to run to the destination along the current target path.
According to the technical scheme of the unmanned vehicle path planning method, when the target path planning failure of the unmanned vehicle in the first coordinate system is detected, the second position coordinate of the unmanned vehicle in the second coordinate system is determined according to the first position coordinate of the unmanned vehicle in the first coordinate system; in a second coordinate system, generating an alternative path connecting the second position coordinate and the road center line, wherein the alternative path is a monotonic curve and can guide the unmanned vehicle to drive from the second position coordinate to the road center line, so that the unmanned vehicle can be quickly separated from the current complex scene; the unmanned vehicle returning to the center line of the road runs to the destination along the current target road, and the destination can be accurately and safely reached.
Example two
Fig. 4 is a block diagram of the unmanned vehicle route planning apparatus according to the second embodiment of the present invention. The device is used for executing the unmanned vehicle path planning method provided by any embodiment, and the device can be implemented in a software or hardware mode. The device comprises
The coordinate conversion module 41 is used for determining a second position coordinate of the unmanned vehicle in a second coordinate system according to a first position coordinate of the unmanned vehicle in the first coordinate system when the target path failure in the first coordinate system is detected, wherein the second coordinate system comprises coordinate axes which are respectively along a road center line and are perpendicular to the road center line;
an alternative path module 42, configured to generate an alternative path connecting the second position coordinate and the road centerline in the second coordinate system; wherein, the alternative path is a monotonic curve;
and the operation module 43 is used for controlling the unmanned vehicle to drive from the second position coordinate to the road center line along the alternative path and drive to the destination along the current target path.
Optionally, the failure of the target path planning includes that the unmanned vehicle deviates from the center line of the road, and the distance between the unmanned vehicle and the edge of the road is within a preset early warning range.
Optionally, the operation module 43 is configured to control the unmanned vehicle to travel from the second position coordinate to the road centerline along the alternative path, and replan the target path with an intersection of the alternative path and the road centerline as a starting point; and when the unmanned vehicle is detected to run to the road center line, controlling the unmanned vehicle to run to the destination along the current target path.
Optionally, in a direction from the second position coordinate to the intersection point of the alternative path and the road centerline, the curvature and the curvature change rate of each discrete point of the alternative path and the distance between each discrete point and the road centerline are respectively and uniformly decreased.
Optionally, each discrete point on the alternative path is represented as:
where i is the index number of the discrete point on the alternative path, siFor the position of the discrete point with index number i on the alternative path in the direction parallel to the road centerline, liFor a discrete point on the alternative path with index number iPosition in the direction of the road centerline, < l'iThe curvature, l ″, of the alternative path corresponding to the discrete point with index number i on the alternative pathiThe curvature change rate, s, of the alternative path corresponding to the coordinate point with index number i on the alternative pathpIs the value of the second position coordinate on the s-axis, lpIs the value of the second position coordinate on the l axis l'pIs the curvature corresponding to the second position coordinate, l ″)pThe curvature change rate corresponding to the second position coordinate; n is the total number of discrete points on the alternative path.
Optionally, the second coordinate system is a friends coordinate system.
According to the technical scheme of the unmanned vehicle path planning device, when the coordinate conversion module detects that the target path planning of the unmanned vehicle in the first coordinate system fails, the second position coordinate of the unmanned vehicle in the second coordinate system is determined according to the first position coordinate of the unmanned vehicle in the first coordinate system; generating an alternative path connecting the second position coordinate and the road center line in a second coordinate system through an alternative path module, wherein the alternative path is a monotonic curve and can guide the unmanned vehicle to quickly and safely drive from the second position coordinate to the road center line, so that the unmanned vehicle can be quickly separated from the current complex scene; the unmanned vehicle returning to the center line of the road can accurately and safely reach the destination by driving to the destination along the current target path.
The unmanned vehicle path planning device provided by the embodiment of the invention can execute the unmanned vehicle path planning method provided by any embodiment of the invention, and has corresponding functional modules and beneficial effects of the execution method.
EXAMPLE III
Fig. 5 is a schematic structural diagram of an electronic apparatus according to a third embodiment of the present invention, as shown in fig. 5, the electronic apparatus includes a processor 501, a memory 502, an input device 503, and an output device 504; the number of the processors 501 in the device may be one or more, and one processor 501 is taken as an example in fig. 5; the processor 501, the memory 502, the input device 503 and the output device 504 of the apparatus may be connected by a bus or other means, and fig. 5 illustrates the connection by a bus as an example.
The memory 502, which is a computer-readable storage medium, may be used to store software programs, computer-executable programs, and modules, such as program instructions/modules (e.g., the coordinate conversion module 41, the alternative path module 42, and the travel module 43) corresponding to the unmanned vehicle path planning method in the embodiments of the present invention. The processor 501 executes various functional applications of the device and data processing by running software programs, instructions, and modules stored in the memory 502, that is, implements the above-described unmanned vehicle path planning method.
The memory 502 may mainly include a program storage area and a data storage area, wherein the program storage area may store an operating system, an application program required for at least one function; the storage data area may store data created according to the use of the terminal, and the like. Further, the memory 502 may include high speed random access memory, and may also include non-volatile memory, such as at least one magnetic disk storage device, flash memory device, or other non-volatile solid state storage device. In some examples, memory 502 may further include memory located remotely from processor 501, which may be connected to devices through a network. Examples of such networks include, but are not limited to, the internet, intranets, local area networks, mobile communication networks, and combinations thereof.
The input device 503 may be used to receive input numeric or character information and generate key signal inputs related to user settings and function control of the apparatus.
The output device 504 may include a display device such as a display screen, for example, of a user terminal.
Example four
Fig. 6 is a schematic structural diagram of an unmanned vehicle according to a fourth embodiment of the present invention. The unmanned vehicle can quickly return to the central line of the road through the alternative path when the target path planning fails. As shown in fig. 6, the unmanned vehicle includes a vehicle body, a traveling mechanism 6 for driving the vehicle body to travel, and a processor 501 connected to the traveling mechanism, where the processor 501 is configured to determine a second position coordinate of the unmanned vehicle in a second coordinate system according to a first position coordinate of the unmanned vehicle in a first coordinate system when a failure of planning a target path is detected, where the second coordinate system includes coordinate axes along a center line of a road and perpendicular to the center line of the road; in a second coordinate system, generating an alternative path connecting the second position coordinate and the road center line; wherein, the alternative path is a monotonic curve; and controlling the running mechanism to drive the vehicle body to run from the second position coordinate to the road center line along the alternative path, and continuing to run to the destination along the current target path.
The target path planning failure comprises that the unmanned vehicle deviates from the center line of the road, and the distance between the unmanned vehicle and the edge of the road is within a preset early warning range. The first coordinate System includes, but is not limited to, a cartesian coordinate System, a UTM (Universal Transverse Mercator Grid System, UTM for short) coordinate System. The second coordinate system is preferably, but not limited to, a fresene coordinate system as long as it includes coordinate axes along and perpendicular to the center line of the roadway, respectively.
Take the first coordinate system as the Cartesian coordinate system and the second coordinate system as the friends coordinate system. If the road is complicated, for example, the road is particularly curved, it is difficult to quickly plan a target path completely along the center line of the road in a cartesian coordinate system, and if the target path is not completely along the center line of the road, the unmanned vehicle may deviate from the center line of the road and drive to the edge of the road when driving along the target path. Once such a situation occurs and is not solved in time, the unmanned vehicle can rush out of the road. For this reason, the present embodiment determines the second position coordinate of the unmanned vehicle in the frient coordinate system according to the first position coordinate of the unmanned vehicle in the cartesian coordinate system, that is, quickly switches from the current cartesian coordinate system to the frient coordinate system.
Since the S-axis of the wavelet coordinate system always follows the road centerline, the road centerline can be quickly located through the S-axis of the wavelet coordinate system along the road centerline, and then an alternative path (see fig. 3) connecting the second position coordinate and the road centerline is planned according to the distribution of the second position coordinate and the road centerline, and the alternative path is a monotonic curve.
In some embodiments, the curvature and the curvature change rate of each discrete point of the alternative path, and the distance between each discrete point and the center line of the roadway are uniformly decreased respectively in the direction from the second position coordinate to the intersection of the alternative path and the center line of the roadway.
For example, each discrete point on the alternative path may be represented as:
where i is the index number of the discrete point on the alternative path, siFor the position of the discrete point with index number i on the alternative path in the direction parallel to the road centerline, liFor the position of the discrete point with index number i on the alternative path in the direction perpendicular to the center line of the road, li' is the curvature, l ″, of the alternative path corresponding to the discrete point with index i on the alternative pathiThe curvature change rate(s) of the alternative path corresponding to the coordinate point with index number i on the alternative pathp，lp，l'p，l″p) Is the coordinate of the second position coordinate, spIs the value of the second position coordinate on the S axis, lpIs the value of the second position coordinate on the l axis l'pIs the curvature corresponding to the second position coordinate, l ″)pThe curvature change rate corresponding to the second position coordinate; n is the total number of discrete points on the alternative path, and the index number is gradually increased along the direction from the second position coordinate to the alternative path and the road center line.
As can be seen from equation (10) and FIG. 3, the alternative path
The appearance is a smooth path gradually from the second position coordinate to the center line of the road, and the arc center direction of each arc included in the smooth path is consistent with the arc center direction of the corresponding arc of the center line of the road. The alternative path in fig. 3 includes two arcs, each arc having a center of arc direction that coincides with the center of arc direction of the corresponding road centerline. It will be appreciated that the alternative path has a tendency to coincide with the tendency of the centre line of the road. When the center line of the road is a straight line, the alternative path is also a straight line; when the center line of the road near the unmanned vehicle has only one arc line, the alternative path also has only one arc line; when the road center line near the unmanned vehicle is composed of two arc lines, namely the unmanned vehicle currently corresponds to one arc line of the road center line, but the operation of returning to the road center line cannot be completed near the arc line, the next arc line needs to enter the road center line, at the moment, the alternative path also has two arc lines, and the arc center direction of each arc line of the alternative path is consistent with the arc center direction of the arc line corresponding to the road center line.
And taking the alternative path as the current path, and controlling a running mechanism to drive the vehicle body to run from the second position coordinate to the road center line along the alternative path, so that the vehicle body can run to the destination along the current target path without the current complex condition.
In some embodiments, the road traveled by the unmanned vehicle is only heavily curved near the second location coordinates and the alternate path. When the unmanned vehicle enters the center line of the road along the alternative path, the unmanned vehicle can be considered to return to the original target path, the current target path is still taken as the target path along the target path, and the unmanned vehicle is controlled to continue to run along the original target path; or replanning the target path in a Cartesian coordinate system or a frient coordinate system by taking the intersection point of the alternative path and the road center line as a starting point to update the target path and control the unmanned vehicle to continue to run along the updated target path, so that the destination can be accurately and safely reached.
In some embodiments, if there are a large number of curved road segments on the road traveled by the unmanned vehicle, the target path is re-planned in the fresent coordinate system, starting from the intersection of the alternative path and the road centerline, and the target path is always along the road centerline. And when the unmanned vehicle runs to the road center line from the alternative path, taking the newly planned target path as the current target path, and controlling the unmanned vehicle to run along the current target path.
According to the technical scheme of the unmanned vehicle, when the target path planning failure of the unmanned vehicle in the first coordinate system is detected, the second position coordinate of the unmanned vehicle in the second coordinate system is determined according to the current first position coordinate of the unmanned vehicle in the first coordinate system; in a second coordinate system, generating an alternative path connecting the second position coordinate and the road center line, wherein the alternative path is a monotonic curve and can guide the unmanned vehicle to drive from the second position coordinate to the road center line, so that the unmanned vehicle can be quickly separated from the current complex scene; the unmanned vehicle returning to the center line of the road runs to the destination along the current target road, and the destination can be accurately and safely reached.
EXAMPLE five
An embodiment of the present invention further provides a storage medium containing computer-executable instructions, which when executed by a computer processor, are configured to perform a method for unmanned vehicle path planning, the method including:
when the target path planning failure in the first coordinate system is detected, determining a second position coordinate of the unmanned vehicle in a second coordinate system according to a first position coordinate of the unmanned vehicle in the first coordinate system, wherein the second coordinate system comprises coordinate axes which are respectively along a road center line and are perpendicular to the road center line;
in the second coordinate system, generating an alternative path connecting the second position coordinate and the road center line, wherein the alternative path is a monotonic curve;
and controlling the unmanned vehicle to travel from the second position coordinate to the road center line along the alternative path and to travel to the destination along the current target path.
Of course, the storage medium containing the computer-executable instructions provided by the embodiments of the present invention is not limited to the method operations described above, and may also perform related operations in the unmanned vehicle path planning method provided by any embodiments of the present invention.
From the above description of the embodiments, it is obvious for those skilled in the art that the present invention can be implemented by software and necessary general hardware, and certainly, can also be implemented by hardware, but the former is a better embodiment in many cases. Based on such understanding, the technical solutions of the present invention may be embodied in the form of a software product, which may be stored in a computer-readable storage medium, such as a floppy disk, a Read-Only Memory (ROM), a Random Access Memory (RAM), a FLASH Memory (FLASH), a hard disk or an optical disk of a computer, and includes several instructions for enabling a computer device (which may be a personal computer, a server, or a network device) to execute the method for planning routes of an unmanned vehicle according to the embodiments of the present invention.
It should be noted that, in the embodiment of the unmanned vehicle path planning apparatus, each unit and each module included in the unmanned vehicle path planning apparatus are only divided according to functional logic, but are not limited to the above division, as long as the corresponding function can be realized; in addition, specific names of the functional units are only for convenience of distinguishing from each other, and are not used for limiting the protection scope of the present invention.
It is to be noted that the foregoing is only illustrative of the preferred embodiments of the present invention and the technical principles employed. It will be understood by those skilled in the art that the present invention is not limited to the particular embodiments described herein, but is capable of various obvious changes, rearrangements and substitutions as will now become apparent to those skilled in the art without departing from the scope of the invention. Therefore, although the present invention has been described in greater detail by the above embodiments, the present invention is not limited to the above embodiments, and may include other equivalent embodiments without departing from the spirit of the present invention, and the scope of the present invention is determined by the scope of the appended claims.
Claims (10)
1. An unmanned vehicle path planning method is characterized by comprising the following steps:
when the target path planning failure in the first coordinate system is detected, determining a second position coordinate of the unmanned vehicle in a second coordinate system according to a first position coordinate of the unmanned vehicle in the first coordinate system, wherein the second coordinate system comprises coordinate axes which are respectively along a road center line and are perpendicular to the road center line;
in the second coordinate system, generating an alternative path connecting the second position coordinate and the road center line, wherein the alternative path is a monotonic curve;
and controlling the unmanned vehicle to travel from the second position coordinate to the road center line along the alternative path and to travel to the destination along the current target path.
2. The method of claim 1, wherein the failure of the target path plan comprises: the unmanned vehicle deviates from the center line of the road, and the distance between the unmanned vehicle and the edge of the road is within a preset early warning range.
3. The method of claim 1, wherein the controlling the unmanned vehicle to travel along the alternate path from the second location coordinate onto the road centerline and along the current target path to the destination comprises:
controlling the unmanned vehicle to drive from the second position coordinate to the road center line along the alternative path, and replanning a target path by taking the intersection point of the alternative path and the road center line as a starting point;
and when the unmanned vehicle is detected to run to the road center line, controlling the unmanned vehicle to run to the destination along the current target path.
4. The method according to claim 1, wherein the curvature and the curvature change rate of each discrete point of the alternative path and the distance between each discrete point and the center line of the roadway are uniformly decreased in the direction from the second position coordinate to the intersection of the alternative path and the center line of the roadway, respectively.
5. The method of claim 4, wherein each discrete point on the alternative path is represented as:
where i is the index number of the discrete point on the alternative path, siFor the position of the discrete point with index number i on the alternative path in the direction parallel to the center line of the road,/iIs the position l 'of a discrete point with index number i in the direction vertical to the road center line on the alternative path'iThe curvature, l ″, of the alternative path corresponding to the discrete point with index number i on the alternative pathiThe curvature change rate, s, of the alternative path corresponding to the coordinate point with index number i on the alternative pathpIs the value of the second position coordinate on the s-axis, lpIs the value of the second position coordinate on the l axis l'pIs the curvature corresponding to the second position coordinate, l ″)pThe curvature change rate corresponding to the second position coordinate; n is the total number of discrete points on the alternative path.
6. The method according to any one of claims 1-5, wherein the second coordinate system is a frenet coordinate system.
7. An unmanned vehicle path planning device, comprising:
the coordinate conversion module is used for determining a second position coordinate of the unmanned vehicle in a second coordinate system according to a first position coordinate of the unmanned vehicle in the first coordinate system when the target path in the first coordinate system fails, wherein the second coordinate system comprises coordinate axes which are respectively along a road center line and are perpendicular to the road center line;
the alternative path module is used for generating an alternative path for connecting the second position coordinate with the road center line in a second coordinate system; wherein the alternative path is a monotonic curve;
and the operation module is used for controlling the unmanned vehicle to drive from the second position coordinate to the road center line along the alternative path and drive to the destination along the current target path.
8. An electronic device, comprising:
one or more processors;
a memory for storing one or more programs;
when executed by the one or more processors, cause the one or more processors to implement the method of unmanned vehicle path planning as claimed in any of claims 1-6.
9. An unmanned vehicle, comprising:
a vehicle body;
the running mechanism is arranged on the vehicle body and used for driving the vehicle body to run;
the processor is used for determining a second position coordinate of the unmanned vehicle in a second coordinate system according to a first position coordinate of the unmanned vehicle in a first coordinate system when the target path planning is detected to fail, wherein the second coordinate system comprises coordinate axes which are respectively along a road center line and are perpendicular to the road center line; in a second coordinate system, generating an alternative path connecting the second position coordinate and the road center line; wherein the alternative path is a monotonic curve; and controlling a running mechanism to drive the vehicle body to run from the second position coordinate to the road center line along the alternative path and to run to the destination along the current target path.
10. A storage medium containing computer-executable instructions for performing the unmanned vehicle path planning method of any of claims 1-6 when executed by a computer processor.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202010979321.6A CN113778071A (en) | 2020-09-17 | 2020-09-17 | Unmanned vehicle path planning method and device, electronic equipment, unmanned vehicle and medium |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202010979321.6A CN113778071A (en) | 2020-09-17 | 2020-09-17 | Unmanned vehicle path planning method and device, electronic equipment, unmanned vehicle and medium |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN113778071A true CN113778071A (en) | 2021-12-10 |
Family
ID=78835057
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202010979321.6A Pending CN113778071A (en) | 2020-09-17 | 2020-09-17 | Unmanned vehicle path planning method and device, electronic equipment, unmanned vehicle and medium |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN113778071A (en) |
Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20180304890A1 (en) * | 2017-04-19 | 2018-10-25 | Toyota Jidosha Kabushiki Kaisha | Autonomous driving system |
| CN109324620A (en) * | 2018-09-25 | 2019-02-12 | 北京主线科技有限公司 | The dynamic trajectory planing method for carrying out avoidance based on lane line parallel offset and overtaking other vehicles |
| CN109631902A (en) * | 2018-12-28 | 2019-04-16 | 天津大学 | The online optimal path planning method near point of ship based on Ocean Surveying |
| CN110362096A (en) * | 2019-08-13 | 2019-10-22 | 东北大学 | A kind of automatic driving vehicle dynamic trajectory planing method based on local optimality |
| CN110533730A (en) * | 2019-08-26 | 2019-12-03 | 智久(厦门)机器人科技有限公司 | Paths planning method, device, computer readable storage medium based on B-spline curves |
| CN110749333A (en) * | 2019-11-07 | 2020-02-04 | 中南大学 | Unmanned vehicle motion planning method based on multi-objective optimization |
| CN111231950A (en) * | 2020-03-05 | 2020-06-05 | 北京百度网讯科技有限公司 | Method, device and equipment for planning lane change path of vehicle and readable storage medium |
| CN111338346A (en) * | 2020-03-05 | 2020-06-26 | 中国第一汽车股份有限公司 | Automatic driving control method and device, vehicle and storage medium |
-
2020
- 2020-09-17 CN CN202010979321.6A patent/CN113778071A/en active Pending
Patent Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20180304890A1 (en) * | 2017-04-19 | 2018-10-25 | Toyota Jidosha Kabushiki Kaisha | Autonomous driving system |
| CN109324620A (en) * | 2018-09-25 | 2019-02-12 | 北京主线科技有限公司 | The dynamic trajectory planing method for carrying out avoidance based on lane line parallel offset and overtaking other vehicles |
| CN109631902A (en) * | 2018-12-28 | 2019-04-16 | 天津大学 | The online optimal path planning method near point of ship based on Ocean Surveying |
| CN110362096A (en) * | 2019-08-13 | 2019-10-22 | 东北大学 | A kind of automatic driving vehicle dynamic trajectory planing method based on local optimality |
| CN110533730A (en) * | 2019-08-26 | 2019-12-03 | 智久(厦门)机器人科技有限公司 | Paths planning method, device, computer readable storage medium based on B-spline curves |
| CN110749333A (en) * | 2019-11-07 | 2020-02-04 | 中南大学 | Unmanned vehicle motion planning method based on multi-objective optimization |
| CN111231950A (en) * | 2020-03-05 | 2020-06-05 | 北京百度网讯科技有限公司 | Method, device and equipment for planning lane change path of vehicle and readable storage medium |
| CN111338346A (en) * | 2020-03-05 | 2020-06-26 | 中国第一汽车股份有限公司 | Automatic driving control method and device, vehicle and storage medium |
Non-Patent Citations (1)
| Title |
|---|
| 肖宏宇;付志强;陈慧勇;: "面向低速自动驾驶车辆的避障规划研究", 同济大学学报(自然科学版), no. 1, 15 December 2019 (2019-12-15), pages 170 - 176 * |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| US20140207325A1 (en) | Efficient data flow algorithms for autonomous lane changing, passing and overtaking behaviors | |
| CN110162029B (en) | Motion control method and device based on planned path and robot | |
| JP2023546810A (en) | Vehicle trajectory planning method, vehicle trajectory planning device, electronic device, and computer program | |
| CN113682318B (en) | Vehicle running control method and device | |
| CN113085850B (en) | Vehicle obstacle avoidance method and device, electronic equipment and storage medium | |
| CN111007862B (en) | Path planning method for cooperative work of multiple AGVs | |
| WO2020125686A1 (en) | Method for generating real-time relative map, intelligent driving device and computer storage medium | |
| CN111813117A (en) | Robot line patrol priority navigation method, device and equipment | |
| CN109062218B (en) | Method and device for controlling robot driving path | |
| CN109115220B (en) | Method for parking lot system path planning | |
| CN111998858B (en) | Unmanned aerial vehicle route planning method based on improved A-algorithm | |
| CN112327826A (en) | Path planning method, device, equipment and medium | |
| CN112622933A (en) | Method and device for determining vehicle stop point | |
| CN114923496A (en) | Path planning method and device, electronic equipment and storage medium | |
| CN114179815B (en) | Method and device for determining vehicle driving track, vehicle, electronic equipment and medium | |
| CN114061606B (en) | Path planning method, path planning device, electronic equipment and storage medium | |
| CN114771551A (en) | Method and device for planning track of automatic driving vehicle and automatic driving vehicle | |
| CN114526752A (en) | Path planning method and device, electronic equipment and storage medium | |
| CN113759892A (en) | Unmanned vehicle obstacle detouring method and device, unmanned vehicle and storage medium | |
| EP4151487A2 (en) | Method and apparatus for controlling lane changing, electronic device and storage medium | |
| CN113778071A (en) | Unmanned vehicle path planning method and device, electronic equipment, unmanned vehicle and medium | |
| CN110083158B (en) | Method and equipment for determining local planning path | |
| CN113401141B (en) | Route processing method and device | |
| CN115743183A (en) | Automatic driving control method, device, equipment, medium and vehicle | |
| CN111830957A (en) | Path planning method and device |
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 |