CN117705123B - Track planning method, device, equipment and storage medium - Google Patents
Track planning method, device, equipment and storage medium Download PDFInfo
- Publication number
- CN117705123B CN117705123B CN202410144447.XA CN202410144447A CN117705123B CN 117705123 B CN117705123 B CN 117705123B CN 202410144447 A CN202410144447 A CN 202410144447A CN 117705123 B CN117705123 B CN 117705123B
- Authority
- CN
- China
- Prior art keywords
- track
- obstacle
- point
- self
- mobile device
- 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
- 238000000034 method Methods 0.000 title claims abstract description 42
- 239000013598 vector Substances 0.000 claims description 20
- 230000006870 function Effects 0.000 claims description 17
- 230000000007 visual effect Effects 0.000 claims description 12
- 230000004888 barrier function Effects 0.000 claims description 8
- 238000004590 computer program Methods 0.000 claims description 8
- 238000005457 optimization Methods 0.000 claims description 5
- 238000009966 trimming Methods 0.000 claims description 4
- 238000010276 construction Methods 0.000 claims description 3
- 238000005516 engineering process Methods 0.000 description 10
- 230000008569 process Effects 0.000 description 6
- 238000004891 communication Methods 0.000 description 3
- 238000010586 diagram Methods 0.000 description 3
- 230000000694 effects Effects 0.000 description 3
- 230000007613 environmental effect Effects 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 238000013473 artificial intelligence Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 238000001914 filtration Methods 0.000 description 1
- 238000009499 grossing Methods 0.000 description 1
- 238000010845 search algorithm Methods 0.000 description 1
- 230000003068 static effect Effects 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 238000012546 transfer Methods 0.000 description 1
- 230000009466 transformation Effects 0.000 description 1
- 230000001131 transforming effect Effects 0.000 description 1
Landscapes
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
The invention relates to the technical field of track planning, and provides a track planning method, a track planning device, track planning equipment and a storage medium, wherein the track planning method comprises the steps of acquiring a global track of walking of mobile equipment; acquiring a local map walking from the mobile device according to the global track row, and updating the areas in the field of view and outside the field of view of the local map according to point clouds acquired by the mobile device in different periods of time; when the intersection of the track to be tracked of the updated local map and the obstacle is predicted, reconstructing the current track to be tracked, and taking the track to be tracked which is not intersected with the obstacle after reconstruction as the local track. By taking the track to be tracked which is not intersected with the obstacle after reconstruction as a local track for avoiding the obstacle, the problem that the self-moving equipment cannot avoid the unknown obstacle in time and collide with the unknown obstacle due to the fact that the track route which is not timely planned for avoiding the unknown obstacle when the unknown obstacle is encountered is solved.
Description
Technical Field
The present disclosure relates to the field of track planning technologies, and in particular, to a track planning method, apparatus, device, and storage medium.
Background
An Unmanned Ground Vehicle (UGV) is self-moving equipment capable of running autonomously or semi-autonomously on the ground, and is widely applied to the fields of military, industry, agriculture, rescue and the like; one of the core technologies of the self-mobile device is track planning, namely, a feasible track from a starting point to an end point is generated according to environment information and a target position, so that the self-mobile device can avoid an obstacle and move according to a desired speed and direction, and the difficulty of track planning is how to realize safe, effective and real-time track generation in a complex, dynamic and uncertain environment.
In the related technical means, the self-mobile equipment adopts the technical means in the fields of sensing technology (machine vision, laser radar), control technology (path planning, obstacle avoidance), communication technology (wireless communication, satellite communication), energy technology (battery, solar energy), mechanical design technology (chassis, tire, mechanical arm), artificial intelligence, cloud computing, big data and the like, and provides support and guarantee for UGVs to realize autonomous driving, task execution and remote control.
Aiming at the technical means, in the process that the self-mobile device senses environmental information through a limited field of view (mechanical vision) in a sensing technology, due to the limitation of various factors such as the cost, the structure and the like of the self-mobile device, complete barrier information on a map cannot be obtained generally, so that when the self-mobile device plans a path track through a control technology, a track route which avoids the unknown barrier cannot be planned in time when the self-mobile device encounters the unknown barrier, and further the self-mobile device cannot avoid the unknown barrier in time, and the self-mobile device collides with the unknown barrier.
Disclosure of Invention
In order to solve the problem that when an unknown obstacle is encountered, a track route avoiding the unknown obstacle cannot be planned in time, so that the unknown obstacle cannot be avoided in time by the self-moving equipment, and the self-moving equipment collides with the unknown obstacle, the application provides a track planning method, a track planning device, track planning equipment and a storage medium.
The invention provides a track planning method which is applied to self-mobile equipment, and comprises the following steps: acquiring a global track of walking of the mobile device; acquiring a local map walked by the self-mobile device according to the global track row, and updating the areas in the field of view and outside the field of view of the local map according to point clouds acquired by the self-mobile device in different periods of time; when the updated track to be tracked of the local map is predicted to intersect with an obstacle, reconstructing the current track to be tracked, and taking the reconstructed track to be tracked which does not intersect with the obstacle as a local track, wherein the method comprises the following steps of: the method comprises the steps of projecting a point cloud of an obstacle on a global track, selecting a point, close to the obstacle, far away from the self-moving equipment, of the global track as a target point, selecting a point, between the obstacle and the self-moving equipment, of the global track as a starting point, searching local path points between the target point and the starting point, searching out path points which do not intersect with the obstacle, generating a multi-segment spline curve according to the path points which do not intersect with the obstacle, and taking the multi-segment spline curve as a local track.
Preferably, before the step of obtaining the global track walked by the mobile device, the method further includes: constructing a map by the information of the environment and the information of the obstacle; searching path points on the global map by adopting a path planning algorithm, and constructing a spline curve through the path points; and carrying out spline curve optimization on the spline curve through the motion characteristics of the self-mobile equipment, and taking the optimized spline curve as the global track.
Preferably, the path planning algorithm is any one of the following: hybrid a algorithm, dijkstra algorithm, SPFA algorithm.
Preferably, the step of obtaining the local map walked by the self-mobile device according to the global track row includes; and obtaining a local map according to the visual field range shot by the sensor of the mobile device.
Preferably, the step of updating the area inside and outside the field of view of the local map according to the point clouds acquired by the sensor of the self-mobile device over different time periods includes: acquiring point clouds of the obstacles based on the acquired image information in the visual field range, projecting the point clouds of the obstacles on the local map, and updating the area in the visual field of the local map; and comparing the point cloud of the currently detected obstacle with the point cloud of the obstacle detected when walking along the global track, and updating the area outside the field of view of the local map.
Preferably, the generating of the multi-segment spline curve includes: obtaining track points and tangent vectors of a global track, wherein the tangent vectors are arc trimming edges of the track points, and each track point corresponds to one tangent vector; for each trace point, four parameters are determined according to the following formula:wherein (1)>Is->Coordinates of the individual track points>Is->Tangential vector of the individual track points, +.>The method comprises the steps of carrying out a first treatment on the surface of the Every two adjacent track points, a section of spline curve is generated according to the following formula:wherein (1)>，/>，/>，/>Is->Parameters of the section curve,/>Is a normalized variable representing the position on each segment of the curve, t=0 corresponding to the start point and t=1 corresponding to the end point; calculating an objective function of each spline curve, wherein the objective function comprises a smoothness cost and a collision cost, and the smoothness cost is: />Wherein (1)>,/>Is->Section curveIs the collision cost: />Wherein (1)>Is->The distance from the track point on the segment curve to the nearest barrier; calculating the objective function minimization:wherein (1)>Is the parameter vector for all segments,/>Is a weight coefficient, +.>Is a constant, & gt>Is a kinematic model of the self-moving device.
The application also provides a track planning device, comprising: the construction module is used for acquiring a global track of the mobile equipment walking; the updating module is used for acquiring a local map walked by the self-mobile device according to the global track row, and updating the areas in the field of view and outside the field of view of the local map according to the point clouds shot by the self-mobile device in different periods of time; and the reconstruction module is used for projecting the point cloud of the obstacle on the global track when the updated track to be tracked of the local map intersects with the obstacle, selecting the point, close to the obstacle and far away from the self-moving equipment, of the global track as a target point, selecting the point, between the obstacle and the self-moving equipment, of the global track as a starting point, searching local path points between the target point and the starting point, searching out path points which do not intersect with the obstacle, generating a multi-section spline curve according to the path points which do not intersect with the obstacle, and taking the multi-section spline curve as the local track.
The application also provides self-mobile equipment, which comprises a memory and a processor, wherein the memory stores a computer program capable of running on the processor, and the self-mobile equipment is characterized in that the processor realizes a track planning method according to any one of the above methods when executing the computer program.
The present application also provides a computer readable storage medium having stored thereon a computer program which, when executed by a processor, causes the processor to perform a trajectory planning method as claimed in any one of the preceding claims.
Compared with the prior art, the application has the following beneficial effects: and (5) avoiding unknown obstacles. The method comprises the steps that the areas in the view field and outside the view field of a local map are updated through point clouds shot by a self-moving device in different periods, whether the areas intersect with an obstacle or not is judged according to the to-be-tracked track of the updated local map, if the areas intersect with the to-be-tracked track, the self-moving device is proved to collide with the obstacle, the current to-be-tracked track is reconstructed until the to-be-tracked track does not intersect with the obstacle, the disjoint to-be-tracked track is taken as the local track, the self-moving device walks along the local track, the problem that the track route avoiding the unknown obstacle cannot be planned in time when the self-moving device encounters the unknown obstacle is solved, and the problem that the self-moving device collides with the unknown obstacle is caused.
Drawings
In order to more clearly illustrate the embodiments of the invention or the technical solutions of the prior art, the drawings which are used in the description of the embodiments or the prior art will be briefly described, it being obvious that the drawings in the description below are only some embodiments of the invention, and that other drawings can be obtained from these drawings without inventive faculty for a person skilled in the art.
The structures, proportions, sizes, etc. shown in the drawings are shown only in connection with the present disclosure, and are not intended to limit the scope of the invention, since any modification, variation in proportions, or adjustment of the size, etc. of the structures, proportions, etc. should be considered as falling within the spirit and scope of the invention, without affecting the effect or achievement of the objective.
Fig. 1 is a schematic flow chart of a track planning method according to an embodiment of the present invention;
FIG. 2 is a schematic block diagram of a track planning apparatus according to an embodiment of the present invention;
fig. 3 is a schematic block diagram of a self-mobile device according to an embodiment of the present invention.
Reference numerals illustrate:
10. a trajectory planning device; 11. constructing a module; 12. updating a module; 13. a reconstruction module; 20. a self-moving device; 21. a memory; 22. a processor.
Detailed Description
The following description of the embodiments of the present invention will be made clearly and fully with reference to the accompanying drawings, in which it is evident that the embodiments described are some, but not all embodiments of the invention. All other embodiments, which can be made by those skilled in the art based on the embodiments of the invention without making any inventive effort, are intended to be within the scope of the invention.
The flow diagrams depicted in the figures are merely illustrative and not necessarily all of the elements and operations/steps are included or performed in the order described. For example, some operations/steps may be further divided, combined, or partially combined, so that the order of actual execution may be changed according to actual situations.
It is also to be understood that the terminology used in the description of the present application is for the purpose of describing particular embodiments only and is not intended to be limiting of the application. As used in this specification 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 be further understood that the term "and/or" as used in this specification and the appended claims refers to any and all possible combinations of one or more of the associated listed items, and includes such combinations.
The technical scheme of the invention is further described below by the specific embodiments with reference to the accompanying drawings.
In one embodiment
The invention relates to a track planning method which can improve the defect of sensing environmental information by a limited field of view (machine vision) in a sensing technology of self-mobile equipment and plan a track route avoiding unknown obstacles. Wherein the self-moving device may be a robot, a mower, or the like.
As shown in fig. 1, steps S100 to S300 respectively represent steps of the track planning method of the present invention, and specifically are as follows:
step S100: a global trajectory is obtained from the mobile device walking.
In the step, global track planning refers to a process of searching a path from a starting point to an end point on an ESDF map, smoothing the path through a spline curve and generating a global track;
the ESDF map is a euclidean sign distance field, each voxel in the field contains euclidean distance of an obstacle nearest to the euclidean sign distance field, and the value of the euclidean distance is the euclidean distance from each grid point to the nearest obstacle in the environment, and is negative when the euclidean distance is positioned in the obstacle and positive when the euclidean distance is positioned outside the obstacle; the ESDF map can be constructed through a static grid map or updated through a dynamic point cloud, and can be used for obstacle avoidance and optimization in track planning, and modeling and estimating of obstacles.
Step S200: and acquiring a local map walking from the mobile device according to the global track row, and updating the areas in the field of view and outside the field of view of the local map according to the point clouds acquired by the mobile device in different periods.
In the step, the local map is a map of a visual field range acquired from a sensor of the mobile device, the updating of the local map is a process of updating the local map according to sensing information of a limited visual field, and the local map is updated according to different point clouds acquired from the sensor of the mobile device in different time periods of a global track. A local map is a map representing information of the surroundings of the self-mobile device, its size and shape being the same as the field of view of the self-mobile device, its value being the euclidean distance of each grid point to the nearest obstacle, negative when inside the obstacle and positive when outside the obstacle, the local map being usable for obstacle avoidance and optimization in local trajectory planning, as well as modeling and estimation of the obstacle.
Step S300: when the to-be-tracked track of the updated local map is predicted to intersect with the obstacle, the point cloud of the obstacle is projected on the global track, the point, close to the obstacle and far away from the self-moving equipment, of the global track is selected as a target point, the point, between the obstacle and the self-moving equipment, of the global track is selected as a starting point, local path point searching is conducted between the target point and the starting point, path points which do not intersect with the obstacle are searched, a multi-section spline curve is generated according to the path points which do not intersect with the obstacle, and the multi-section spline curve is used as the local track.
In the method, when the to-be-tracked track on the updated local map is predicted to intersect with an obstacle by the self-moving equipment, a point cloud of the obstacle is projected on a global track, a point, close to the obstacle, far away from the self-moving equipment is selected as a target point, a point, between the obstacle and the self-moving equipment, of the global track is selected as a starting point, local path point searching is carried out between the target point and the starting point, a path point which does not intersect with the obstacle is searched, a multi-section spline curve is generated according to the path point which does not intersect with the obstacle, the multi-section spline curve is used as a local track, and the self-moving equipment moves along the local track; reconstructing the track to be tracked on the local map refers to a process of carrying out local re-planning after the track to be tracked intersects with the obstacle in a limited view field.
Updating the areas in and out of the view field of the local map through point cloud data of different time periods acquired by the self-moving equipment, judging whether collision occurs between the self-moving equipment and the obstacle or not through the distance relation between the track prediction to be tracked and the obstacle of the updated local map, if the track prediction to be tracked is intersected with the obstacle, proving that the self-moving equipment collides with the obstacle, reconstructing the track to be tracked until the track to be tracked is not intersected with the obstacle, taking the new track to be tracked as the local track, enabling the self-moving equipment to run along the local track, and further improving the problem that the track route avoiding the unknown obstacle cannot be planned in time when the self-moving equipment encounters the unknown obstacle, so that the self-moving equipment cannot avoid the unknown obstacle in time and collides with the unknown obstacle.
In one embodiment
Before step S100, the method further includes:
constructing a global map through the information of the environment and the information of the obstacle;
searching path points on the global map by adopting a path planning algorithm, and constructing a spline curve through the path points;
and (3) performing spline curve optimization on the spline curve through the motion characteristics of the self-moving equipment, and taking the optimized spline curve as a global track.
In this step, a path planning algorithm or other heuristic search algorithm is first used on the global map, for example: hybrid a algorithm, dijkstra algorithm; the hybrid A algorithm is adopted in the application, and is a graph searching algorithm which is improved to the A algorithm. Searching a shortest path through a hybrid A algorithm, wherein the path consists of a series of path points, and the coordinates of the path points are the coordinates of the central point of the grid map; and interpolating the path points through the spline curve to generate a continuous track spline curve, so as to obtain a global track.
Wherein, step S200 includes:
obtaining a local map from a field of view taken from a sensor of the mobile device;
acquiring point clouds of the obstacle based on the acquired image information in the visual field range, projecting the point clouds of the obstacle on a local map, and updating the area in the visual field of the local map;
and comparing the point cloud of the currently detected obstacle with the point cloud of the obstacle detected when walking along the global track, and updating the area outside the field of view of the local map.
In the step, an obstacle point cloud, namely a group of three-dimensional coordinate points representing the position of an obstacle, is acquired through a sensor (such as a laser radar, a camera and the like) of the mobile equipment; and filtering the obstacle point cloud, removing noise points, repeated points, useless points and the like, improving the quality and sparseness of the point cloud, and transforming the coordinates of the obstacle point cloud from a sensor coordinate system to a self-moving device body coordinate system, namely, a coordinate system with the center of the self-moving device as an origin, the front of the self-moving device as an x-axis positive direction, the left of the self-moving device as a y-axis positive direction and the upper of the self-moving device as a z-axis positive direction.
Update within the local map field of view: and updating the area in the view field of the local map according to the coordinates after the point cloud transformation, namely projecting the point cloud onto the plane of the local map, and updating the value of the grid point according to the distance from the point cloud to the grid point so as to enable the value to represent the distance from the point cloud to the nearest obstacle.
Local map out-of-view update: based on the current observation and past characteristic point clouds, updating the area outside the field of view of the local map, namely matching the current point cloud with the past point cloud according to the characteristics (such as color, shape, scale and the like) of the point cloud, determining that the point cloud outside the field of view should be reserved, filled or eliminated, and thus updating the value of the local map.
Wherein, step S300 includes: the multi-segment spline curve generation comprises the following steps:
acquiring track points and tangent vectors of a global track, wherein the tangent vectors are arc trimming of the track points, and each track point corresponds to one tangent vector;
for each trace point, four parameters are determined according to the following formula:
wherein,is->Coordinates of the individual track points>Is->Tangential vector of the individual track points, +.>；
For each two adjacent track points, generating a section of spline curve according to the following formula:
wherein,，/>，/>，/>is->Parameters of the section curve,/>Is a normalized variable representing the position on each segment of the curve, t=0 corresponding to the start point and t=1 corresponding to the end point;
for calculating an objective function of each spline curve, the objective function comprises a smoothness cost and a collision cost, wherein the smoothness cost is:
wherein,,/>is->Parameters of the section curve, collision cost is:
wherein,is->The distance from the track point on the segment curve to the nearest obstacle is a reference toIs obtained from a local map, is negative when inside an obstacle, and imposes a large penalty;
calculating an objective function minimization:
wherein,is the parameter vector for all segments,/>Is a weight coefficient, +.>Is a constant, & gt>Is the first term of minimization of the objective function from the kinematic model of the mobile device +.>In order to maximize the smoothness of the track, even if the curvature of the track is minimized; second term of minimization of objective function +.>In order to maximize the obstacle avoidance of the track, even if the track is far from the obstacle; third term of minimization of objective function +.>In order to maximize the feasibility of the trajectory even if the trajectory meets the kinematic constraints of the self-mobile device.
Finally, a local trajectory is obtained by minimizing the objective function and a part is extracted from the local trajectory as a reference trajectory consisting of a number of trajectory points, which should be selected to meet the following conditions: (1) Is located within the field of view, i.e., is less than the radius of the field of view from the mobile device; (2) At a future location, i.e., a distance from the mobile device that is greater than the speed of the mobile device times a time interval; (3) Even distribution, i.e. the distance between two adjacent track points is equal or close. The reference track is ensured to be continuous, visible and reachable, and has moderate length, and the function of the reference track is to provide a local navigation guide and transmit the navigation guide to the self-mobile device for tracking, so that the obstacle is avoided.
It should be noted that, for convenience and brevity of description, specific working processes of the above-described apparatus and each module may refer to corresponding processes in the foregoing embodiment of the encryption method of hard disk data, which are not described herein again.
In one embodiment
As shown in fig. 2, the present application further provides a track planning apparatus 10 of the limited field-of-view self-mobile device, which includes a construction module 11, an update module 12, and a reconstruction module 13;
building module 11: the global track is used for acquiring the walking of the mobile device; i.e. the target path from the mobile device.
Update module 12: the method is used for acquiring the local map walking from the mobile device according to the global track row, and updating the areas inside and outside the field of view of the local map according to the point clouds acquired by the mobile device in different periods. When the self-mobile device walks along the global track, a local map is obtained according to the visual field range shot by the sensor of the self-mobile device, wherein the local map is the current visible environment of the self-mobile device, and the local map can be updated according to point clouds shot by the sensor of the self-mobile device at different time intervals, namely, the error of the local map is corrected according to new observation data.
Reconstruction module 13: when the to-be-tracked track of the updated local map is predicted to intersect with the obstacle, the point cloud of the obstacle is projected on the global track, the point, close to the obstacle and far away from the self-moving equipment, of the global track is selected as a target point, the point, between the obstacle and the self-moving equipment, of the global track is selected as a starting point, local path point searching is conducted between the target point and the starting point, path points which do not intersect with the obstacle are searched, a multi-segment spline curve is generated according to the path points which do not intersect with the obstacle, and the multi-segment spline curve is used as the local track. When the self-moving equipment detects that the local track walking along the local map collides with an obstacle, the self-moving equipment reconstructs the current local track, namely, regenerates a safe and smooth local track according to the local map and the global track, and can also transmit the local track to a controller of the self-moving equipment to control the self-moving equipment to walk along the local track until the next node or end point of the global track is reached.
The local map and the local track can be dynamically generated and updated according to the visual field range of the self-mobile device and the sensor information by the track planning device 10, so that the self-mobile device can avoid obstacles and walk along the global track.
In this embodiment, for specific implementation of each module in the above-mentioned track planning apparatus embodiment, please refer to the description in the above-mentioned method embodiment, and no further description is given here.
In one embodiment
As shown in fig. 3, the present application further provides a self-mobile device 20, including a memory 21 and a processor 22, where the memory 21 stores a computer program that can run on the processor 22, and has the effect of implementing the track planning method in embodiment 1, that is, dynamically generating and updating the local map and the local track according to the field of view of the self-mobile device and the sensor information, so that the self-mobile device can avoid an obstacle and walk along the global track.
In one embodiment
The present application further provides a computer readable storage medium, on which a computer program is stored, which when executed by a processor causes the processor to perform a method for trajectory planning for a limited field-of-view self-mobile device as described above, which has the effect of providing a convenient way to transfer a specific implementation of the trajectory planning method in embodiment 1 to the trajectory planning device 10 in embodiment 2 via the storage medium, thereby enabling the trajectory planning device 10 to implement the trajectory planning method.
The above embodiments are only for illustrating the technical solution of the present invention, and not for limiting the same; although the invention has been described in detail with reference to the foregoing embodiments, it will be understood by those of ordinary skill in the art that: the technical scheme described in the foregoing embodiments can be modified or some technical features thereof can be replaced by equivalents; such modifications and substitutions do not depart from the spirit and scope of the technical solutions of the embodiments of the present invention.
Claims (8)
1. A track planning method, characterized by being applied to a self-mobile device, the method comprising:
acquiring a global track of walking of the mobile device;
acquiring a local map walked by the self-mobile device according to the global track row, and updating the areas in the field of view and outside the field of view of the local map according to point clouds acquired by the self-mobile device in different periods of time;
when the updated track to be tracked of the local map intersects an obstacle, projecting a point cloud of the obstacle on the global track, selecting a point, close to the obstacle and far away from the self-mobile device, of the global track as a target point, selecting a point, between the obstacle and the self-mobile device, of the global track as a starting point, searching local path points between the target point and the starting point, searching out path points which do not intersect the obstacle, generating a multi-section spline curve according to the path points which do not intersect the obstacle, and taking the multi-section spline curve as a local track;
the multi-segment spline curve generation includes:
obtaining track points and tangent vectors of the global track, wherein the tangent vectors are arc trimming edges of the track points, and each track point corresponds to one tangent vector;
for each trace point, four parameters are determined according to the following formula:wherein->Is->Coordinates of the individual track points>Is->Tangential vector of the individual track points, +.>；
Every two adjacent track points, a section of spline curve is generated according to the following formula:wherein->，/>，/>Is->Parameters of the section curve,/>Is a normalized variable representing the position on each segment of the curve,/for each segment of the curve>Corresponding to the starting point->Corresponding to the end point;
calculating an objective function of each spline curve, wherein the objective function comprises a smoothness cost and a collision cost, and the smoothness cost is:wherein->，/>Is->Parameters of the segment curve;
the collision cost is as follows:wherein->Is->The distance from the track point on the segment curve to the nearest barrier;
calculating the objective function minimization:wherein->Is the parameter vector for all segments,/>Is a weight coefficient, +.>Is a constant, & gt>Is a kinematic model of the self-moving device.
2. The method of claim 1, wherein the step of obtaining the global trajectory from the mobile device walking is preceded by the step of:
constructing a global map through the information of the environment and the information of the obstacle;
searching path points on the global map by adopting a path planning algorithm, and constructing a spline curve through the path points;
and carrying out spline curve optimization on the spline curve through the motion characteristics of the self-mobile equipment, and taking the optimized spline curve as the global track.
3. A trajectory planning method according to claim 2, characterized in that the path planning algorithm is any one of the following: hybrid a algorithm, dijkstra algorithm.
4. The method of claim 1, wherein the step of obtaining the local map of the self-mobile device walking from the global track row comprises:
and obtaining a local map according to the visual field range shot by the sensor of the mobile device.
5. A trajectory planning method according to claim 1, characterized in that said step of updating the area inside and outside the field of view of said local map according to the point clouds taken by said self-mobile device over different time periods comprises:
acquiring point clouds of the obstacles based on the acquired image information in the visual field range, projecting the point clouds of the obstacles on the local map, and updating the area in the visual field of the local map;
and comparing the point cloud of the currently detected obstacle with the point cloud of the obstacle detected when walking along the global track, and updating the area outside the field of view of the local map.
6. A trajectory planning device, comprising:
the construction module is used for acquiring a global track of the mobile equipment walking;
the updating module is used for acquiring a local map walked by the self-mobile device according to the global track row, and updating the areas in the field of view and outside the field of view of the local map according to the point clouds shot by the self-mobile device in different periods of time;
a reconstruction module, configured to, when it is predicted that an updated track to be tracked of the local map intersects an obstacle, project a point cloud of the obstacle on the global track, select a point of the global track, which is close to the obstacle and far from the self-mobile device, as a target point, select a point of the global track, which is between the obstacle and the self-mobile device, as a starting point, perform local path point search between the target point and the starting point, search for a path point that does not intersect the obstacle, generate a multi-segment spline curve according to the path point that does not intersect the obstacle, and use the multi-segment spline curve as a local track;
the multi-segment spline curve generation includes:
obtaining track points and tangent vectors of the global track, wherein the tangent vectors are arc trimming edges of the track points, and each track point corresponds to one tangent vector;
for each trace point, four parameters are determined according to the following formula:wherein, the method comprises the steps of, wherein,is->Coordinates of the individual track points>Is->Tangential vector of the individual track points, +.>；
Every two adjacent track points, a section of spline curve is generated according to the following formula:wherein->，/>，/>Is->Parameters of the section curve,/>Is a normalized variable representing the position on each segment of the curve,/for each segment of the curve>Corresponding to the starting point->Corresponding to the end point;
calculating an objective function of each spline curve, wherein the objective function comprises a smoothness cost and a collision cost, and the smoothness cost is:wherein->，/>Is->Parameters of the segment curve;
the collision cost is as follows:wherein->Is->The distance from the track point on the segment curve to the nearest barrier;
calculating the objective function minimization:wherein->Is the parameter vector for all segments,/>Is a weight coefficient, +.>Is a constant, & gt>Is a kinematic model of the self-moving device.
7. A self-moving device comprising a memory and a processor, the memory storing a computer program executable on the processor, characterized in that the processor implements a trajectory planning method according to any one of claims 1 to 5 when executing the computer program.
8. A computer readable storage medium, characterized in that it has stored thereon a computer program which, when run by a processor, causes the processor to perform a trajectory planning method as claimed in any one of claims 1 to 5.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202410144447.XA CN117705123B (en) | 2024-02-01 | 2024-02-01 | Track planning method, device, equipment and storage medium |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202410144447.XA CN117705123B (en) | 2024-02-01 | 2024-02-01 | Track planning method, device, equipment and storage medium |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN117705123A CN117705123A (en) | 2024-03-15 |
| CN117705123B true CN117705123B (en) | 2024-04-09 |
Family
ID=90146453
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202410144447.XA Active CN117705123B (en) | 2024-02-01 | 2024-02-01 | Track planning method, device, equipment and storage medium |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN117705123B (en) |
Citations (9)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN101837591A (en) * | 2010-03-12 | 2010-09-22 | 西安电子科技大学 | Robot path planning method based on two cooperative competition particle swarms and Ferguson spline |
| WO2021142799A1 (en) * | 2020-01-17 | 2021-07-22 | 华为技术有限公司 | Path selection method and path selection device |
| CN114510057A (en) * | 2022-02-21 | 2022-05-17 | 沈阳理工大学 | ROS-based mobile robot autonomous navigation method in indoor environment |
| CN114779785A (en) * | 2022-05-06 | 2022-07-22 | 浙江工业大学 | Mobile robot smooth track planning method based on PSO parameter setting |
| CN115140096A (en) * | 2022-07-28 | 2022-10-04 | 西安电子科技大学芜湖研究院 | Spline curve and polynomial curve-based automatic driving track planning method |
| CN115933668A (en) * | 2022-12-15 | 2023-04-07 | 广东工业大学 | Mobile robot path planning method, system, robot and storage medium |
| CN116203990A (en) * | 2023-03-17 | 2023-06-02 | 山东大学 | Unmanned plane path planning method and system based on gradient descent method |
| CN117369467A (en) * | 2023-10-31 | 2024-01-09 | 深圳亿嘉和科技研发有限公司 | Spline curve optimization-based robot track optimization method and device |
| CN117406771A (en) * | 2023-10-17 | 2024-01-16 | 武汉大学 | Efficient autonomous exploration method, system and equipment based on four-rotor unmanned aerial vehicle |
Family Cites Families (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN107702716B (en) * | 2017-08-31 | 2021-04-13 | 广州小鹏汽车科技有限公司 | Unmanned driving path planning method, system and device |
| US20240001961A1 (en) * | 2022-05-31 | 2024-01-04 | Korea Advanced Institute Of Science And Technology | High-Definition Map-Based Local Path Planning Method and Apparatus for Dynamic and Static Obstacle Avoidance |
-
2024
- 2024-02-01 CN CN202410144447.XA patent/CN117705123B/en active Active
Patent Citations (9)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN101837591A (en) * | 2010-03-12 | 2010-09-22 | 西安电子科技大学 | Robot path planning method based on two cooperative competition particle swarms and Ferguson spline |
| WO2021142799A1 (en) * | 2020-01-17 | 2021-07-22 | 华为技术有限公司 | Path selection method and path selection device |
| CN114510057A (en) * | 2022-02-21 | 2022-05-17 | 沈阳理工大学 | ROS-based mobile robot autonomous navigation method in indoor environment |
| CN114779785A (en) * | 2022-05-06 | 2022-07-22 | 浙江工业大学 | Mobile robot smooth track planning method based on PSO parameter setting |
| CN115140096A (en) * | 2022-07-28 | 2022-10-04 | 西安电子科技大学芜湖研究院 | Spline curve and polynomial curve-based automatic driving track planning method |
| CN115933668A (en) * | 2022-12-15 | 2023-04-07 | 广东工业大学 | Mobile robot path planning method, system, robot and storage medium |
| CN116203990A (en) * | 2023-03-17 | 2023-06-02 | 山东大学 | Unmanned plane path planning method and system based on gradient descent method |
| CN117406771A (en) * | 2023-10-17 | 2024-01-16 | 武汉大学 | Efficient autonomous exploration method, system and equipment based on four-rotor unmanned aerial vehicle |
| CN117369467A (en) * | 2023-10-31 | 2024-01-09 | 深圳亿嘉和科技研发有限公司 | Spline curve optimization-based robot track optimization method and device |
Also Published As
| Publication number | Publication date |
|---|---|
| CN117705123A (en) | 2024-03-15 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| Zhang et al. | 2d lidar-based slam and path planning for indoor rescue using mobile robots | |
| Zelinsky et al. | Planning paths of complete coverage of an unstructured environment by a mobile robot | |
| Basiri et al. | A survey on the application of path-planning algorithms for multi-rotor UAVs in precision agriculture | |
| CN112835333B (en) | Multi-AGV obstacle avoidance and path planning method and system based on deep reinforcement learning | |
| Magid et al. | Voronoi-based trajectory optimization for UGV path planning | |
| CN114119920A (en) | Three-dimensional point cloud map construction method and system | |
| Jian et al. | Putn: A plane-fitting based uneven terrain navigation framework | |
| CN113819917A (en) | Automatic driving path planning method, device, equipment and storage medium | |
| CN111578926A (en) | Map generation and navigation obstacle avoidance method based on automatic driving platform | |
| Ye et al. | Path planning for mobile robots in unstructured orchard environments: An improved kinematically constrained bi-directional RRT approach | |
| Balan et al. | Optimal trajectory planning for multiple waypoint path planning using tabu search | |
| Hu et al. | Path planning of UGV based on Bézier curves | |
| Liu et al. | Hercules: An autonomous logistic vehicle for contact-less goods transportation during the COVID-19 outbreak | |
| CN114063615B (en) | Reversing navigation control method and system of intelligent pesticide spraying vehicle among ridges in greenhouse | |
| Finean et al. | Predicted composite signed-distance fields for real-time motion planning in dynamic environments | |
| Wu et al. | Real-time three-dimensional smooth path planning for unmanned aerial vehicles in completely unknown cluttered environments | |
| CN117705123B (en) | Track planning method, device, equipment and storage medium | |
| Höffmann et al. | Coverage path planning and precise localization for autonomous lawn mowers | |
| CN116136417B (en) | Unmanned vehicle local path planning method facing off-road environment | |
| CN116009558A (en) | Mobile robot path planning method combined with kinematic constraint | |
| Dhiman et al. | A review of path planning and mapping technologies for autonomous mobile robot systems | |
| Akmandor et al. | Reactive navigation framework for mobile robots by heuristically evaluated pre-sampled trajectories | |
| JP2022157259A (en) | System, method and work vehicle | |
| Alagić et al. | Design of mobile robot motion framework based on modified vector field histogram | |
| Luo et al. | An effective search and navigation model to an auto-recharging station of driverless vehicles |
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 |