Robot navigation planning method, device and robot in uneven ground environment

Abstract

The present invention discloses a robot navigation planning method, device and robot in a non-flat ground environment. The method comprises: obtaining an original point cloud, filtering the original point cloud for a traversable area; downsampling the original point cloud to generate an occupancy grid map; downsampling the filtered point cloud to construct a front-end path search space; performing an A*-based graph search in the generated front-end path search space to obtain a front-end path; generating an initial value of a trajectory based on the obtained front-end path, constructing a penalty function according to the occupancy grid map, and generating a final trajectory using a convex optimization algorithm, wherein the penalty function comprises a smoothness penalty function for smoothing the trajectory, a total time penalty function for controlling the total duration of the trajectory, a dynamics penalty function for ensuring that the expected speed and expected acceleration of the robot at any time on the trajectory meet the dynamics constraints of the robot itself, and a safety penalty function for ensuring that the trajectory avoids dangerous areas and does not cause collisions.

Description

Technical Field

[0001] The present invention belongs to the technical field of autonomous unmanned systems, and in particular relates to a robot navigation planning method, device, and robot in a non-flat ground environment. Background Art

[0002] Autonomous navigation is crucial for the safe and efficient operation of autonomous vehicles in many challenging application scenarios, such as search and rescue, exploration, inspection, or transportation. It allows autonomous vehicles to perform or assist in tasks that are repetitive, tiring, or even dangerous for humans. In addition to localization and mapping, trajectory generation is a key function of any autonomous navigation system.

[0003] Unmanned vehicle trajectory generation technology involves automatically planning a safe trajectory from the current point to the target point based on the vehicle's sensor-based environmental perception. Trajectory generation for ground robots primarily addresses three key challenges: 1) connectivity, meaning the trajectory must connect the starting and ending points; 2) executability, meaning the trajectory must meet the robot's constraints, such as minimum turning radius (maximum curvature), maximum speed, and maximum acceleration; and 3) optimality, meaning the trajectory length and energy consumption must be optimized while achieving the aforementioned tasks.

[0004] For computational convenience, existing algorithms mostly perform navigation planning in a two-dimensional environment. However, the actual working environment of ground robots cannot be simply described in two dimensions. For example, in the wild (rough lawns, mountain roads full of dirt mounds) and in man-made buildings with multi-story structures, existing technologies often only divide the environment into obstacles and traversable areas in a two-dimensional plane. For example, using a height threshold dichotomy method does not consider the impact of terrain on the unmanned vehicle and omits a large amount of actual traversable ground. Therefore, the calculated trajectory cannot guarantee either feasibility or optimality under certain criteria (such as trajectory length and energy consumption of the robot). In fact, it is even impossible to plan a reasonable trajectory in complex environments.

[0005] New technologies allow for direct vehicle navigation planning on 3D point clouds. However, direct calculations on 3D point clouds typically require online KNN nearest neighbor matching of the point clouds to fit local terrain features, requiring significant computational effort, making real-time calculations impossible. Furthermore, algorithms based on local fitting cannot capture the characteristics of point clouds within a larger area and are not robust enough in larger, more complex environments. More importantly, the trajectories generated by direct navigation planning on point clouds are limited by the discrete nature of point cloud information and often appear as broken-line paths without temporal information. This results in poorly executed trajectories and high energy consumption. Summary of the Invention

[0006] The purpose of the embodiments of the present application is to provide a robot navigation planning method, device and robot in a non-flat ground environment, so as to solve the problem of inaccurate environment representation for two-dimensional plane planning in related technologies, and the problems of large computational complexity, poor real-time performance, poor trajectory smoothness, lack of time information and high energy consumption in direct navigation on point clouds. By utilizing structured grid maps and an optimization-based trajectory method, and by constructing special safety constraints that meet the needs of ground robots, smooth and collision-free energy-optimal trajectory planning for ground robots in non-flat ground environments is achieved.

[0007] According to a first aspect of an embodiment of the present application, a robot navigation planning method in an uneven ground environment is provided, comprising:

[0008] Step (1): obtaining an original point cloud and filtering the original point cloud for a passable area;

