CN115355916B - Trajectory planning method, apparatus and computer-readable storage medium for moving tool - Google Patents
Trajectory planning method, apparatus and computer-readable storage medium for moving tool Download PDFInfo
- Publication number
- CN115355916B CN115355916B CN202211298959.9A CN202211298959A CN115355916B CN 115355916 B CN115355916 B CN 115355916B CN 202211298959 A CN202211298959 A CN 202211298959A CN 115355916 B CN115355916 B CN 115355916B
- Authority
- CN
- China
- Prior art keywords
- planning
- moving tool
- obstacle
- tool
- trajectory
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/20—Instruments for performing navigational calculations
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
The application relates to a trajectory planning method, equipment and a computer-readable storage medium for a mobile tool. The method comprises the following steps: when the track of the moving tool is planned transversely, roughly planning the track of the moving tool longitudinally to obtain a roughly longitudinal planning result; performing transverse planning on the track of the moving tool based on the rough longitudinal planning result; judging whether the transverse planning of the moving tool track is successful or not; and if the transverse planning of the track of the moving tool is successful, after the successful transverse planning is completed, performing longitudinal planning on the track of the moving tool by taking the rough longitudinal planning result as an optimization target. The technical scheme of the application can enhance the intelligent degree of track planning, thereby improving the track planning efficiency and the operation safety of the moving tool.
Description
Technical Field
The present application relates to the field of artificial intelligence, and in particular, to a trajectory planning method, apparatus, and computer-readable storage medium for a mobile tool.
Background
With the rapid development of scientific technology, artificial intelligence technology is increasingly used, for example, mobile tools based on artificial intelligence, such as automatic driving vehicles and intelligent robots. For the above-mentioned mobile tools, trajectory planning is a key technology. Trajectory planning includes lateral planning, also referred to as path planning, which determines information such as the position and orientation of a mobile tool based on constraint information such as the position and velocity of an obstacle, and vertical planning, which predicts and adjusts data such as the velocity and acceleration of a mobile tool based on information such as the constraint of an obstacle and the curvature of a path planned in the lateral direction.
In the related art, the trajectory planning of the moving tool is a decoupling of the transverse planning and the longitudinal planning, that is, information of the transverse planning such as the position and the orientation of the moving tool and information of the longitudinal planning such as the speed and the acceleration of the moving tool are planned through independent algorithms respectively. However, the decoupling of the transverse planning and the longitudinal planning has obvious defects, which are mainly reflected in that when a scene is relatively complex, in the process of planning a track each time, the transverse planning and the longitudinal planning of a moving tool are required to be completed first, the response of the moving tool to a dynamic obstacle is not active enough, and the intelligent degree is limited.
Disclosure of Invention
In order to solve or partially solve the problems in the related art, the application provides a trajectory planning method and device for a mobile tool, and a computer-readable storage medium, which can enhance the intelligence degree of trajectory planning, thereby improving the trajectory planning efficiency and the operation safety of the mobile tool.
A first aspect of the present application provides a trajectory planning method for a mobile tool, which is applied to a trajectory planning cycle of the mobile tool, and includes:
when the track of the moving tool is planned transversely, roughly planning the track of the moving tool longitudinally to obtain a roughly longitudinal planning result;
performing transverse planning on the track of the moving tool based on the rough longitudinal planning result;
judging whether the transverse planning is successful;
and if the transverse planning is successful, after the successful transverse planning is completed, longitudinally planning the track of the mobile tool by taking the rough longitudinal planning result as an optimization target.
A second aspect of the present application provides a trajectory planning device for a mobile tool, which is applied to a trajectory planning cycle of the mobile tool, and includes:
the first longitudinal planning module is used for carrying out rough longitudinal planning on the track of the moving tool when the track of the moving tool starts to be planned transversely so as to obtain a rough longitudinal planning result;
a transverse planning module, configured to perform transverse planning on the trajectory of the moving tool based on the coarse longitudinal planning result;
the judging module is used for judging whether the transverse planning is successful or not;
and the second longitudinal planning module is used for performing longitudinal planning on the track of the mobile tool by taking the rough longitudinal planning result as an optimization target after the successful transverse planning is completed if the transverse planning of the current track planning period is successful.
A third aspect of the present application provides an electronic device comprising:
a processor; and
a memory having executable code stored thereon, which when executed by the processor, causes the processor to perform the method as described above.
A fourth aspect of the present application provides a computer-readable storage medium having stored thereon executable code, which, when executed by a processor of an electronic device, causes the processor to perform the method as described above.
A fifth aspect of the present application provides a computer program product, which, when run on a computer, causes the computer to execute computer program code instructions corresponding to the above-described method.
A sixth aspect of the present application provides a moving tool, including the electronic device provided in the third aspect.
The technical scheme provided by the application can comprise the following beneficial effects: compared with the prior art that the response to the dynamic obstacle is not sufficiently active and the intelligent degree is poor due to the fact that the longitudinal planning and the transverse planning are completely decoupled when the track of the moving tool is planned, the technical scheme of the application is that on one hand, the track of the moving tool is roughly longitudinally planned when the transverse planning is started, and then the track of the moving tool is transversely planned according to the roughly longitudinally planned result, in other words, the transverse planning result capable of actively avoiding the dynamic obstacle is obtained through weak coupling of the longitudinal planning and the transverse planning, so that the intelligent degree of the track planning of the moving tool is improved; on the other hand, after the transverse planning is finished, the track of the mobile tool is planned longitudinally by taking the rough longitudinal planning result as an optimization target, so that the longitudinal planning result approaches the optimization target as much as possible, and the planned path in the transverse planning link can be executed really and smoothly. In conclusion, the technical scheme of the application fundamentally enhances the intelligence degree of the trajectory planning, and promotes the trajectory planning efficiency and the operation safety of the mobile tool.
It is to be understood that both the foregoing general description and the following detailed description are exemplary and explanatory only and are not restrictive of the application.
Drawings
The foregoing and other objects, features and advantages of the application will be apparent from the following more particular descriptions of exemplary embodiments of the application as illustrated in the accompanying drawings wherein like reference numbers generally represent like parts throughout the exemplary embodiments of the application.
FIG. 1a is a diagram illustrating a related art trajectory planning for a mobile tool according to an embodiment of the present application;
FIG. 1b is a schematic diagram illustrating a related art trajectory planning for a mobile tool according to another embodiment of the present application;
fig. 2 is a flowchart illustrating a trajectory planning method for a mobile tool according to an embodiment of the present disclosure;
fig. 3a is a schematic diagram illustrating an estimation of a collision position of a mobile tool with an obstacle according to current positioning information of the mobile tool, state information of the obstacle on a driving road of the mobile tool, and a reference value according to an embodiment of the present application;
fig. 3b is a schematic diagram of an obstacle avoidance decision assuming that a collision position of a predicted moving tool with an obstacle is at a position identified by (1) in fig. 3a according to an embodiment of the present application;
FIG. 4 is a schematic view of the mobile tool fully blocking the lateral passable space according to the boundary determined by the dynamic obstacle and the static obstacle;
FIG. 5 is a schematic diagram of a Frenet coordinate system shown in an embodiment of the present application;
FIG. 6 is a diagram illustrating an ST diagram based on the Frenet coordinate system in accordance with an embodiment of the present application;
FIG. 7 shows an embodiment of the present application with coordinates of: (A), (B), and (C)
，
) The distance-time ST map is sampled based on the current travel speed of the moving tool to obtain a schematic diagram of four distance-time curves of the moving tool under the ST map;
fig. 8 is a schematic diagram illustrating the predicted collision position between the moving tool and the obstacle when the projected area of the obstacle m in the ST diagram and the distance-time curve B of the moving tool under the ST diagram overlap the point P in the embodiment of the present application;
FIG. 9 is a schematic diagram illustrating a transverse planning of a current trajectory planning cycle of a trajectory of a mobile tool using a curve A illustrated in FIG. 7 for the scenario illustrated in FIG. 4 according to an embodiment of the present application;
fig. 10a is a schematic diagram of a moving tool being laterally planned an obstacle avoidance path according to a curve B obtained in fig. 7 according to an embodiment of the present application;
FIG. 10b is a schematic diagram of an embodiment of the present application illustrating the lateral planning of an obstacle avoidance path for a mobile tool according to curve C obtained in FIG. 7;
fig. 11 is a schematic diagram of a distance-time curve obtained by performing longitudinal planning on the trajectory of the mobile tool in the current trajectory planning cycle with the curve a illustrated in fig. 7 as an optimization target according to an embodiment of the present application;
fig. 12 is a schematic structural diagram of a trajectory planning device for a mobile tool according to an embodiment of the present application;
fig. 13 is a schematic structural diagram of an electronic device shown in an embodiment of the present application.
Detailed Description
Embodiments of the present application will be described in more detail below with reference to the accompanying drawings. While embodiments of the present application are illustrated in the accompanying drawings, it should be understood that the present application may be embodied in various forms and should not be limited to the embodiments set forth herein. Rather, these embodiments are provided so that this disclosure will be thorough and complete, and will fully convey the scope of the disclosure to those skilled in the art.
The terminology used herein is for the purpose of describing particular embodiments only and is not intended to be limiting of the application. As used in this application and the appended claims, the singular forms "a", "an", and "the" are intended to include the plural forms as well, unless the context clearly indicates otherwise. It should also be understood that the term "and/or" as used herein refers to and encompasses any and all possible combinations of one or more of the associated listed items.
It should be understood that although the terms "first," "second," "third," etc. may be used herein to describe various information, these information should not be limited to these terms. These terms are only used to distinguish one type of information from another. For example, first information may also be referred to as second information, and similarly, second information may also be referred to as first information, without departing from the scope of the present application. Thus, a feature defined as "first" or "second" may explicitly or implicitly include one or more of that feature. In the description of the present application, "a plurality" means two or more unless specifically limited otherwise.
Trajectory planning is a key technology for mobile tools based on artificial intelligence, such as autonomous vehicles, intelligent robots, and the like. Trajectory planning includes lateral planning, also referred to as path planning, which determines information such as the position and orientation of a mobile tool based on constraint information such as the position and velocity of an obstacle, and vertical planning, which predicts and adjusts data such as the velocity and acceleration of a mobile tool based on information such as the constraint of an obstacle and the curvature of a path planned in the lateral direction. In the related art, the trajectory planning of the moving tool is a decoupling of the transverse planning and the longitudinal planning, that is, the information of the transverse planning, such as the position and the orientation of the moving tool, and the information of the longitudinal planning, such as the speed and the acceleration of the moving tool, are planned through independent algorithms respectively. In other words, in the related art, when planning the trajectory of the moving tool, the general idea is as follows: firstly, performing transverse planning on the basis of a reference path and all static obstacles to obtain a path without collision with the static obstacles; and (3) making obstacle avoidance decisions on all dynamic obstacles on the basis of the path, namely obtaining a longitudinal planning track according to decision results of each dynamic obstacle in the states of 'exceeding', 'giving way' or 'following', and the like of each dynamic obstacle, and finally combining the transverse and longitudinal results to obtain a final track.
Take the scenario of two vehicles in tandem in a two-way dual lane as illustrated in fig. 1a as an example. The host vehicle (vehicle 1 in the example of fig. 1 a) travels along the black solid line from the right-side lane in the traveling direction (as indicated by the arrow direction in the figure), and the obstacle (vehicle 2 in the example of fig. 1 a) in the left-side lane travels along its black predicted trajectory, possibly encroaching on the host vehicle traveling space. In the process of transverse planning of the self-vehicle, because no static barrier blocks the path, the shape of the planned path cannot be changed, and in the process of longitudinal planning, the action of decelerating and stopping can be planned in consideration of the condition that the driving track is invaded by a dynamic barrier. In order to improve the response capability to the dynamic obstacle, an improved scheme is to consider the predicted track of the dynamic obstacle as a static obstacle, so that the self-vehicle makes some actions far away from the dynamic obstacle in a horizontal planning link, as shown in fig. 1b (the vehicle 2 in the example of fig. 1b is a dynamic obstacle). After the predicted track of the obstacle is expanded, an area expressed by a dotted line shadow part is obtained, the whole area is used as a static obstacle to be processed, and under the condition that the scene is single, the effect of avoiding the dynamic obstacle can be achieved to a certain extent by the self-vehicle.
However, both the solution corresponding to the exemplary scenario of fig. 1a and the improved solution corresponding to the exemplary scenario of fig. 1b have drawbacks in different degrees. The above-mentioned drawback is essentially caused by the fact that the collision position judgment of the dynamic barrier is not accurate enough when the lateral planning is performed. In other words, it is not known at the time of the lateral planning which speed and acceleration the vehicle will follow, and these attributes can be obtained only by completing the longitudinal planning, but the speed of the vehicle is different, and the collision position with the obstacle is naturally different. Therefore, in the scenario corresponding to the example scenario in fig. 1a, only static obstacles are considered during the horizontal planning process, while only dynamic obstacles can be handled by controlling the speed in the vertical planning. As for the scenario illustrated in fig. 1b, the corresponding technical solution is to treat the predicted trajectories of all dynamic obstacles (the vehicle 2 illustrated in fig. 1 b) as static obstacles, which is obviously not suitable in a complex scenario because the predicted trajectories of a plurality of dynamic obstacles easily block all passable spaces, so that an unresolvable situation occurs.
In view of the above problems, embodiments of the present application provide a trajectory planning method for a mobile tool, which can enhance the intelligence degree of trajectory planning, thereby improving the trajectory planning efficiency and the operation safety of the mobile tool.
The technical solutions of the embodiments of the present application are described in detail below with reference to the accompanying drawings.
Fig. 2 is a schematic flow chart of a trajectory planning method for a mobile tool according to an embodiment of the present application. The method illustrated in fig. 2 may be applied to a trajectory planning cycle of a mobile tool, and mainly includes steps S201 to S204, which are described as follows:
step S201: and when the transverse planning is started to be carried out on the track of the moving tool, carrying out rough longitudinal planning on the track of the moving tool to obtain a rough longitudinal planning result.
In the embodiment of the present application, the mobile tool may be any device with a mobile capability, including a conventional fuel automobile, a new energy automobile, an autonomous vehicle (including a manned vehicle, such as a car, a bus, a minibus, etc.), a cargo-carrying vehicle (such as a general truck, a van, a dump truck, an enclosed truck, a tank truck, a flatbed truck, a container van, a dump truck, a special structure truck), a special vehicle (such as a logistics distribution truck, an automated guided vehicle AGV, a patrol truck, a crane, an excavator, a bulldozer, a forklift, a road roller, a loader, an off-road vehicle, an armored vehicle, a sewage treatment vehicle, a sanitation vehicle, a dust collection vehicle, a ground cleaning vehicle, a water sprinkler, a sweeping robot, a meal delivery robot, a shopping guide robot, a grass cutter, a golf cart, etc.), an entertainment-function vehicle (such as an amusement ride, a playground automatic driving device, a balance car, etc.), a rescue vehicle (such as a fire truck, an ambulance, an emergency rescue vehicle, a push chair, a sweeping robot, a meal delivery robot, a sweeping robot, etc.), a sweeping robot, etc. The autonomous vehicle and the related technical solutions thereof mentioned in the present application are only examples of the technical solutions of the present application, and are not meant to be limited to the autonomous vehicle, and are not meant to be limited to any degree by the technical solutions of the present application.
As mentioned above, a complete trajectory planning cycle includes the processes of transverse planning and longitudinal planning, and in the related art, the transverse planning is performed on the mobile tool, then the longitudinal planning is performed on the mobile tool based on the transverse planning, and the transverse planning and the longitudinal planning are completely decoupled through independent algorithms respectively. The technical scheme of the application is different from the related technology, and is that in the current trajectory planning period of the mobile tool, when the trajectory of the mobile tool starts to be planned transversely, the trajectory of the mobile tool is roughly planned longitudinally, and a rough longitudinal planning result is obtained. The rough longitudinal planning is referred to herein because the parameters of the longitudinal planning, such as the speed, used in the implementation of the longitudinal planning are not real-time or real parameters, but approximate parameters or sampled parameters, as compared to the prior art longitudinal planning. From this perspective, the technical solution of the present application realizes coupling between the longitudinal planning and the transverse planning to some extent, but not in a complete sense, that is, the result of the longitudinal planning is not completely considered in the transverse planning, but the result of the longitudinal planning is still utilized, and only this result is an approximation, so that there is some incomplete coupling between the transverse planning and the longitudinal planning, which may be referred to as weak coupling or loose coupling. As an embodiment of the present application, when the trajectory of the moving tool is planned horizontally, the trajectory of the moving tool is planned longitudinally roughly, and a rough longitudinal planning result can be obtained through steps S2011 to S2013, which are described in detail as follows:
step S2011: and acquiring the current positioning information of the moving tool and the state information of the obstacles on the driving road of the moving tool.
In the embodiment of the application, the current positioning information of the mobile tool can be obtained through a global satellite positioning system, and under the condition that satellite signals are poor, the current positioning information of the mobile tool can also be obtained through motion sensors such as a wheel-type odometer and an inertial measurement unit which are carried by the mobile tool. Certainly, under the condition that the satellite signal is good, the current positioning information of the mobile tool can be obtained through the fusion of the two schemes, and the positioning information of the mobile tool can be calculated by fusing the image obtained by the vision device with the satellite signal and the data obtained by the motion sensor through the vision device carried by the mobile tool. As for the state information of the obstacle on the driving road of the moving tool, on one hand, if the obstacle is a static obstacle, the position data of the obstacle can be obtained through an electronic map; on the other hand, if the obstacle is a dynamic obstacle, such as a running vehicle, the road side unit communicating with the moving tool may be used to measure and position the speed and position of the obstacle on the running road of the moving tool, and transmit the data obtained by measuring the speed and positioning to the moving tool in real time, or a distance measuring device mounted on the moving tool itself, such as a laser radar, may be used to measure the speed and position of the obstacle on the running road of the moving tool, and obtain the state information such as the speed, direction, and position of the obstacle.
Step S2012: and taking the longitudinal planning result of the moving tool at the latest historical moment as a reference value, and predicting the collision position of the moving tool and the barrier according to the current positioning information of the moving tool, the state information of the barrier on the driving road of the moving tool and the reference value.
Here, the historical time closest to the current time is a time closest to the current time of the moving tool in a past period of time, and the vertical planning result of the historical time closest to the current time may be a vertical planning result acquired at a time closest to the current time of the moving tool in a past period of time, for example, data such as a traveling speed of the moving tool obtained from a previous frame of the vertical planning result. Although the data is not the current real-time data of the moving tool, the data can be used as a reference value, and the collision position of the moving tool and the obstacle can be estimated according to the current positioning information of the moving tool, the state information of the obstacle on the driving road of the moving tool and the reference value. Taking the scenario illustrated in fig. 3a, where the moving means is an automobile as an example, when the moving means (illustrated as vehicle 1 in fig. 3 a) executes the same path at different speeds, collision may occur with an obstacle (illustrated as vehicle 2 in fig. 3 a) at different positions. The vehicle may be at a position indicated by (1) in the figure when the speed is high, and at a position indicated by (2) in the figure when the speed is low. Although the transverse planning is carried out independently, the longitudinal motion state of the self-vehicle in a transverse link is not estimated at all. When the longitudinal planning result of the historical moment closest to the moving tool is used as a reference value, whether the collision position of the moving tool and the obstacle occurs at the position marked by (1) or (2) in the graph can be estimated according to the current positioning information of the moving tool, the state information of the obstacle on the driving road of the moving tool and the reference value.
Step S2013: and determining an obstacle avoidance decision according to the estimated collision position of the moving tool and the obstacle.
The obstacle avoidance decision includes surmounting, following, yielding and the like, and can be determined according to the actual scene of the collision position of the estimated moving tool and the obstacle. As for the obstacle avoidance decision, it can be implemented in the transverse planning. Corresponding to the scenario illustrated in fig. 3a, assuming that the collision position of the mobile tool with the obstacle is predicted at the position indicated by (1) in fig. 3a, an obstacle avoidance decision may be as illustrated in fig. 3b, where the obstacle avoidance path of the mobile tool (vehicle 1 illustrated in the figure) is far away from the center of the lane at position 1, and the obstacle (vehicle 2 illustrated in the figure) is actively avoided at the predicted collision position.
The embodiments of step S2011 to step S2013 are more suitable for the estimated collision position between the moving tool and the obstacle, which is only the obstacle itself. In fact, if there are other obstacles, especially static obstacles, in addition to the obstacle itself, at the estimated collision position between the moving tool and the obstacle, that is, the obstacle that is fixed relative to the road, the rough longitudinal planning results obtained in the above steps S2011 to S2013 may be difficult to search for a feasible transverse planning result. As shown in fig. 4, it is assumed that the collision position between the moving vehicle (the vehicle 1 in the example of fig. 4) and the obstacle is estimated to be the position indicated by (1) in fig. 3a through the above steps S2011 to S2013. Since there is just one static obstacle (the small cylinder in the example of fig. 4) at the position identified by (1), i.e., the right side of the position identified by (1) is just occupied by the static obstacle. The boundary determined by the dynamic obstacle (vehicle 2 in the example of fig. 4) and the static obstacle (small cylinder in the example of fig. 4) completely blocks the space that the mobile tool can pass through laterally, and no feasible longitudinal planning solution can be obtained.
For the scenario illustrated in fig. 4, the present application provides another solution, that is, as another embodiment of the present application, when the trajectory of the moving tool is started to be planned laterally, the trajectory of the moving tool is planned roughly longitudinally, and the rough longitudinal planning result obtained may be implemented through steps S '2011 to S'2014, which are described in detail as follows:
step S'2011: the state information of the obstacle on the driving road of the moving tool is obtained.
The specific method for acquiring the state information of the obstacle on the driving road of the mobile tool is the same as the method for acquiring the state information of the obstacle on the driving road of the mobile tool in the foregoing embodiment, and reference may be made to the related description of step S2011 in the foregoing embodiment, which is not repeated here.
Step S'2012: the distance-time ST map is sampled based on the current driving speed of the moving tool, and a plurality of distance-time curves of the moving tool under the ST map are obtained.
In the embodiment of the application, the distance-time ST diagram indicates the relationship between the cumulative distance and time of the moving tool in the S direction in a frelnet (Frenet) coordinate system, when the occupied space of the obstacle is projected onto the ST diagram of the moving tool, the projection area corresponding to the occupied space of the obstacle is obtained, and according to the relationship between the ST curve of the moving tool on the ST diagram and the projection area, the estimated collision position between the moving tool and the obstacle avoidance decision and the like can be obtained. As a basis for understanding ST diagram, in order to better explain the technical solution of the present application, frenet coordinate system (also referred to as "Frenet coordinate system") is first defined hereinReferred to as a road coordinate system). As shown in fig. 5, the starting position of a moving tool such as a vehicle is set as the origin, the coordinate axes are perpendicular to each other, the tangential direction along a road reference line (generally, a road center line is set as the reference line, as shown by a dotted line with an arrow in the figure) is referred to as the transverse direction or the S-axis direction, and the current normal direction of the reference line is referred to as the longitudinal direction or the L-axis direction. As can be seen from the graph, the cumulative distance in the reference line direction, i.e., the S direction, increases with timeSAnd gradually becomes larger. If the vehicle is deviated from the reference line by a distance (positive left and negative right)LIt is shown that the transverse planning of the moving tool is actually to obtain the function L (S), i.e. the transverse positionLRelative to cumulative distanceSIn the vertical direction, the function S (T), i.e., the cumulative distance, is obtainedSAnd timeTThe relationship (2) of (c). The distance-time curve corresponding to the function S (T) is a so-called ST diagram, with T and S on the horizontal axis and the vertical axis, respectively. As shown in fig. 6, the relationship between the curve a and the curve B corresponding to the two functions S (T) of the moving tool in the ST diagram, the projected area of the obstacle 1 in the ST diagram, and the projected area of the obstacle 2 in the ST diagram is shown.
And
representing the upper limit of the plan for calculating the cumulative distance S and time T considered. From the predicted trajectory of the dynamic obstacle, the location and duration of the collision of the dynamic obstacle with the moving implement can be predicted. The projected area of the obstacle 1 in the ST diagram represents the time and position of the obstacle 1 occupying the ST diagram (also referred to as the ST boundary), and the projected area of the obstacle 2 in the ST diagram represents the time and position of the obstacle 2 occupying the ST diagram, and the size thereof is related to the size of the obstacle occupying S in the event of a collision. In longitudinal planning, a curve needs to be generated from an origin to a planning upper limit
And with
The boundary line does not intersect the ST boundaries of all obstacles while satisfying monotonicity of S and T. Curves a and B in fig. 6 represent two possible planning results, where curve a represents an obstacle avoidance decision to take "override" for both obstacle 1 and obstacle 2 (represented in the figure by the fact that the planned S for the moving tool at the same time T is greater than the ST boundaries of obstacle 1 and obstacle 2), and curve B represents an obstacle avoidance decision to take "override" for obstacle 2 (represented in the figure by the fact that the planned S for the moving tool at the same time T is greater than the ST boundary of obstacle 1), and an obstacle avoidance decision to take "follow" or "yield" for obstacle 1 (represented in the figure by the fact that the planned S for the moving tool at the same time T is less than the ST boundary of obstacle 1).
After the details of the Frenet coordinate system and the ST diagram are described, step S'2012 is described below. Specifically, as an embodiment of the present application, the distance-time ST map is sampled based on the current driving speed of the moving tool, and the obtained several distance-time curves of the moving tool under the ST map may be: selecting at least one specific point in the ST graph as a separation point; and calculating a plurality of distance-time curves of the moving tool under the ST diagram according to the current driving speed of the moving tool and at least one separation point, wherein the separation point is within the accumulated distance and the time planning upper limit of the moving tool in the ST diagram. The separation points selected within the cumulative distance and upper time plan limit of the moving tool in the ST diagram are different, and reaching the separation points with the current speed and different acceleration of the moving tool according to the kinematic equation results in different distance-time curves, or reaching different separation points with the current speed and the same acceleration of the moving tool results in different distance-time curves, which are all possible as a result of the longitudinal planning of the moving tool, which correspond to a subset of the entire longitudinal planning solution space with respect to the entire longitudinal planning solution space of the moving tool, which is what the meaning of "sampling" in several distance-time curves of the moving tool under the ST diagram is obtained by sampling the distance-time ST diagram based on the current travel speed of the moving tool. For the separation points of the above embodiments, in practical application, according to the shiftThe current speed of the moving tool is reasonably selected, for example, when the current speed of the moving tool is larger, the separation point, namely the distance planning upper limit can be properly reduced
Or
Far away. Without loss of generality, as an embodiment of the present application, the midpoint of the ST diagram and the midpoint of the cumulative distance and/or the upper limit of the time plan may be a separation point, if the coordinates of the separation point may be (a), (b), (c), and (d)
，s）、（t，
) And (a) and (b)
，
) And the like, wherein,
，
. Further, the coordinates of the separation points are (
，
) For example, when the moving tool starts from the origin O of the ST diagram at the current velocity V, it accelerates (and decelerates) to the separation point at the maximum acceleration and the minimum deceleration, respectively (and)
，
) From the separation point (
，
In the same way, (2) equal acceleration reaches the upper limit of the plan
Or
) A total of four distance-time curves can be obtained, as shown in FIG. 7 (in the figure)
=
，
=
) Shown by curve a, curve B, curve C and curve D. From FIG. 7, it can be seen that the moving tool is from the originoThe upper limit of the cumulative distance S and time T is reached, and the driving states thereof include four types, i.e., continuous acceleration (corresponding to curve a), acceleration before deceleration (corresponding to curve B), acceleration after deceleration (corresponding to curve C), and continuous deceleration (corresponding to curve D). In consideration of monotonicity in the S direction (i.e., the moving tool is not allowed to move backward), when the speed of the moving tool is reduced to 0 (in the ST diagram, the curve is parallel to the horizontal axis or the T axis), S needs to be kept unchanged and cannot be reduced continuously in the process of sampling the distance-time ST diagram; similarly, the moving tool cannot be accelerated indefinitely, as the distance-time ST map is sampledIn the process, when the speed of the moving tool is too high, the speed needs to be limited within the maximum speed limit allowed by the moving tool and cannot be increased continuously.
Step S'2013: and predicting the collision position of the moving tool and the obstacle according to the state information of the obstacle on the driving road of the moving tool and a plurality of distance-time curves of the moving tool under the ST diagram.
In the embodiment of the application, the projected area of the obstacle in the ST diagram can be compared with the plurality of distance-time curves of the moving tool under the ST diagram according to the state information (including the predicted position, speed, orientation, and track obtained based on the predicted position, speed, orientation, and the like) of the obstacle on the driving road of the moving tool, and the collision position of the moving tool and the obstacle can be estimated according to the overlapping condition of the projected area of the obstacle in the ST diagram and the plurality of distance-time curves of the moving tool under the ST diagram. As shown in FIG. 8, assume that the projected area of the obstacle m in the ST diagram overlaps the distance-time curve B of the moving tool under the ST diagramPPoint on, thenPCoordinates of points on the S-axis in the ST diagramSmI.e. the estimated collision position of the moving tool with the obstacle.
Step S'2014: and determining an obstacle avoidance decision according to the estimated collision position of the moving tool and the obstacle.
In this embodiment, the determination of the obstacle avoidance decision according to the estimated collision position of the mobile tool and the obstacle is the same as the method for determining the obstacle avoidance decision according to the estimated collision position of the mobile tool and the obstacle in the foregoing embodiment, and reference may be made to the relevant description of step S2013 in the foregoing embodiment, which is not repeated here.
Step S202: and performing transverse planning on the track of the moving tool based on the rough longitudinal planning result.
As described above, the trajectory of the moving tool is planned laterally, and a path along the S direction of the Frenet coordinate system is actually planned. In the embodiment of the present application, after one or more rough longitudinal planning results are obtained in step S201, including the collision position of the mobile tool with the obstacle, the boundary constraint condition, the physical constraint condition, the dynamic constraint condition, and the like near the collision position may be modified based on the rough longitudinal planning results, so as to plan the trajectory of the mobile tool in the transverse direction of the current trajectory planning cycle. Specifically, as an embodiment of the present application, based on the rough longitudinal planning result, the transverse planning of the current trajectory planning cycle for the trajectory of the mobile tool may be: determining an obstacle reference position which needs to pass when the moving tool bypasses the obstacle according to the estimated collision position of the moving tool and the obstacle; estimating the end position of the moving tool bypassing the obstacle according to the position relation between the moving tool and the obstacle; and determining a path of the moving tool to bypass the obstacle based on the current positioning information of the moving tool, the obstacle reference position and the end point position of the moving tool to bypass the obstacle. In the above embodiment, the obstacle reference position that the mobile tool needs to pass through when passing around the obstacle according to the estimated collision position of the mobile tool with the obstacle may be: acquiring the position of the barrier close to the boundary of one side of the moving tool according to the collision position of the moving tool and the barrier; and calculating the position separated from the position of the boundary by a preset offset distance according to the obstacle avoidance decision to obtain the obstacle reference position which needs to pass when the moving tool bypasses the obstacle, wherein the preset offset distance is the preset separation distance from the obstacle in the direction vertical to the reference line. As for determining the path of the moving tool around the obstacle based on the current positioning information of the moving tool, the reference position of the obstacle, and the end position of the moving tool around the obstacle, the path of the positioning information of the moving tool, the reference position of the obstacle, and the end position may be used as a track point of the path of the moving tool around the obstacle; calculating the driving angular speed of the mobile tool from the current positioning information to the end position along the direction of a reference line (usually a road center line); and constructing a path passing through the track points at the driving angular speed as a path for the moving tool to bypass the obstacle. The obstacle avoidance decision of the above embodiment includes override, follow, and yield (left yield or right yield).
As another embodiment of the present application, based on the coarse longitudinal planning result, the transverse planning of the current trajectory planning cycle for the trajectory of the moving tool may further be: determining a feasible track area of the moving tool at each future moment according to the position information of the obstacle at each future moment, the collision position of the moving tool and the obstacle and obstacle avoidance decisions; and obtaining a target planning track of the moving tool in a time period formed by a plurality of future moments according to the feasible track area. In the above embodiment, according to the position information of the obstacle at each future time, the collision position between the mobile tool and the obstacle, and the obstacle avoidance decision, determining the feasible trajectory area of the mobile tool at each future time may be: performing secondary planning on an initial planned driving area corresponding to the collision position according to the position information and obstacle avoidance decisions of the dynamic obstacle at a plurality of future moments to obtain a re-planned driving area of the mobile tool at each future moment; and acquiring the estimated longitudinal displacement of the mobile tool at each future moment, and determining a feasible track area of the mobile tool at each future moment according to the re-planned driving area and the estimated longitudinal displacement, wherein the initially planned driving area comprises at least one transverse width range and is expressed as the length along the L-axis direction in a Frenet coordinate system. As for the secondary planning of the initial planned driving area corresponding to the collision position according to the position information and obstacle avoidance decisions of the dynamic obstacle at a plurality of future moments, a re-planned driving area of the mobile tool at each future moment is obtained, one technical scheme may be: taking each dynamic obstacle as a target obstacle one by one, taking a plurality of future moments as target moments one by one, and transversely extending the area of the position occupied by the obstacle to the opposite direction of the bypassing direction indicated by the obstacle avoidance decision of the target obstacle to the boundary of the corresponding transverse width interval according to the position information of the obstacle at the target moment to obtain an obstacle area; and removing the obstacle area from the initial planned driving area to obtain a re-planned driving area of the moving tool at the target moment.
The following describes, in conjunction with some scenarios of the foregoing embodiments, the lateral planning of the trajectory of the moving tool for the current trajectory planning cycle. Assuming that the rough longitudinal planning results of the example in fig. 7 are feasible solutions, for the scenario of the example in fig. 4, the obtained rough longitudinal planning result of the curve a may be used to perform the transverse planning of the trajectory of the mobile tool for the current trajectory planning cycle. As shown in fig. 9, according to the rough longitudinal planning result of curve B, the moving tool (vehicle 1 in the example of fig. 9) first accelerates to pass the static obstacle (small pillar in the example of fig. 9) so that it no longer interferes with the dynamic obstacle (vehicle 2 in the example of fig. 9) at the position indicated by (1) in the example of fig. 4; after the static obstacle is surpassed, the moving tool starts to decelerate, and finally interferes with the dynamic obstacle at the position marked by (2) in the example of fig. 4 to give way or follow the dynamic obstacle, so that a path which avoids the static obstacle and the moving tool at the same time is transversely planned for the moving tool.
It should be noted that, when the horizontal planning based on the plurality of rough longitudinal planning results is successful, for the same scene, the mobile tool may show different obstacle avoidance decisions for the dynamic obstacle for the same scene, so that different paths are obtained through the horizontal planning. As shown in fig. 10a, according to a curve B obtained in fig. 7, the moving tool (fig. 10a is identified by a vehicle 1), the obstacle avoidance decision of acceleration and passing is firstly performed on the rider at the position identified by (1) in the example of fig. 10a, then the vehicle-meeting action is performed on the dynamic obstacle (fig. 10a is identified by a vehicle 2) at the position identified by (2), and the obstacle avoidance decision of following or yielding is performed, so that an obstacle avoidance path is transversely planned for the moving tool. Of course, for the scenario illustrated in fig. 10a, the scheme illustrated in fig. 10b may also be adopted, specifically, the moving tool (the vehicle 1 is illustrated in fig. 10 b) may first decelerate to wait for the dynamic obstacle (the vehicle 2 is illustrated in fig. 10 b) according to the curve C obtained in fig. 7, and perform the obstacle avoidance decision for the dynamic obstacle at the position indicated by (2) in fig. 10b, and then surmount the rider at the position indicated by (1) in fig. 10b, so as to plan an obstacle avoidance path for the moving tool laterally. The above is merely an example of several different lateral plans and is not meant to be a limitation of the present application. In fact, in different application occasions, the mobile tool can be transversely planned by adopting a more appropriate rough longitudinal planning result through different performances, for example, in a scene that the mobile tool is expected to aggressively drive, an obstacle avoidance decision of firstly accelerating and then decelerating and then letting a line (or following) is carried out, and the effect of the obstacle avoidance decision is possibly better than that of firstly decelerating and letting a line (or following) and then accelerating and surpassing.
Step S203: and judging whether the transverse planning of the track of the moving tool is successful.
Due to unreasonable constraints on boundary constraints, physical constraints, dynamic constraints and the like near the collision position of the mobile tool with the obstacle, it may be possible that the lateral planning of the trajectory of the mobile tool for the current trajectory planning cycle based on the rough longitudinal planning result is not successful, which appears as blocking the lateral passable space of the mobile tool at the collision position of the mobile tool with the obstacle, as shown in fig. 4. Therefore, after the transverse planning of the mobile tool in the current trajectory planning cycle is obtained based on the rough longitudinal planning result, it is necessary to determine whether the transverse planning of the trajectory of the mobile tool is successful, and select a successful transverse planning.
Step S204: and if the transverse planning of the track of the moving tool is successful, after the successful transverse planning is completed, longitudinally planning the track of the moving tool by taking the rough longitudinal planning result as an optimization target.
After the successful transverse planning is completed, when the longitudinal planning is performed on the trajectory of the mobile tool, the longitudinal planning needs to be made as close as possible to the rough longitudinal planning result obtained in the foregoing embodiment, so that the path obtained by the transverse planning performed on the trajectory of the mobile tool can be smoothly executed. Therefore, if the transverse planning of the moving tool track is successful, after the successful transverse planning is completed, the track of the moving tool is longitudinally planned by taking the rough longitudinal planning result as an optimization target. Taking the curve a illustrated in fig. 7 as an example of the rough longitudinal planning result obtained by the present application, that is, performing an obstacle avoidance decision for continuously accelerating and surmounting the obstacle car and the obstacle cycle, and after completing the successful transverse planning, taking the rough longitudinal planning result as an optimization target to perform longitudinal planning on the track of the mobile tool, a specific implementation manner may be: the S value of the curve a corresponding to the time T in the ST diagram is sampled, and the square of the difference between the S value and the S value of the curve to be planned is made as small as possible, as shown in fig. 11, the thinner curve is a distance-time curve obtained by performing longitudinal planning on the trajectory of the mobile tool in the current trajectory planning period with the curve a illustrated in fig. 7 as an optimization target.
As can be known from the trajectory planning method of the mobile tool illustrated in fig. 2, compared with the related art that the response to the dynamic obstacle is not sufficiently active and intelligent due to the fact that the longitudinal planning and the transverse planning are completely decoupled when the trajectory of the mobile tool is planned, the technical scheme of the present application, on one hand, roughly performs longitudinal planning on the trajectory of the mobile tool when the transverse planning is performed on the trajectory of the mobile tool, and then performs transverse planning on the trajectory of the mobile tool according to the roughly-obtained longitudinal planning result, in other words, the present application obtains the transverse planning result capable of actively avoiding the dynamic obstacle through weak coupling of the longitudinal planning and the transverse planning, thereby improving the intelligent degree of the trajectory planning of the mobile tool; on the other hand, after the transverse planning is finished, the track of the mobile tool is planned longitudinally by taking the rough longitudinal planning result as an optimization target, so that the result of the longitudinal planning approaches the optimization target as much as possible, and the path planned in the transverse planning link can be executed really and smoothly. In conclusion, the technical scheme of the application fundamentally enhances the intelligence degree of the trajectory planning, and improves the trajectory planning efficiency and the operation safety of the mobile tool.
Corresponding to the embodiment of the application function implementation method, the application also provides a trajectory planning device of the moving tool, the electronic equipment and a corresponding embodiment.
Fig. 12 is a schematic structural diagram of a trajectory planning apparatus for a mobile tool according to an embodiment of the present application. For convenience of explanation, only portions related to the embodiments of the present application are shown. The trajectory planning apparatus for a mobile tool illustrated in fig. 12 may be applied to one trajectory planning cycle of the mobile tool, and mainly includes a first longitudinal planning module 1201, a determining module 1203, and a second longitudinal planning module 1204, where:
a first longitudinal planning module 1201, configured to perform rough longitudinal planning on a trajectory of a moving tool when the trajectory of the moving tool starts to be planned transversely, so as to obtain a rough longitudinal planning result;
a transverse planning module 1202, configured to perform transverse planning on the trajectory of the mobile tool based on the rough longitudinal planning result;
a judging module 1203, configured to judge whether the transverse planning of the trajectory of the moving tool is successful;
a second longitudinal planning module 1204, configured to, if the transverse planning of the trajectory of the mobile tool is successful, perform longitudinal planning on the trajectory of the mobile tool with the coarse longitudinal planning result as an optimization target after the successful transverse planning is completed.
With regard to the apparatus in the above embodiment, the specific manner in which each module performs the operation has been described in detail in the embodiment related to the method, and will not be elaborated herein.
As can be seen from the trajectory planning apparatus of the mobile tool illustrated in fig. 12, compared with the related art that the response to the dynamic obstacle is not sufficiently active and intelligent due to the fact that the longitudinal planning and the transverse planning are completely decoupled when the trajectory of the mobile tool is planned, the technical solution of the present application, on one hand, roughly performs the longitudinal planning on the trajectory of the mobile tool when the transverse planning starts, and then performs the transverse planning on the trajectory of the mobile tool according to the roughly-obtained longitudinal planning result, in other words, the present application obtains the transverse planning result capable of actively avoiding the dynamic obstacle through the weak coupling of the longitudinal planning and the transverse planning, thereby improving the intelligent degree of the trajectory planning of the mobile tool; on the other hand, after the transverse planning is finished, the track of the mobile tool is planned longitudinally by taking the rough longitudinal planning result as an optimization target, so that the result of the longitudinal planning approaches the optimization target as much as possible, and the path planned in the transverse planning link can be executed really and smoothly. In conclusion, the technical scheme of the application fundamentally enhances the intelligence degree of the trajectory planning, and improves the trajectory planning efficiency and the operation safety of the mobile tool.
Fig. 13 is a schematic structural diagram of an electronic device shown in an embodiment of the present application.
Referring to fig. 13, an electronic device 1300 includes a memory 1310 and a processor 1320.
Processor 1320 may be a Central Processing Unit (CPU), other general purpose Processor, a Digital Signal Processor (DSP), an Application Specific Integrated Circuit (ASIC), a Field Programmable Gate Array (FPGA) or other Programmable logic device, discrete Gate or transistor logic, discrete hardware components, etc. A general purpose processor may be a microprocessor or the processor may be any conventional processor or the like.
The memory 1310 may include various types of storage units, such as system memory, read Only Memory (ROM), and permanent storage. The ROM may store, among other things, static data or instructions for the processor 1320 or other modules of the computer. The persistent storage device may be a read-write storage device. The persistent storage may be a non-volatile storage device that does not lose stored instructions and data even after the computer is powered down. In some embodiments, the persistent storage device employs a mass storage device (e.g., magnetic or optical disk, flash memory) as the persistent storage device. In other embodiments, the permanent storage may be a removable storage device (e.g., floppy disk, optical drive). The system memory may be a read-write memory device or a volatile read-write memory device, such as a dynamic random access memory. The system memory may store instructions and data that some or all of the processors require at run-time. Further, memory 1310 may comprise any combination of computer-readable storage media, including various types of semiconductor memory chips (e.g., DRAM, SRAM, SDRAM, flash, programmable read only memory), magnetic and/or optical disks, may also be employed. In some embodiments, memory 1310 may include a removable storage device that is readable and/or writable, such as a Compact Disc (CD), a read-only digital versatile disc (e.g., DVD-ROM, dual layer DVD-ROM), a read-only Blu-ray disc, an ultra-density optical disc, a flash memory card (e.g., SD card, min SD card, micro-SD card, etc.), a magnetic floppy disc, or the like. Computer-readable storage media do not contain carrier waves or transitory electronic signals transmitted by wireless or wired means.
The memory 1310 has stored thereon executable code that, when processed by the processor 1320, may cause the processor 1320 to perform some or all of the methods described above.
Furthermore, the method according to the present application may also be implemented as a computer program or computer program product comprising computer program code instructions for performing some or all of the steps of the above-described method of the present application. Alternatively, the present application may also be embodied as a computer-readable storage medium (or non-transitory machine-readable storage medium or machine-readable storage medium) having executable code (or a computer program or computer instruction code) stored thereon, which, when executed by a processor of an electronic device (or server, etc.), causes the processor to perform part or all of the various steps of the above-described method according to the present application.
The application also provides a moving tool, which comprises the electronic equipment illustrated in fig. 13, and the moving tool can be all equipment with moving capability, including a traditional fuel automobile, a new energy automobile, an automatic driving vehicle (including a passenger car, a small bus, a truck, a sanitation car, a logistics car, a ground washing car, a dust collection car, an AGV, a motorcycle and the like with automatic driving or intelligent driving), a robot (for example, a sweeping robot, a meal delivery robot and the like), a wheelchair and the like.
The foregoing description of the embodiments of the present application has been presented for purposes of illustration and description and is not intended to be exhaustive or limited to the embodiments disclosed. Many modifications and variations will be apparent to those of ordinary skill in the art without departing from the scope and spirit of the described embodiments. The terminology used herein is chosen in order to best explain the principles of the embodiments, the practical application, or improvements made to the technology in the marketplace, or to enable others of ordinary skill in the art to understand the embodiments disclosed herein.
Claims (13)
1. A method for trajectory planning of a mobile tool, applied to a trajectory planning cycle of the mobile tool, the trajectory planning cycle comprising a process of lateral planning and longitudinal planning, the method comprising:
when the track of the moving tool is planned transversely, roughly planning the track of the moving tool longitudinally to obtain a rough longitudinal planning result, wherein parameters of the longitudinal planning used in the rough longitudinal planning are approximate parameters or parameters obtained by sampling;
modifying constraint conditions near the collision position of the moving tool and the obstacle based on the rough longitudinal planning result to perform transverse planning of the current trajectory planning cycle on the trajectory of the moving tool;
judging whether the transverse planning is successful or not;
and if the transverse planning is successful, after the transverse planning is finished, performing longitudinal planning on the track of the mobile tool by taking the rough longitudinal planning result as an optimization target.
2. The method for planning a trajectory of a moving tool according to claim 1, wherein performing a coarse longitudinal planning on the trajectory of the moving tool to obtain a coarse longitudinal planning result comprises:
acquiring current positioning information of the moving tool and state information of obstacles on a driving road of the moving tool;
taking a longitudinal planning result of the moving tool at a historical moment closest to the current moment as a reference value, and estimating the collision position of the moving tool and the obstacle according to the current positioning information of the moving tool, the state information of the obstacle on the road where the moving tool runs and the reference value;
and determining an obstacle avoidance decision according to the collision position.
3. The method for planning a trajectory of a moving tool according to claim 1, wherein the performing a rough longitudinal planning on the trajectory of the moving tool to obtain a rough longitudinal planning result comprises:
acquiring state information of obstacles on a driving road of the moving tool;
sampling a distance-time ST diagram based on the current driving speed of the moving tool to obtain a plurality of distance-time curves of the moving tool under the ST diagram;
estimating the collision position of the moving tool and the obstacle according to the state information of the obstacle on the driving road of the moving tool and a plurality of distance-time curves of the moving tool under the ST diagram;
and determining an obstacle avoidance decision according to the collision position.
4. The method according to claim 3, wherein the step of sampling a distance-time (ST) map based on the current driving speed of the moving tool to obtain a plurality of distance-time curves of the moving tool under the ST map comprises:
selecting at least one specific point in the ST map as a separation point, wherein the separation point is within the accumulated distance and the time planning upper limit of the moving tool in the ST map;
and calculating a plurality of position distance-time curves of the moving tool under the ST diagram according to the current driving speed of the moving tool and the at least one separation point.
5. The method according to claim 4, wherein the separation point is a midpoint of the ST diagram and the cumulative distance and/or time planning upper limit, and the calculated distance-time curves of the moving tool under the ST diagram include distance-time curves in which the driving state of the moving tool is continuous acceleration, acceleration-first deceleration, deceleration-first acceleration-second acceleration, and continuous deceleration.
6. The method for planning the trajectory of the mobile tool according to claim 2 or 3, wherein the modifying the constraint condition around the collision position of the mobile tool with the obstacle based on the rough longitudinal planning result to plan the trajectory of the mobile tool in the transverse direction of the current trajectory planning cycle comprises:
determining an obstacle reference position which needs to pass when the moving tool bypasses the obstacle according to the collision position of the moving tool and the obstacle;
estimating the end position of the moving tool bypassing the obstacle according to the position relation between the moving tool and the obstacle;
determining a path for the mobile tool to detour around the obstacle based on the current positioning information of the mobile tool, the obstacle reference position, and the end point position.
7. The method for planning the trajectory of the mobile tool according to claim 6, wherein the determining the reference position of the obstacle that the mobile tool needs to pass through when bypassing the obstacle according to the collision position of the mobile tool with the obstacle comprises:
acquiring the position of the barrier close to the boundary on one side of the moving tool according to the collision position of the moving tool and the barrier;
and calculating the position which is separated from the position of the boundary by a preset offset distance according to the obstacle avoidance decision to obtain the obstacle reference position which needs to pass when the moving tool bypasses the obstacle, wherein the preset offset distance is the preset separation distance from the obstacle in the direction vertical to the reference line.
8. The method according to claim 2 or 3, wherein the modifying the constraint condition near the collision position of the moving tool with the obstacle based on the rough longitudinal planning result performs the transverse planning of the current trajectory planning cycle on the trajectory of the moving tool, and comprises:
determining a feasible track area of the moving tool at each future moment according to the position information of the obstacle at each future moment, the collision position and the obstacle avoidance decision;
and obtaining a target planning track of the mobile tool in a time period formed by a plurality of future moments according to the feasible track area.
9. The method for planning the trajectory of the mobile tool according to claim 8, wherein the determining a feasible trajectory area of the mobile tool at each future time according to the position information of the obstacle at each future time, the collision position, and the obstacle avoidance decision includes:
performing secondary planning on an initial planned driving area corresponding to the collision position according to the position information and obstacle avoidance decisions of the dynamic obstacle at a plurality of future moments to obtain a re-planned driving area of the mobile tool at each future moment;
and acquiring the estimated longitudinal displacement of the mobile tool at each future moment, and determining a feasible track area of the mobile tool at each future moment according to the re-planned driving area and the estimated longitudinal displacement.
10. An apparatus for trajectory planning of a moving tool, applied to a trajectory planning cycle of the moving tool, the trajectory planning cycle comprising a process of transverse planning and longitudinal planning, the apparatus comprising:
the first longitudinal planning module is used for performing rough longitudinal planning on the track of the moving tool when the track of the moving tool is subjected to transverse planning to obtain a rough longitudinal planning result, and parameters of the longitudinal planning used in the implementation of the rough longitudinal planning are approximate parameters or parameters obtained through sampling;
the transverse planning module is used for modifying constraint conditions near the collision position of the moving tool and an obstacle to carry out transverse planning of the current trajectory planning period on the trajectory of the moving tool based on the rough longitudinal planning result;
the judging module is used for judging whether the transverse planning is successful or not;
and the second longitudinal planning module is used for performing longitudinal planning on the track of the mobile tool by taking the rough longitudinal planning result as an optimization target after the successful transverse planning is finished if the transverse planning is successful.
11. An electronic device, comprising:
a processor; and
a memory having executable code stored thereon, which when executed by the processor, causes the processor to perform the method of any one of claims 1 to 9.
12. A computer readable storage medium having stored thereon executable code which, when executed by a processor of an electronic device, causes the processor to perform the method of any of claims 1 to 9.
13. A mobile tool comprising the electronic device of claim 11.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202211298959.9A CN115355916B (en) | 2022-10-24 | 2022-10-24 | Trajectory planning method, apparatus and computer-readable storage medium for moving tool |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202211298959.9A CN115355916B (en) | 2022-10-24 | 2022-10-24 | Trajectory planning method, apparatus and computer-readable storage medium for moving tool |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN115355916A CN115355916A (en) | 2022-11-18 |
| CN115355916B true CN115355916B (en) | 2023-03-03 |
Family
ID=84007748
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202211298959.9A Active CN115355916B (en) | 2022-10-24 | 2022-10-24 | Trajectory planning method, apparatus and computer-readable storage medium for moving tool |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN115355916B (en) |
Families Citing this family (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117516550B (en) * | 2024-01-04 | 2024-03-15 | 三一重型装备有限公司 | Path planning method and system, and readable storage medium |
Citations (9)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| WO2021135728A1 (en) * | 2019-12-30 | 2021-07-08 | 郑州宇通客车股份有限公司 | Determination method and device for collision prediction of autonomous vehicle |
| CN113799797A (en) * | 2021-07-27 | 2021-12-17 | 北京三快在线科技有限公司 | Trajectory planning method and device, storage medium and electronic equipment |
| CN114489044A (en) * | 2019-12-31 | 2022-05-13 | 华为技术有限公司 | Trajectory planning method and device |
| CN114620071A (en) * | 2022-02-16 | 2022-06-14 | 杭州飞步科技有限公司 | Detour trajectory planning method, device, equipment and storage medium |
| CN114620070A (en) * | 2022-02-16 | 2022-06-14 | 杭州飞步科技有限公司 | Driving track planning method, device, equipment and storage medium |
| CN114715192A (en) * | 2022-04-15 | 2022-07-08 | 重庆大学 | Decoupled real-time trajectory planning method, device and system for automatic driving vehicle |
| CN114715193A (en) * | 2022-04-15 | 2022-07-08 | 重庆大学 | Real-time trajectory planning method and system |
| CN114993335A (en) * | 2022-06-30 | 2022-09-02 | 重庆长安汽车股份有限公司 | Automatic driving path planning method and device, electronic equipment and storage medium |
| WO2022193584A1 (en) * | 2021-03-15 | 2022-09-22 | 西安交通大学 | Multi-scenario-oriented autonomous driving planning method and system |
Family Cites Families (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US11378961B2 (en) * | 2018-04-17 | 2022-07-05 | Baidu Usa Llc | Method for generating prediction trajectories of obstacles for autonomous driving vehicles |
-
2022
- 2022-10-24 CN CN202211298959.9A patent/CN115355916B/en active Active
Patent Citations (9)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| WO2021135728A1 (en) * | 2019-12-30 | 2021-07-08 | 郑州宇通客车股份有限公司 | Determination method and device for collision prediction of autonomous vehicle |
| CN114489044A (en) * | 2019-12-31 | 2022-05-13 | 华为技术有限公司 | Trajectory planning method and device |
| WO2022193584A1 (en) * | 2021-03-15 | 2022-09-22 | 西安交通大学 | Multi-scenario-oriented autonomous driving planning method and system |
| CN113799797A (en) * | 2021-07-27 | 2021-12-17 | 北京三快在线科技有限公司 | Trajectory planning method and device, storage medium and electronic equipment |
| CN114620071A (en) * | 2022-02-16 | 2022-06-14 | 杭州飞步科技有限公司 | Detour trajectory planning method, device, equipment and storage medium |
| CN114620070A (en) * | 2022-02-16 | 2022-06-14 | 杭州飞步科技有限公司 | Driving track planning method, device, equipment and storage medium |
| CN114715192A (en) * | 2022-04-15 | 2022-07-08 | 重庆大学 | Decoupled real-time trajectory planning method, device and system for automatic driving vehicle |
| CN114715193A (en) * | 2022-04-15 | 2022-07-08 | 重庆大学 | Real-time trajectory planning method and system |
| CN114993335A (en) * | 2022-06-30 | 2022-09-02 | 重庆长安汽车股份有限公司 | Automatic driving path planning method and device, electronic equipment and storage medium |
Also Published As
| Publication number | Publication date |
|---|---|
| CN115355916A (en) | 2022-11-18 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| US11126187B2 (en) | Systems and methods for controlling the operation of a vehicle | |
| CN111688663B (en) | Motor vehicle and method for controlling automatic driving operation thereof | |
| US10802492B2 (en) | Vehicle path identification | |
| JP4466717B2 (en) | Traveling locus generation method and traveling locus generation device | |
| JP4466718B2 (en) | Traveling locus generation method and traveling locus generation device | |
| CN113916246A (en) | Unmanned obstacle avoidance path planning method and system | |
| CN110473416B (en) | Vehicle control device | |
| CN110494339A (en) | Vehicle control system, control method for vehicle and vehicle control program | |
| US11584248B2 (en) | Method of parking an autonomous driving vehicle for autonomous charging | |
| CN110103962A (en) | Controller of vehicle, control method for vehicle and storage medium | |
| US10377383B2 (en) | Vehicle lane change | |
| US20220227390A1 (en) | Proactive lane change for autonomous vehicles | |
| CN115355916B (en) | Trajectory planning method, apparatus and computer-readable storage medium for moving tool | |
| US20220063604A1 (en) | Vehicle travel control device | |
| US11807234B2 (en) | Automated driving trajectory generating device and automated driving device | |
| JP2020147139A (en) | Vehicle control device, vehicle control method, and program | |
| US20220009493A1 (en) | Vehicle travel control device | |
| CN113044058A (en) | Three-point turn for automatic driving vehicle | |
| CN115840454A (en) | Multi-vehicle track collaborative planning method and device for unstructured road conflict area | |
| US20230012853A1 (en) | Proactive Risk Mitigation | |
| CN111688693B (en) | Vehicle control device, vehicle control method, and storage medium | |
| CN114545950A (en) | Control method of unmanned intelligent vehicle | |
| EP3748604B1 (en) | Vehicle travelling control apparatus, vehicle travelling control method and computer program product | |
| JP2022142863A (en) | Movable body control device, movable body control method and program | |
| JP7381608B2 (en) | How to operate an autonomous vehicle |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |