CN115752474A - Robot navigation planning method and device under non-flat ground environment and robot - Google Patents
Robot navigation planning method and device under non-flat ground environment and robot Download PDFInfo
- Publication number
- CN115752474A CN115752474A CN202211472175.3A CN202211472175A CN115752474A CN 115752474 A CN115752474 A CN 115752474A CN 202211472175 A CN202211472175 A CN 202211472175A CN 115752474 A CN115752474 A CN 115752474A
- Authority
- CN
- China
- Prior art keywords
- track
- penalty function
- robot
- point cloud
- grid
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
Images
Abstract
The invention discloses a robot navigation planning method, a device and a robot under a non-flat ground environment, wherein the method comprises the following steps: acquiring an original point cloud, and filtering a passable area of the original point cloud; carrying out down-sampling on the original point cloud to generate a grid occupying map; down-sampling the filtered point cloud to construct a front-end path search space; in the generated front-end path search space, carrying out graph search based on A to obtain a front-end path; generating a track initial value based on the obtained front end path, constructing a penalty function according to an occupied grid map, and generating a final track by using a convex optimization algorithm, wherein the penalty function comprises a smoothness penalty function for smoothing the track, a total time penalty function for controlling the total duration of the track, a dynamics penalty function for enabling the expected speed and the expected acceleration of the robot at any moment on the track to meet the dynamic limit of the robot, and a safety penalty function for enabling the track to avoid a dangerous area and not generate a collision.
Description
Technical Field
The invention belongs to the technical field of autonomous unmanned systems, and particularly relates to a robot navigation planning method and device in a non-flat ground environment and a robot.
Background
In many challenging application scenarios (such as search and rescue, exploration, inspection, or transportation), autonomous navigation is critical to the safe and efficient operation of an unmanned vehicle, which allows the unmanned vehicle to perform or assist tasks that are repetitive, exhausting, and even dangerous to humans. In addition to localization and mapping, trajectory generation is a key function of any autonomous navigation system.
The unmanned vehicle track generation technology means that an unmanned vehicle automatically plans a section of safe motion track from a current point to a target point according to the environment sensing of a sensor of the unmanned vehicle. Aiming at the generation of the track of the ground robot, three problems are mainly solved: 1) Connectivity, i.e. the trajectory must be able to connect the start and end points; 2) Performability, i.e. the trajectory fulfils constraints of the robot itself, such as minimum turning radius (maximum curvature), maximum speed, maximum acceleration, etc.; 3) Optimality, namely, the length of the track and the energy consumption of the robot should be optimized as much as possible on the premise of completing the tasks.
In the existing algorithm, for convenience of calculation, navigation planning is mostly carried out in a two-dimensional environment; however, the working environment of the ground robot cannot be simply described by a two-dimensional environment, such as a field environment (rough lawn, mountain road full of soil bags), an artificial building with a multi-layer structure, and the like, in the prior art, the environment is often divided into obstacles and passable areas only in a two-dimensional plane, for example, a height threshold dichotomy is used, so that the influence of the terrain on the unmanned vehicle is not considered, and a large amount of actually passable ground is omitted, so that the calculated track cannot guarantee the performability, the optimality under a certain standard (such as the length of the track and the energy consumption of the robot), and even a reasonable track cannot be planned in a complex environment.
There are some new technologies that allow vehicle navigation planning to be performed directly on the three-dimensional point cloud, however, the method of calculating directly on the three-dimensional point cloud usually needs to perform KNN nearest neighbor on the point cloud online, and to fit local terrain features, the computation amount is so large that real-time calculation cannot be performed. Moreover, the algorithm based on local fitting cannot reflect the characteristics of the point cloud in a larger range, and the robustness is not high in a large-scale complex environment. More importantly, the track generated by directly performing navigation planning on the point cloud is limited by the discreteness of point cloud information, is often represented as a broken line path without time information, and has weak track performability and high energy consumption.
Disclosure of Invention
The embodiment of the application aims to provide a method, a device and a robot for planning robot navigation under a non-flat ground environment, so as to solve the problems of inaccurate environment representation existing in the related technology aiming at two-dimensional plane planning, large calculation amount, low instantaneity, poor track smoothness, lack of time information and high energy consumption existing in the process of navigating on direct point cloud.
According to a first aspect of the embodiments of the present application, there is provided a method for planning robot navigation in a non-flat ground environment, including:
step (1): acquiring an original point cloud, and filtering a passable area of the original point cloud;
step (2): carrying out down-sampling on the original point cloud to generate an occupied grid map;
and (3): down-sampling the filtered point cloud to construct a front-end path search space;
and (4): performing graph search based on A in the front-end path search space generated in the step (3) to obtain a front-end path;
and (5): generating a track initial value based on the front-end path obtained in the step (4), constructing a penalty function according to the occupancy grid map, and generating a final track by using a convex optimization algorithm, wherein the penalty function comprises a smoothness penalty function for smoothing the track, a total time penalty function for controlling the total duration of the track, a dynamics penalty function for enabling the robot to expect the speed and the acceleration at any time on the track to meet the dynamic limit of the robot, and a safety penalty function for enabling the track to avoid a dangerous area and not to generate collision.
Further, the step (1) includes:
(1.1) traversing each point m in the original point cloud, and searching a field point with the radius r near the point m;
(1.2) traversing each point d in the set Dr formed by the neighborhood points to obtain the height difference z between the point d and the point m dm And an included angle alpha between the connecting line and the horizontal plane, and the calculation mode is as follows:
z dm ＝d 3 -m 3
wherein d is 3 And m 3 Pointing to the z-component of the d and m coordinates, respectively;
(1.3) judging whether the point m meets the condition of the passable area, wherein the judging condition is as follows: for any point d in Dr, z does not exist dm The case of < 0 and α < θ, if this condition is met, then point m is within the passable region Ms, otherwise point m is filtered out, where θ is the maximum safe standing tilt angle of the robot.
Further, the step (2) includes:
calculating the corresponding grid subscript of each point in the original point cloud:
wherein the floor function means rounding down, res occ For resolution of grid maps, bound min And bound max The boundary range of the original point cloud in the three-dimensional space is defined;
and judging the number of the points falling into each grid according to the subscript of the grid corresponding to each point, setting the value of the grid to be 1 if the number of the points falling into one grid is larger than a preset threshold, and setting the value of the grid to be 0 if the number of the points falling into one grid is not larger than the preset threshold, so as to complete reassignment of the grid, thereby generating the occupied grid map.
Further, the step (3) includes:
constructing an occupation grid map for the filtered point cloud Ms in the same manner as the step (2);
traversing the occupied grids in each filtered point cloud Ms, judging whether the grids above each occupied grid are non-occupied grids, and if so, defining the non-occupied grids in the front-end path search space so as to complete the construction of the front-end path search space.
Further, the smoothness penalty function J e For making the trajectory as smooth as possible, ensuring energy optimality:
where p (t) is the trajectory equation and M is the number of trajectory segments.
Further, the total time penalty function J t For making the total time of the trajectory as short as possible:
wherein T is i The total duration of the ith track is shown, and M is the number of track segments.
Further, the dynamics penalty function J d For enabling the desired speed and the desired acceleration of the robot at any time on the trajectory to satisfy the dynamic limits of the robot itself:
wherein C (= max {, 0} 3 In the form of a cubic penalty function,
in order to be able to take the desired speed,
in order to expect the acceleration of the vehicle,m is the number of track segments, v max And a max Maximum speed limit and maximum acceleration limit, respectively, k i The number of samples of the ith section of track is as follows:
wherein β is a polynomial locus p (t) = c T Beta (t) and c is a polynomial coefficient.
Further, the security penalty function J c For keeping the track away from the dangerous area and not generating collision, the construction process of the safety penalty function is as follows:
constructing an ESDF map on an occupied grid map constructed by the original point cloud, selecting a proper truncation distance, and generating a TSDF map:
wherein d is thr Is a safe distance threshold, d (p) is the TSDF function value, which penalizes the point on the trajectory too close to the obstacle,
an ith sampling point of a jth track is taken;
constructing a collision-free penalty function J based on the TSDF map ct ：
According to the pose transmission function of the robot and the analysis of an extra dangerous area brought by the terrain, a terrain penalty function is constructed:
wherein the pose transmission function of the robot
The coordinate of the center of the robot chassis at the space position p is represented as x 0 ,y 0 ,z 0 The inclination angle of the robot is
λ sp And λ se Is a weight coefficient;
according to the TSDF map and the terrain penalty function, constructing a security penalty function:
according to a second aspect of the embodiments of the present application, there is provided a robot navigation planning apparatus in a non-flat ground environment, including:
the filtering module is used for acquiring original point cloud and filtering the passable area of the original point cloud;
the generating module is used for carrying out down-sampling on the original point cloud to generate an occupation grid map;
the construction module is used for carrying out down-sampling on the filtered point cloud and constructing a front-end path search space;
the path searching module is used for carrying out graph searching based on A in the front-end path searching space generated in the construction module to obtain a front-end path;
and the path optimization module is used for generating an initial track value based on the front-end path obtained by the path search module, constructing a penalty function according to the occupation grid map, and generating a final track by using a convex optimization algorithm, wherein the penalty function comprises a smoothness penalty function for smoothing the track, a total time penalty function for controlling the total track duration, a dynamics penalty function for enabling the robot to expect the speed and the acceleration at any moment on the track to meet the dynamics limitation of the robot, and a safety penalty function for enabling the track to avoid a dangerous area and not generate a collision.
According to a third aspect of embodiments of the present application, there is provided a robot comprising:
one or more processors;
a memory for storing one or more programs;
when executed by the one or more processors, cause the one or more processors to implement a method as described in the first aspect.
The technical scheme provided by the embodiment of the application can have the following beneficial effects:
(1) The track generated by the method is safe and smooth, has energy optimality and good execution, and meets the motion rule of the ground robot.
(2) The invention directly utilizes the original point cloud to generate the map representation of the three-dimensional space, furthest reserves the information of the passable area and avoids the compression and the abandonment of the traditional two-dimensional algorithm to the environmental information.
(3) After the initial trajectory is obtained, the trajectory generated by the method is subjected to global optimization and local optimization, so that the optimality of the obtained trajectory is ensured.
(4) Trajectory generation may use a local point cloud map, which may be used if the global environment is unknown.
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 accompanying drawings, which are incorporated in and constitute a part of this specification, illustrate embodiments consistent with the present application and together with the description, serve to explain the principles of the application.
FIG. 1 is an overall block diagram illustrating a method for robot navigation planning in a non-flat ground environment, according to an exemplary embodiment.
FIG. 2 is a schematic diagram illustrating filtering of a passable area of a point cloud according to an exemplary embodiment.
FIG. 3 is a diagram illustrating the effectiveness of a terrain risk analysis, according to an exemplary embodiment.
FIG. 4 is a diagram illustrating front-end search effects, according to an example embodiment.
FIG. 5 is a diagram illustrating a final track effect according to an exemplary embodiment.
FIG. 6 is a block diagram illustrating a robotic navigation planning device in a non-flat ground environment, according to an exemplary embodiment.
FIG. 7 is a schematic diagram of an electronic device shown in accordance with an example embodiment.
Detailed Description
Reference will now be made in detail to the exemplary embodiments, examples of which are illustrated in the accompanying drawings. When the following description refers to the accompanying drawings, like numbers in different drawings represent the same or similar elements unless otherwise indicated. The embodiments described in the following exemplary embodiments do not represent all embodiments consistent with the present application.
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 is to be understood that, although the terms first, second, third, etc. may be used herein to describe various information, such information should not be limited by 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. Depending on the context, the word "if" as used herein may be interpreted as "at" \8230; … "or" when 8230; \8230; or "in response to a determination".
Fig. 1 is a flowchart illustrating a method for planning navigation of a ground robot in a non-flat ground environment according to an exemplary embodiment, where the method is applied to a terminal, and may include the following steps:
step (1): acquiring an original point cloud, and filtering a passable area of the original point cloud;
step (2): carrying out down-sampling on the original point cloud to generate a grid occupying map;
and (3): down-sampling the filtered point cloud to construct a front-end path search space;
and (4): performing graph search based on A in the front-end path search space generated in the step (3) to obtain a front-end path;
and (5): generating an initial track value based on the front-end path obtained in the step (4), constructing a penalty function according to the occupation grid map, and generating a final track by using a convex optimization algorithm, wherein the penalty function comprises a smoothness penalty function for smoothing the track, a total time penalty function for controlling the total duration of the track, a dynamics penalty function for enabling the robot to expect the speed and the acceleration of the robot to meet the dynamics limitation of the robot at any moment on the track, and a safety penalty function for enabling the track to avoid a dangerous area and not generate a collision.
According to the embodiment, the track generated by the method is safe and smooth, has energy optimality and good execution performance, and meets the motion rule of the ground robot; the map representation of the three-dimensional space is generated by directly utilizing the original point cloud, the information of the passable area is reserved to the maximum extent, and the compression and the abandonment of the traditional two-dimensional algorithm to the environmental information are avoided; after the initial track is obtained, the generated track is subjected to global optimization and local optimization, so that the optimality of the obtained track is ensured; trajectory generation may use local point cloud maps, which may be used if the global environment is unknown.
In the specific implementation of step (1), assuming that a set formed by points in the original point cloud is Mo and a set formed by filtered points is Ms, the specific steps of filtering the passable area of the point cloud are as follows:
and (1.1) traversing each point m in the original point cloud, searching a domain point with the radius r near the point m, and assuming that a set formed by the domain points is Dr.
(1.2) traversing each point d in Dr, and calculating the height difference between the point d and the point m and the included angle between the connecting line and the horizontal plane, wherein the calculation mode is as follows:
z dm ＝d 3 -m 3
wherein d is 3 And m 3 Pointing to the z-axis component of the d and m coordinates, respectively.
(1.3) judging whether the point m meets the condition of the passable area, wherein the condition is described as follows: for any point d in Dr, z does not exist dm < 0 and α < θ. If this condition is met, m points are within the passable region Ms, otherwise points m are filtered out. And theta is the maximum safe standing inclination angle of the robot and is determined according to the actual performance of the robot.
Based on the three steps, the original point clouds are utilized to screen out the point clouds in the passable area of the robot, the point clouds belonging to obstacles are screened out, and the point clouds play a vital role in subsequent front-end path search.
In an embodiment, fig. 2 shows the filtering of the passable area of the point cloud in this step, in fig. 2, (a) point a in the graph is a point satisfying passability and should be retained, points B, C, and D do not satisfy passability because the included angles between the red line and the horizontal plane exceed a threshold, in fig. 2, (B) the graph shows the determination of each point after the traversal is completed, and in fig. 2, (C) the graph is a point in Ms after the filtering is completed.
In the specific implementation of step (2), the resolution res of the grid map is first specified occ Then, traversing the points in the original point cloud, and determining the boundary range bound of the original point cloud in the three-dimensional space min And bound max The rectangular parallelepiped with the two points as the diagonal line will completely wrap the original pointAnd (4) cloud. Then traversing the point cloud again, and judging the number of points falling into each grid, wherein the calculation mode of the corresponding grid subscript of each point is as follows:
the floor function represents rounding down. After point cloud traversal operation is carried out, all grids are traversed, and the grids are assigned again: the value of the grid is set to 1 if the number of points falling in the grid is greater than a certain threshold, otherwise it is set to 0. Generally, the threshold value is often greater than 1 for a point cloud map obtained by the SLAM algorithm, and the threshold value may be set to 1 for a point cloud map that has undergone preliminary down-sampling. After this step is completed, the generation of the grid map is completed.
In the specific implementation of the step (3), the occupancy data grid map is constructed for the filtered point cloud Ms in the same manner as the step (2). Because Ms is a subset of Mo, no additional memory space is required to be applied for storing the occupied grid maps of Mo and Ms respectively. And traversing the occupied grids in each Ms, judging whether the grids above the Ms are non-occupied grids, if so, defining the non-occupied grids in the front-end path search space, and completing construction of the front-end path search space after the Ms is traversed by the occupied grids. It can be easily seen that the front-end path search space is a layer of space tightly attached to the upper surface of the object, so that the front-end path can be ensured to basically accord with the motion rule of the ground robot, and a good initial value is provided for subsequent track optimization.
In the specific implementation of the step (4), a graph search based on a is adopted, a starting point and an end point of the search are firstly determined, and the position of the robot at the moment is assumed to be p, and the position of the end point is assumed to be p e To ensure that the starting point and the ending point are both in the front path search space, p and p need to be aligned e Making a downward projection, i.e. determining p and p e If the grid is not in the front-end path search space, moving a grid downwards and judging again until the grid is in the front-end path search space. After the projection of the starting point and the end point is finishedA search is performed, which is a mature path search algorithm, and the specific process is not described herein.
In the specific implementation of step (5), the expression form of the track adopts the expression form of an mioco track class, which is specifically defined as follows:
the trajectory class uses parameters q, T to represent a polynomial, meaning: the polynomial trace that satisfies energy optimality passes through the intermediate point q and each segment is of time T. Where c is the polynomial coefficient, M is q, and the mapping of the T parameter to the c parameter.
A commonly used polynomial representation is to represent the polynomial using a coefficient c:
p(t)＝c T β(t)
β is the base of a polynomial. A mapping c = M (q, T) exists between the representation method of the mionc and the representation method of the coefficient c, and by using the mapping, the penalty function J (c, T) constructed on the polynomial can be conveniently converted into a penalty function constructed on the mionc locus: h (q, T) = J (M (q, T), and each partial derivative can also be found quickly.
The penalty functions of the track optimization comprise smoothness penalty functions, total time penalty functions, dynamics penalty functions and safety penalty functions. The optimization problem is of the form:
min c,T λ e J e +λ t J t +λ d J d +λ s J s
where λ is a corresponding weighting factor, and should be set empirically in a specific implementation.
The smoothness penalty function is used for enabling the track to be as smooth as possible and ensuring energy optimality:
where p (t) is the trajectory equation and M is the number of trajectory segments.
(II) the total time penalty function makes the total time of the trajectory as short as possible:
wherein T is i The total duration of the ith track is shown, and M is the number of track segments.
(III) the dynamics penalty function enables the speed and the acceleration of the robot expected at any time on the track to meet the dynamics limit of the robot, and the expected speed and the acceleration of a certain point on the track can be respectively calculated as follows because the track has time information:
the kinetic penalty function is as follows:
wherein C () = max {.,0} 3 For the cubic penalty function, M is the number of segments, v max And a max Maximum speed limit and maximum acceleration limit, k, respectively i The number of samples (numerical integration is approximately calculated by discrete numerical sampling) of the ith trace.
(IV) safety penalty function keeps the trajectory away from the danger zone and no collision occurs. For a ground robot, in addition to regarding an object as an obstacle, an additional dangerous area due to the terrain needs to be considered, and since the ground robot cannot fly and has a limited balancing capability, the additional dangerous area due to the terrain includes the following two types: 1. too far above the inclined ground, the robot standing above can cause overturning; 2. steep edges where the robot may move may fall or change posture dramatically. Therefore, the safety penalty function needs to take into account all the above mentioned dangerous situations.
Firstly, the track needs to be far away from the object, in order to realize this, an ESDF (explicit signaled Distance Functions) map is constructed on an occupied grid map constructed by the original point cloud, and a proper truncation Distance is selected to generate a TSDF (signaled Distance Function) map:
wherein d is thr Is the safe distance threshold, d (p) is the TSDF function value,
an ith sampling point of the jth track is obtained;
constructing a collision-free penalty function J based on the TSDF map ct ：
In addition, in order to determine the terrain dangerous area, terrain analysis is performed on the original point cloud: the RANSAC algorithm is utilized to calculate the projection pose of the robot at each position, the projection pose refers to the pose of the robot projected from a certain point of the space to the nearest surface, the terrain penalty function in the whole three-dimensional space can be calculated in such a way, the terrain penalty function is the same as the dimension of a track, and the ground robot cannot fly, the motion space of the ground robot is a subset of the whole three-dimensional space, so the calculation method can completely cover the motion space of the robot.
Setting the projection pose function of the robot as follows:
it shows the projection pose of the robot at the spatial position p as
Wherein x 0 ,y 0 ,z 0 The coordinates of the center of the robot chassis are shown,
indicating the robot tilt angle. After a pose projection function is obtained, a terrain penalty function can be constructed according to the analysis of an extra dangerous area brought by the terrain:
the first term of the function keeps the trajectory away from regions that are too inclined, while the second term keeps the trajectory away from steep edges and regions that cause drastic changes in the pose of the robot, λ is a weighting factor that, in a specific implementation, is taken according to the needs and experience of the robot.
Finally, the security penalty function is as follows:
and after the penalty function is constructed, performing numerical optimization on the track parameters by using a numerical optimizer, wherein an LBFGS numerical optimization algorithm is selected. And (4) after the numerical optimization is completed, obtaining a final track.
Fig. 3 shows the result of the risk analysis in a certain scenario, and it can be seen that on a slope without guardrails, the algorithm automatically separates dangerous edge zones.
Fig. 4 shows the effect of the front-end search, and it can be seen that the front-end is almost ground-fit, which can give a good initial value to the back-end optimization.
Fig. 5 shows the effect of the final trajectory planning, which is a smooth, safe and easy-to-implement ground robot trajectory thanks to the back-end safety constraints and the good initial values of the front-end.
Corresponding to the embodiment of the robot navigation planning method in the non-flat ground environment, the application also provides an embodiment of a robot navigation planning device in the non-flat ground environment.
FIG. 6 is a block diagram of a robotic navigation planner in a non-flat ground environment, according to an example embodiment.
Referring to fig. 6, the apparatus may include:
the filtering module 21 is configured to obtain an original point cloud, and perform passable area filtering on the original point cloud;
the generating module 22 is used for down-sampling the original point cloud to generate an occupation grid map;
the construction module 23 is configured to perform down-sampling on the filtered point cloud, and construct a front-end path search space;
a path search module 24, configured to perform a graph search based on a in the front-end path search space generated in the building module to obtain a front-end path;
and a path optimization module 25, configured to generate an initial value of a trajectory based on the front-end path obtained by the path search module, construct a penalty function according to the occupancy grid map, and generate a final trajectory by using a convex optimization algorithm, where the penalty function includes a smoothness penalty function for smoothing the trajectory, a total time penalty function for controlling a total duration of the trajectory, a dynamics penalty function for making a robot expected speed and an expected acceleration at any time on the trajectory meet a dynamics limit of the robot, and a safety penalty function for making the trajectory avoid a dangerous area and not generate a collision.
With regard to the apparatus in the above-described 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 here.
For the device embodiment, since it basically corresponds to the method embodiment, reference may be made to the partial description of the method embodiment for relevant points. The above-described embodiments of the apparatus are merely illustrative, and the units described as separate parts may or may not be physically separate, and parts displayed as units may or may not be physical units, may be located in one place, or may be distributed on a plurality of network units. Some or all of the modules can be selected according to actual needs to achieve the purpose of the scheme of the application. One of ordinary skill in the art can understand and implement without inventive effort.
Correspondingly, the present application also provides a robot, comprising: one or more processors; a memory for storing one or more programs; when executed by the one or more processors, cause the one or more processors to implement a method for robot navigation planning in a non-flat ground environment as described above. As shown in fig. 7, for a hardware structure diagram of any device with data processing capability where a robot navigation planning method in a non-flat ground environment according to an embodiment of the present invention is located, in addition to the processor, the memory, and the network interface shown in fig. 7, any device with data processing capability where an apparatus in the embodiment is located may generally include other hardware according to an actual function of the any device with data processing capability, which is not described again.
Accordingly, the present application also provides a computer readable storage medium, on which computer instructions are stored, and the instructions, when executed by a processor, implement the robot navigation planning method in the non-flat ground environment as described above. The computer readable storage medium may be an internal storage unit, such as a hard disk or a memory, of any data processing apparatus described in any of the previous embodiments. The computer readable storage medium may also be an external storage device such as a plug-in hard disk, a Smart Media Card (SMC), an SD Card, a Flash memory Card (Flash Card), etc. provided on the device. Further, the computer readable storage medium may include both an internal storage unit of any data processing capable device and an external storage device. The computer-readable storage medium is used for storing the computer program and other programs and data required by the arbitrary data processing-capable device, and may also be used for temporarily storing data that has been output or is to be output.
Other embodiments of the present application will be apparent to those skilled in the art from consideration of the specification and practice of the disclosure disclosed herein. This application is intended to cover any variations, uses, or adaptations of the invention following, in general, the principles of the application and including such departures from the present disclosure as come within known or customary practice within the art to which the invention pertains.
It will be understood that the present application is not limited to the precise arrangements described above and shown in the drawings and that various modifications and changes may be made without departing from the scope thereof.
Claims (10)
1. A robot navigation planning method in a non-flat ground environment is characterized by comprising the following steps:
step (1): acquiring an original point cloud, and filtering a passable area of the original point cloud;
step (2): carrying out down-sampling on the original point cloud to generate an occupied grid map;
and (3): down-sampling the filtered point cloud to construct a front-end path search space;
and (4): performing graph search based on A in the front-end path search space generated in the step (3) to obtain a front-end path;
and (5): generating a track initial value based on the front-end path obtained in the step (4), constructing a penalty function according to the occupancy grid map, and generating a final track by using a convex optimization algorithm, wherein the penalty function comprises a smoothness penalty function for smoothing the track, a total time penalty function for controlling the total time of the track, a dynamics penalty function for enabling the robot to expect speed and acceleration at any time on the track to meet the dynamic limit of the robot, and a safety penalty function for enabling the track to avoid a dangerous area and not to generate collision.
2. The method of claim 1, wherein step (1) comprises:
(1.1) traversing each point m in the original point cloud, and searching a field point with the radius r near the point m;
(1.2) traversing each point d in the set Dr formed by the neighborhood points to obtain the height difference z between the point d and the point m dm And an included angle alpha between the connecting line and the horizontal plane, and the calculation mode is as follows:
Z dm ＝d 3 -m 3
wherein d is 3 And m 3 Pointing the z-axis component of the d and m coordinates, respectively;
(1.3) judging whether the point m meets the conditions of the passable area, wherein the judging conditions are as follows: for any point d in Dr, z is not present dm The case of < 0 and α < θ, if this condition is met, then point m is within the passable region Ms, otherwise point m is filtered out, where θ is the maximum safe standing tilt angle of the robot.
3. The method of claim 1, wherein step (2) comprises:
calculating a grid subscript corresponding to each point in the original point cloud:
wherein the floor function means rounding down, res occ For resolution of grid maps, bound min And bound max The boundary range of the original point cloud in the three-dimensional space is defined;
and judging the number of points falling into each grid according to the subscript of the grid corresponding to each point, setting the value of the grid to be 1 if the number of the points falling into one grid is larger than a preset threshold, and setting the value of the grid to be 0 if the number of the points falling into the grid is not larger than the preset threshold, so as to complete reassignment of the grid, thereby generating the occupied grid map.
4. The method of claim 1, wherein step (3) comprises:
constructing an occupation grid map for the filtered point cloud Ms in the same manner as the step (2);
traversing the occupied grids in each filtered point cloud Ms, judging whether the grids above each occupied grid are non-occupied grids, and if so, defining the non-occupied grids in the front-end path search space, thereby completing the construction of the front-end path search space.
5. The method of claim 1, wherein the smoothness penalty function J is e For making the trajectory as smooth as possible, ensuring energy optimality:
where p (t) is the trajectory equation and M is the number of trajectory segments.
6. The method of claim 1, wherein the total temporal penalty function J t For making the total time of the trajectory as short as possible:
wherein T is i The total duration of the ith track is shown, and M is the number of track segments.
7. The method of claim 1, wherein the kinetic penalty function J d For enabling the desired speed and the desired acceleration of the robot at any time on the trajectory to satisfy the dynamic limits of the robot itself:
wherein C (= max {, 0} 3 In the form of a cubic penalty function,
in order to be able to take the desired speed,
for desired acceleration, M is the number of track segments, v max And a max Maximum speed limit and maximum acceleration limit, k, respectively i The number of samples of the ith section of track is as follows:
wherein β is a polynomial locus p (t) = c T β (t) and c is a polynomial coefficient.
8. The method of claim 1, wherein the security penalty function J c For keeping the track away from the dangerous area and not generating collision, the construction process of the safety penalty function is as follows:
constructing an ESDF map on an occupied grid map constructed by the original point cloud, selecting a proper truncation distance, and generating a TSDF map:
wherein d is thr Is a safety distance threshold, d (p) is a TSDF function value, which is used for punishing distance obstacles on the trackThe point at which the object is too close,
an ith sampling point of a jth track is taken;
constructing a collision-free penalty function J based on the TSDF map ct ：
According to the pose transmission function of the robot and the analysis of an extra dangerous area brought by the terrain, a terrain penalty function is constructed:
wherein the pose transmission function of the robot
The coordinate of the center of the robot chassis at the space position p is represented as x 0 ,y 0 ,z 0 The inclination angle of the robot is
λ sp And λ se Is a weight coefficient;
according to the TSDF map and the terrain penalty function, constructing a security penalty function:
9. a robot navigation planning device in a non-flat ground environment, comprising:
the filtering module is used for acquiring the original point cloud and filtering the passable area of the original point cloud;
the generating module is used for carrying out down-sampling on the original point cloud to generate an occupation grid map;
the construction module is used for carrying out down-sampling on the filtered point cloud and constructing a front-end path search space;
the path searching module is used for carrying out graph searching based on A in the front-end path searching space generated in the construction module to obtain a front-end path;
and the path optimization module is used for generating a track initial value based on the front-end path obtained by the path search module, constructing a penalty function according to the occupation grid map, and generating a final track by using a convex optimization algorithm, wherein the penalty function comprises a smoothness penalty function for smoothing the track, a total time penalty function for controlling the total duration of the track, a dynamics penalty function for enabling the robot to expect the speed and the acceleration at any moment on the track to meet the dynamics limitation of the robot, and a safety penalty function for enabling the track to avoid a dangerous area and not generate a collision.
10. A robot, comprising:
one or more processors;
a memory for storing one or more programs;
when executed by the one or more processors, cause the one or more processors to implement the method of any one of claims 1-8.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202211472175.3A CN115752474A (en) | 2022-11-23 | 2022-11-23 | Robot navigation planning method and device under non-flat ground environment and robot |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202211472175.3A CN115752474A (en) | 2022-11-23 | 2022-11-23 | Robot navigation planning method and device under non-flat ground environment and robot |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN115752474A true CN115752474A (en) | 2023-03-07 |
Family
ID=85335760
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202211472175.3A Pending CN115752474A (en) | 2022-11-23 | 2022-11-23 | Robot navigation planning method and device under non-flat ground environment and robot |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN115752474A (en) |
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116147637A (en) * | 2023-04-24 | 2023-05-23 | 福勤智能科技（昆山）有限公司 | Method, device, equipment and storage medium for generating occupied grid map |
| CN117685994A (en) * | 2024-02-04 | 2024-03-12 | 北京航空航天大学 | Unmanned vehicle path planning method for air-ground coordination |
-
2022
- 2022-11-23 CN CN202211472175.3A patent/CN115752474A/en active Pending
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116147637A (en) * | 2023-04-24 | 2023-05-23 | 福勤智能科技（昆山）有限公司 | Method, device, equipment and storage medium for generating occupied grid map |
| CN117685994A (en) * | 2024-02-04 | 2024-03-12 | 北京航空航天大学 | Unmanned vehicle path planning method for air-ground coordination |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN108763287B (en) | Construction method of large-scale passable regional driving map and unmanned application method thereof | |
| EP3635500B1 (en) | Method of navigating a vehicle and system thereof | |
| CN109059944B (en) | Motion planning method based on driving habit learning | |
| CN115752474A (en) | Robot navigation planning method and device under non-flat ground environment and robot | |
| CN111694356B (en) | Driving control method and device, electronic equipment and storage medium | |
| CN106599108A (en) | Method for constructing multi-mode environmental map in three-dimensional environment | |
| US10286901B2 (en) | Map of the surroundings for driving areas with random altitude profile | |
| CN113110455B (en) | Multi-robot collaborative exploration method, device and system for unknown initial state | |
| EP3931657B1 (en) | System and method for surface feature detection and traversal | |
| WO2022115215A1 (en) | Systems and methods for monocular based object detection | |
| CN113238251A (en) | Target-level semantic positioning method based on vehicle-mounted laser radar | |
| CN115639823A (en) | Terrain sensing and movement control method and system for robot under rugged and undulating terrain | |
| CN111046846A (en) | Method for judging passing ability of obstacle in front of robot | |
| CN113515111B (en) | Vehicle obstacle avoidance path planning method and device | |
| CN116136417B (en) | Unmanned vehicle local path planning method facing off-road environment | |
| CN117289301A (en) | Air-ground unmanned platform collaborative path planning method under unknown off-road scene | |
| CN116338729A (en) | Three-dimensional laser radar navigation method based on multilayer map | |
| US20230056589A1 (en) | Systems and methods for generating multilevel occupancy and occlusion grids for controlling navigation of vehicles | |
| CN116279424A (en) | Parking path planning method, device, equipment and storage medium | |
| CN112214037B (en) | Unmanned aerial vehicle remote sensing networking flight path planning method based on field station | |
| CN112734811B (en) | Obstacle tracking method, obstacle tracking device and chip | |
| CN113671511A (en) | Laser radar high-precision positioning method for regional scene | |
| CN116147653B (en) | Three-dimensional reference path planning method for unmanned vehicle | |
| Zhan et al. | ROS-based Mobile Robot for Pavement Defect Detection | |
| CN114440855B (en) | Method and system for positioning and map updating in dynamic scene |
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 |