[0009] Step (2): downsample the original point cloud to generate an occupancy grid map;

[0010] Step (3): downsample the filtered point cloud and construct the front-end path search space;

[0011] Step (4): Perform an A*-based graph search in the front-end path search space generated in step (3) to obtain the front-end path;

[0012] Step (5): Generate an initial value of the trajectory based on the front-end path obtained in step (4), construct a penalty function according to the occupied grid map, and generate a final trajectory using a convex optimization algorithm, wherein the penalty function includes a smoothness penalty function for smoothing the trajectory, a total time penalty function for controlling the total duration of the trajectory, a dynamics penalty function for ensuring that the expected speed and expected acceleration of the robot at any time on the trajectory meet the dynamics limitations of the robot itself, and a safety penalty function for ensuring that the trajectory avoids dangerous areas and does not cause collisions.

[0013] Furthermore, the step (1) includes:

[0014] (1.1) Traverse each point m in the original point cloud and search for a point in the area with a radius of r near the point m;

[0015] (1.2) Traverse each point d in the set Dr of neighboring points and find the height difference z between point d and point m dm And the angle α between the connecting line and the horizontal plane is calculated as follows:

[0016] z dm =d3-m3

[0017]

[0018] Where d3 and m3 refer to the z-axis components of the d and m coordinates, respectively;

[0019] (1.3) Determine whether point m meets the passable area condition, where the judgment condition is: for any point d in Dr, there is no z dm <0 and α<θ, if the condition is met, point m is in the passable area Ms, otherwise point m is filtered out, where θ is the maximum safe standing tilt angle of the robot.

[0020] Furthermore, the step (2) includes:

[0021] Calculate the grid subscript corresponding to each point in the original point cloud:

[0022]

[0023] Among them, the floor function means rounding down, res occ is the resolution of the raster map, bound min and bound max is the boundary range of the original point cloud in three-dimensional space;

[0024] According to the grid subscript corresponding to each point, the number of points falling into each grid is determined. If the number of points falling into a grid is greater than a predetermined threshold, the value of the grid is set to 1, otherwise it is set to 0 to complete the reassignment of the grid and generate an occupied grid map.

[0025] Furthermore, the step (3) includes:

[0026] Using the same method as step (2), construct an occupancy grid map for the filtered point cloud Ms;

[0027] Traverse the occupied grids in each filtered point cloud Ms and determine whether the grid above each occupied grid is an unoccupied grid. If so, the unoccupied grid is included in the front-end path search space, thereby completing the construction of the front-end path search space.

[0028] Furthermore, the smoothness penalty function J e Used to make the trajectory as smooth as possible and ensure energy optimality:

[0029]

[0030] Where p(t) is the trajectory equation and M is the number of trajectory segments.

[0031] Furthermore, the total time penalty function J t To make the total time of the trajectory as short as possible:

[0032]

[0033] Where T i is the total duration of the i-th trajectory, and M is the number of trajectory segments.

[0034] Furthermore, the dynamic penalty function J d Used to ensure that the robot's expected velocity and acceleration at any time on the trajectory meet the robot's own dynamic constraints:

[0035]

[0036] where C(.) = max{.,0} 3 is the cubic penalty function, is the expected speed, is the expected acceleration, M is the number of trajectory segments, v max and a max are the maximum speed limit and the maximum acceleration limit, k i is the number of samples of the i-th trajectory:

[0037]

[0038]

[0039] Where β is the polynomial trajectory p(t)=c T The basis of β(t), c is the polynomial coefficient.

[0040] Furthermore, the security penalty function J c The safety penalty function is used to keep the trajectory away from the dangerous area and avoid collision. The construction process of the safety penalty function is as follows:

[0041] Construct an ESDF map on the occupancy grid map constructed from the original point cloud, and select a suitable cutoff distance to generate a TSDF map:

[0042]

[0043] where d thr is the safety distance threshold, d(p) is the TSDF function value, which is used to punish points on the trajectory that are too close to obstacles. is the i-th sampling point of the j-th trajectory;

[0044] Based on the TSDF map, a collision-free penalty function J is constructed. ct :

[0045]

[0046] According to the robot's posture transmission function and the analysis of the additional dangerous areas brought by the terrain, a terrain penalty function is constructed:

[0047]

[0048] Among them, the robot's posture transmission function Indicates that at the spatial position p, the center coordinates of the robot chassis are x0, y0, z0, and the robot tilt angle is λ sp and λ se is the weight coefficient;

[0049] According to the TSDF map and terrain penalty function, a security penalty function is constructed:

[0050]

[0051] According to a second aspect of an embodiment of the present application, a robot navigation planning device in an uneven ground environment is provided, comprising:

[0052] A filtering module is used to obtain an original point cloud and filter the original point cloud into a passable area;

[0053] The generation module is used to downsample the original point cloud and generate an occupancy grid map;

[0054] A construction module is used to downsample the filtered point cloud and construct the front-end path search space;

[0055] A path search module is used to perform an A*-based graph search in the front-end path search space generated in the construction module to obtain the front-end path;

[0056] The path optimization module is used to generate an initial value of the 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 using a convex optimization algorithm, wherein the penalty function includes a smoothness penalty function for smoothing the trajectory, a total time penalty function for controlling the total duration of the trajectory, a dynamics penalty function for ensuring that the expected speed and expected acceleration of the robot at any time on the trajectory meet the dynamic constraints of the robot itself, and a safety penalty function for ensuring that the trajectory avoids dangerous areas and does not cause collisions.

[0057] According to a third aspect of the embodiments of the present application, there is provided a robot, comprising:

[0058] one or more processors;

[0059] a memory for storing one or more programs;

[0060] When the one or more programs are executed by the one or more processors, the one or more processors implement the method as described in the first aspect.

[0061] The technical solutions provided by the embodiments of the present application may have the following beneficial effects:

[0062] (1) The trajectory generated by the present invention is safe and smooth, has energy optimality and good execution, and meets the motion laws of ground robots.

[0063] (2) The present invention directly uses the original point cloud to generate a three-dimensional map representation, which retains the information of the traversable area to the greatest extent and avoids the compression and discarding of environmental information by traditional two-dimensional algorithms.

[0064] (3) After obtaining the initial trajectory, the trajectory generated by the present invention is globally optimized and locally optimized to ensure the optimality of the obtained trajectory.

[0065] (4) Trajectory generation can use local point cloud maps and can be used when the global environment is unknown.

[0066] It should be understood that the foregoing general description and the following detailed description are exemplary and explanatory only and are not restrictive of the present application. BRIEF DESCRIPTION OF THE DRAWINGS

[0067] 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 present application.

[0068] Figure 1 It is an overall framework diagram of a robot navigation planning method in a non-flat ground environment according to an exemplary embodiment.

[0069] Figure 2 The figure is a schematic diagram of filtering the traversable area of ​​a point cloud according to an exemplary embodiment.

[0070] Figure 3 The figure is a rendering of terrain hazard analysis according to an exemplary embodiment.

[0071] Figure 4 It is a schematic diagram showing a front-end search effect according to an exemplary embodiment.

[0072] Figure 5 is a schematic diagram showing a final trajectory effect according to an exemplary embodiment.

[0073] Figure 6 The present invention is a block diagram of a robot navigation planning device in a non-flat ground environment according to an exemplary embodiment.

[0074] Figure 7 is a schematic diagram of an electronic device according to an exemplary embodiment. DETAILED DESCRIPTION

[0075] Exemplary embodiments are described in detail herein, with examples illustrated in the accompanying drawings. When the following description refers to the drawings, identical numerals in different figures represent identical or similar elements unless otherwise indicated. The embodiments described in the following exemplary embodiments are not intended to represent all embodiments consistent with this application.

[0076] The terms used in this application are for the purpose of describing specific embodiments only and are not intended to limit this application. As used in this application and the appended claims, the singular forms "a," "an," "the," and "the" are intended to include the plural forms, 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.

[0077] It should be understood that although the terms first, second, third, etc. may be used in this application to describe various information, such information should not be limited to these terms. These terms are only used to distinguish information of the same type from each other. For example, without departing from the scope of this application, first information may also be referred to as second information, and similarly, second information may also be referred to as first information. Depending on the context, the word "if" as used herein may be interpreted as "at the time of" or "when" or "in response to determining".

[0078] Figure 1 FIG. 1 is a flow chart showing a method for ground robot navigation planning in a non-flat ground environment according to an exemplary embodiment. Figure 1 As shown, the method is applied in a terminal and may include the following steps:

[0079] Step (1): obtaining an original point cloud and filtering the original point cloud for a passable area;

[0080] Step (2): downsample the original point cloud to generate an occupancy grid map;

[0081] Step (3): downsample the filtered point cloud and construct the front-end path search space;

[0082] Step (4): Perform an A*-based graph search in the front-end path search space generated in step (3) to obtain the front-end path;

[0083] Step (5): Generate an initial value of the trajectory based on the front-end path obtained in step (4), construct a penalty function according to the occupied grid map, and generate a final trajectory using a convex optimization algorithm, wherein the penalty function includes a smoothness penalty function for smoothing the trajectory, a total time penalty function for controlling the total duration of the trajectory, a dynamics penalty function for ensuring that the expected speed and expected acceleration of the robot at any time on the trajectory meet the dynamics limitations of the robot itself, and a safety penalty function for ensuring that the trajectory avoids dangerous areas and does not cause collisions.

[0084] It can be seen from the above embodiments that the trajectory generated by the present application is safe and smooth, has energy optimality and good executability, and meets the motion laws of ground robots; the original point cloud is directly used to generate a map representation of the three-dimensional space, which retains the information of the traversable area to the greatest extent and avoids the compression and discarding of environmental information by traditional two-dimensional algorithms; after obtaining the initial trajectory, the generated trajectory is globally and locally optimized to ensure the optimality of the obtained trajectory; the trajectory generation can use a local point cloud map, which can be used when the global environment is unknown.

[0085] In the specific implementation of step (1), assuming that the set of points in the original point cloud is Mo and the set of filtered points is Ms, the specific steps of filtering the passable area of ​​the point cloud are as follows:

[0086] (1.1) Traverse each point m in the original point cloud and search for neighborhood points with a radius of r near the point m. Assume that the set of neighborhood points is Dr.

[0087] (1.2) Traverse each point d in Dr and find the height difference between it and point m, as well as the angle between the connecting line and the horizontal plane. The calculation method is as follows:

[0088] z dm =d3-m3

[0089]

[0090] Where d3 and m3 refer to the z-axis components of the d and m coordinates, respectively.

[0091] (1.3) Determine whether point m meets the passable area condition, which is described as: for any point d in Dr, there is no z dm < 0 and α < θ. If this condition is met, point m is within the traversable area Ms; otherwise, point m is filtered out. θ is the robot's maximum safe standing tilt angle, determined based on the robot's actual performance.

[0092] Based on the above three steps, we use the original point cloud to filter out the point cloud of the robot's traversable area and filter out the point cloud belonging to obstacles, which plays a crucial role in the subsequent front-end path search.

[0093] In one embodiment, Figure 2 This shows the point cloud traversable area filtering in this step. Figure 2 Point A in (a) of the figure satisfies the passability requirement and should be retained. Points B, C, and D do not meet the passability requirement because the angle between the red line and the horizontal plane exceeds the threshold. Figure 2 Figure (b) shows the judgment of each point after the traversal is completed. Figure 2 Figure (c) shows the points in Ms after filtering.

[0094] In the specific implementation of step (2), first specify the resolution of the grid map res occ Then traverse the points in the original point cloud to determine the boundary range of the original point cloud in three-dimensional space min and bound max , the cuboid constructed with these two points as the diagonal will completely enclose the original point cloud. Then traverse the point cloud again to determine the number of points that fall into each grid. The grid subscript corresponding to each point is calculated as follows:

[0095]

[0096] The floor function rounds down to the nearest integer. After traversing the point cloud, all grid cells are traversed and the grid values ​​are reassigned: if the number of points falling within a grid cell exceeds a threshold, the grid value is set to 1; otherwise, it is set to 0. Typically, for point cloud maps directly generated by SLAM algorithms, the threshold is often greater than 1. For point cloud maps that have undergone preliminary downsampling, the threshold can be set to 1. Once this step is completed, the grid map is generated.

[0097] In the specific implementation of step (3), the same method as step (2) is used to construct an occupied grid map for the filtered point cloud Ms. Since Ms is a subset of Mo, there is no need to apply for additional memory space to store the occupied grid maps of Mo and Ms respectively. Then, each occupied grid in Ms is traversed to determine whether the grid above it is a non-occupied grid. If so, this non-occupied grid is defined in the front-end path search space. After completing the traversal of the occupied grids of Ms, the construction of the front-end path search space is completed. It is not difficult to see that the front-end path search space is a layer of space close to the upper surface of the object, which can ensure that the front-end path basically conforms to the motion law of the ground robot and provides a good initial value for subsequent trajectory optimization.

[0098] In the specific implementation of step (4), the A*-based graph search is used. First, the starting point and the end point of the search are determined. Assume that the robot's current position is p and the end point position is p e In order to ensure that the starting point and the end point are both within the front-end path search space, it is necessary to e Perform a downward projection, that is, determine p and p e If the grid ID is not within the front-end path search space, the algorithm moves down one grid and repeats the search until the grid is within the front-end path search space. After the projection of the start and end points is completed, an A* search is performed. A* is a mature path search algorithm, and the specific process is not detailed here.

[0099] In the specific implementation of step (5), the trajectory representation adopts the representation of MINCO trajectory class, which is specifically defined as follows:

[0100]

[0101] This trajectory class uses parameters q and T to represent a polynomial, meaning: a polynomial trajectory that passes through the intermediate point q and has a duration of T that satisfies energy optimality. c is the polynomial coefficient, and M is the mapping from q and T parameters to c parameters.

[0102] The commonly used polynomial representation method is to use the coefficient c to represent the polynomial:

[0103] p(t)=c T β(t)

[0104] β is the basis of the polynomial. There is a mapping between the MINCO representation and the coefficient c representation: c = M(q, T). Using this mapping, the penalty function J(c, T) constructed on the polynomial can be easily converted to the penalty function constructed on the MINCO trajectory: H(q, T) = J(M(q, T), T), and the partial derivatives can also be quickly calculated.

[0105] The penalty functions for trajectory optimization include smoothness penalty function, total time penalty function, dynamics penalty function, and safety penalty function. The form of the optimization problem is:

[0106] min c,T λ e J e +λ t J t +λ d J d +λ s J s

[0107] Where λ is the corresponding weight coefficient, which should be set based on experience in specific implementation.

[0108] (I) The smoothness penalty function is used to make the trajectory as smooth as possible to ensure energy optimality:

[0109]

[0110] Where p(t) is the trajectory equation and M is the number of trajectory segments.

[0111] (II) The total time penalty function makes the total time of the trajectory as short as possible:

[0112]

[0113] Where T i is the total duration of the i-th trajectory, and M is the number of trajectory segments.

[0114] (III) The dynamic penalty function ensures that the expected robot velocity and acceleration at any time on the trajectory meet the robot's own dynamic constraints. Since the trajectory has time information, the expected velocity and acceleration at a point on the trajectory can be calculated as follows:

[0115]

[0116]

[0117] The dynamic penalty function is as follows:

[0118]

[0119] where C(.) = max{.,0} 3 is a cubic penalty function, M is the number of trajectory segments, v max and a max are the maximum speed limit and the maximum acceleration limit, k i is the number of samples of the i-th trajectory (using discrete numerical sampling to approximate the numerical integral).

[0120] (IV) The safety penalty function ensures that the trajectory avoids dangerous areas and does not cause collisions. For ground robots, in addition to considering objects as obstacles, they also need to consider the additional dangerous areas caused by the terrain. Since ground robots cannot fly and have limited balance capabilities, the additional dangerous areas caused by the terrain include the following two types: 1. The ground is too sloped, so if the robot stands on it, it will capsize; 2. Steep edges, so if the robot moves on the edge, it may fall or its posture will change drastically. Therefore, the safety penalty function needs to consider all of the above dangerous situations.

[0121] First, we need to make the trajectory away from the object. To achieve this, we construct an ESDF (Euclidean Signed Distance Functions) map on the occupancy grid map constructed from the original point cloud, and select a suitable truncation distance to generate a TSDF (Truncated Signed Distance Function) map:

[0122]

[0123] where d thr is the safety distance threshold, d(p) is the TSDF function value, is the i-th sampling point of the j-th trajectory;

[0124] Based on the TSDF map, a collision-free penalty function J is constructed. ct :

[0125]

[0126] In addition, in order to determine the dangerous areas of terrain, terrain analysis is performed on each location of the original point cloud: the RANSAC algorithm is used to calculate the robot's projected pose at each location. The so-called projected pose refers to the robot's pose projected downward from a certain point in space to the nearest surface. This can calculate the terrain penalty function in the entire three-dimensional space, which is the same as the dimension of the trajectory. Since ground robots cannot fly, their motion space is a subset of the entire three-dimensional space. Therefore, the above algorithm can completely cover the robot's motion space.

[0127] Assume that the projection pose function of the robot is:

[0128]

[0129] It means that at the spatial position p, the projected pose of the robot is Among them, x0, y0, z0 represent the center coordinates of the robot chassis. Represents the robot's tilt angle. After obtaining the pose projection function, the terrain penalty function can be constructed based on the analysis of the additional dangerous areas brought by the terrain:

[0130]

[0131] The first term of this function keeps the trajectory away from areas that are too inclined, while the second term keeps the trajectory away from steep edges and areas that cause drastic changes in the robot's posture. λ is a weight coefficient, which is determined according to the robot's needs and experience in specific implementations.

[0132] Finally, the security penalty function is as follows:

[0133]

[0134] After constructing the penalty function, we use a numerical optimizer to optimize the trajectory parameters. Here, we choose the LBFGS algorithm. After completing the numerical optimization, we can obtain the final trajectory.

[0135] Figure 3 The results of the hazard analysis in a certain scenario are shown. It can be seen that on the slope without guardrails, the algorithm automatically analyzes the dangerous edge area.

[0136] Figure 4 The front-end search effect is shown. It can be seen that the front-end is almost close to the ground, which can provide a good initial value for back-end optimization.

[0137] Figure 5 The effect of the final trajectory planning is demonstrated. Thanks to the back-end safety constraints and the good initial values ​​of the front-end, the final trajectory is a smooth, safe and easy-to-execute ground robot trajectory.

[0138] Corresponding to the aforementioned embodiment of the robot navigation planning method in a non-flat ground environment, the present application also provides an embodiment of a robot navigation planning device in a non-flat ground environment.

[0139] Figure 6 The present invention is a block diagram of a robot navigation planning device in a non-flat ground environment according to an exemplary embodiment.

[0140] Reference Figure 6 , the apparatus may include:

[0141] The filtering module 21 is used to obtain the original point cloud and filter the original point cloud into a passable area;

[0142] A generation module 22 is used to downsample the original point cloud to generate an occupancy grid map;

[0143] Construction module 23, used to downsample the filtered point cloud and construct a front-end path search space;

[0144] A path search module 24 is configured to perform an A*-based graph search in the front-end path search space generated in the construction module to obtain a front-end path;

[0145] The path optimization module 25 is used to generate an initial value of the trajectory based on the front-end path obtained by the path search module, construct a penalty function according to the occupied grid map, and generate a final trajectory using a convex optimization algorithm, wherein the penalty function includes a smoothness penalty function for smoothing the trajectory, a total time penalty function for controlling the total duration of the trajectory, a dynamics penalty function for ensuring that the expected speed and expected acceleration of the robot at any time on the trajectory meet the dynamic constraints of the robot itself, and a safety penalty function for ensuring that the trajectory avoids dangerous areas and does not cause collisions.

[0146] Regarding the apparatus in the above embodiment, the specific manner in which each module performs operations has been described in detail in the embodiment of the method, and will not be elaborated here.

[0147] For the device embodiments, since they basically correspond to the method embodiments, the relevant parts can be referred to the partial description of the method embodiments. The device embodiments described above are merely schematic, wherein the units described as separate components may or may not be physically separated, and the components displayed as units may or may not be physical units, that is, they may be located in one place, or they may be distributed on multiple network units. Some or all of the modules may be selected according to actual needs to achieve the purpose of the present application scheme. A person of ordinary skill in the art can understand and implement it without paying any creative work.

[0148] Accordingly, the present application also provides a robot, comprising: one or more processors; a memory for storing one or more programs; when the one or more programs are executed by the one or more processors, the one or more processors implement the robot navigation planning method in a non-flat ground environment as described above. Figure 7 As shown in FIG. 1 , a hardware structure diagram of any device with data processing capability in a robot navigation planning method in a non-flat ground environment provided by an embodiment of the present invention is shown. Figure 7 In addition to the processor, memory, and network interface shown, any device with data processing capabilities in which the apparatus in the embodiment is located may also include other hardware, generally based on the actual functions of the device with data processing capabilities, which will not be described in detail.

[0149] Accordingly, the present application also provides a computer-readable storage medium having computer instructions stored thereon, which, when executed by a processor, implement the robot navigation planning method in a non-flat ground environment as described above. The computer-readable storage medium can be an internal storage unit of any device with data processing capabilities described in any of the aforementioned embodiments, such as a hard disk or memory. The computer-readable storage medium can also be an external storage device, such as a plug-in hard disk, a smart memory card (Smart Media Card, SMC), an SD card, a flash card (Flash Card), etc. equipped on the device. Furthermore, the computer-readable storage medium can also include both an internal storage unit and an external storage device of any device with data processing capabilities. The computer-readable storage medium is used to store the computer program and other programs and data required by any device with data processing capabilities, and can also be used to temporarily store data that has been output or is to be output.

[0150] Those skilled in the art will readily conceive of other embodiments of the present application after considering the specification and practicing the contents disclosed herein. This application is intended to cover any variations, uses, or adaptations of the present application that follow the general principles of this application and include common knowledge or customary techniques in the art that are not disclosed in this application.

[0151] It will be understood that the present application is not limited to the exact construction that has been described above and shown in the drawings, and that various modifications and changes may be made without departing from the scope thereof.

Claims

1. A robot navigation planning method in a non-flat ground environment, characterized in that: include: Step (1): obtaining an original point cloud and filtering the original point cloud for a passable area; Step (2): downsample the original point cloud to generate an occupancy grid map; Step (3): downsample the filtered point cloud and construct the front-end path search space; Step (4): Perform an A*-based graph search in the front-end path search space generated in step (3) to obtain the front-end path; Step (5): Generate an initial value of the trajectory based on the front-end path obtained in step (4), construct a penalty function according to the occupied grid map, and generate a final trajectory using a convex optimization algorithm, wherein the penalty function includes a smoothness penalty function for smoothing the trajectory, a total time penalty function for controlling the total duration of the trajectory, a dynamics penalty function for ensuring that the expected speed and expected acceleration of the robot at any time on the trajectory meet the dynamic constraints of the robot itself, and a safety penalty function for ensuring that the trajectory avoids dangerous areas and does not cause collisions; Wherein, the step (1) comprises: (1.1) Traverse each point m in the original point cloud and search for a point in the area with a radius of r near the point m; (1.2) Traverse each point d in the set Dr of neighboring points and find the height difference z between point d and point m dm And the angle α between the connecting line and the horizontal plane is calculated as follows: z dm =d3-m3 Where d3 and m3 refer to the z-axis components of the d and m coordinates, respectively; (1.3) Determine whether point m meets the passable area condition, where the judgment condition is: for any point d in Dr, there is no z dm <0 and α<θ, if the condition is met, the point m is in the traversable area Ms, otherwise the point m is filtered out, where θ is the maximum safe standing tilt angle of the robot.

2. The method according to claim 1, characterized in that The step (2) comprises: Calculate the grid subscript corresponding to each point in the original point cloud: Among them, the floor function means rounding down, res occ is the resolution of the raster map, bound min and bound max is the boundary range of the original point cloud in three-dimensional space; According to the grid subscript corresponding to each point, the number of points falling into each grid is determined. If the number of points falling into a grid is greater than a predetermined threshold, the value of the grid is set to 1, otherwise it is set to 0 to complete the reassignment of the grid and generate an occupied grid map.

3. The method according to claim 1, characterized in that The step (3) comprises: Using the same method as step (2), construct an occupancy grid map for the filtered point cloud Ms; Traverse the occupied grids in each filtered point cloud Ms and determine whether the grid above each occupied grid is an unoccupied grid. If so, the unoccupied grid is included in the front-end path search space, thereby completing the construction of the front-end path search space.

4. The method according to claim 1, wherein The smoothness penalty function J e Used to make the trajectory as smooth as possible and ensure energy optimality: Where p(t) is the trajectory equation and M is the number of trajectory segments.

5. The method according to claim 1, characterized in that The total time penalty function J t To make the total time of the trajectory as short as possible: Where T i is the total duration of the i-th trajectory, and M is the number of trajectory segments.

6. The method according to claim 1, characterized in that The dynamic penalty function J d Used to ensure that the robot's expected velocity and acceleration at any time on the trajectory meet the robot's own dynamic constraints: where C(.) = max{.,0} 3 is the cubic penalty function, is the expected speed, is the expected acceleration, M is the number of trajectory segments, v max and a max are the maximum speed limit and the maximum acceleration limit, k i is the number of samples of the i-th trajectory: Where β is the polynomial trajectory p(t)=c T The basis of β(t), c is the polynomial coefficient.

7. The method according to claim 1, characterized in that The security penalty function J c The safety penalty function is used to keep the trajectory away from the dangerous area and avoid collision. The construction process of the safety penalty function is as follows: Construct an ESDF map on the occupancy grid map constructed from the original point cloud, and select a suitable cutoff distance to generate a TSDF map: where d thr is the safety distance threshold, d(p) is the TSDF function value, which is used to punish points on the trajectory that are too close to obstacles. is the i-th sampling point of the j-th trajectory; Based on the TSDF map, a collision-free penalty function J is constructed. ct : According to the robot's posture transmission function and the analysis of the additional dangerous areas brought by the terrain, a terrain penalty function is constructed: Among them, the robot's posture transmission function Indicates that at the spatial position p, the center coordinates of the robot chassis are x0, y0, z0, and the robot tilt angle is λ sp and λ se is the weight coefficient; According to the TSDF map and terrain penalty function, a security penalty function is constructed:

8. A robot navigation planning device in a non-flat ground environment, characterized in that: include: A filtering module is used to obtain an original point cloud and filter the original point cloud into a passable area; The generation module is used to downsample the original point cloud and generate an occupancy grid map; A construction module is used to downsample the filtered point cloud and construct the front-end path search space; The path search module is used to perform A*-based graph search in the front-end path search space generated in the construction module to obtain the front-end path; a path optimization module, configured to generate an initial trajectory value 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 using a convex optimization algorithm, wherein the penalty function includes a smoothness penalty function for smoothing the trajectory, a total time penalty function for controlling the total duration of the trajectory, a dynamics penalty function for ensuring that the robot's expected speed and expected acceleration at any time on the trajectory meet the robot's own dynamic constraints, and a safety penalty function for ensuring that the trajectory avoids dangerous areas and does not cause collisions; The processing of the filtering module includes: (1.1) Traverse each point m in the original point cloud and search for a point in the area with a radius of r near the point m; (1.2) Traverse each point d in the set Dr of neighboring points and find the height difference z between point d and point m dm And the angle α between the connecting line and the horizontal plane is calculated as follows: z dm =d3-m3 Where d3 and m3 refer to the z-axis components of the d and m coordinates, respectively; (1.3) Determine whether point m meets the passable area condition, where the judgment condition is: for any point d in Dr, there is no z dm <0 and α<θ, if the condition is met, the point m is in the traversable area Ms, otherwise the point m is filtered out, where θ is the maximum safe standing tilt angle of the robot.

9. A robot, characterized in that: include: one or more processors; a memory for storing one or more programs; When the one or more programs are executed by the one or more processors, the one or more processors implement the method according to any one of claims 1 to 7.

Images