A robot navigation method, apparatus, device and storage medium

Abstract

The application discloses a robot navigation method and device, equipment and storage medium, relates to the technical field of robot autonomous navigation and path planning, and comprises the following steps: in response to a navigation instruction, acquiring hierarchical scene map data, dividing a navigation task into a plurality of continuous navigation subtasks, planning a path for each navigation subtask, determining a terrain feature distribution and a predicted path of a dynamic obstacle based on point cloud data collected by a robot during the planning process, constructing a terrain cost distribution map, constructing an objective function according to a traffic cost value of a robot passing through a to-be-determined path, and determining a speed parameter and a navigation path at a current time by minimizing the objective function; and executing the navigation subtask until the robot reaches the target position. By constructing the terrain cost distribution map that fuses the point cloud data, the application effectively overcomes the defect that related technologies lose terrain features, can distinguish complex terrains during navigation planning, and improves the accuracy and reliability of path planning.

Description

Technical Field

[0001] This application relates to the fields of robot autonomous navigation and path planning technology, and in particular to a robot navigation method, device, equipment and storage medium. Background Technology

[0002] In recent years, with breakthroughs in legged robot motion control technology, legged robots (such as quadruped robots) have theoretically been able to traverse both stairwells with steps and industrial sites filled with gravel, based on their superior ability to select discrete footholds and adapt to terrain.

[0003] Currently, navigation technologies applied to legged robots face several challenges: First, most technologies rely on 2D occupancy grid maps for feasibility analysis, simply dividing the environment into "passable" and "obstacle" zones. This approach loses crucial elevation and terrain features, potentially misclassifying passable areas as insurmountable walls or other obstacles, leading to unnecessary detours or even navigation failures. Second, these technologies are often limited to single-plane scenarios, lacking global planning capabilities across floors, particularly in multi-level environments involving elevators and stairs, hindering autonomous cross-floor operations. Third, the stability of legged robots highly depends on gait matching with terrain. Because these technologies cannot accurately distinguish terrain during navigation, robots cannot adaptively adjust their gait according to environmental changes, easily leading to instability or even rollover accidents. Fourth, these technologies lack predictive and obstacle avoidance capabilities in dynamic environments. Traditional planning algorithms based solely on static cost maps struggle to predict the future trajectories of dynamic obstacles, severely impacting robot efficiency and operational safety.

[0004] In summary, there is an urgent need for a robot navigation method that can deeply integrate 3D terrain perception information, possess cross-floor global topology planning capabilities, and achieve deep coordination between planning and gait control, in order to solve the problem of autonomous passage for legged robots in complex multi-layered environments. Summary of the Invention

[0005] This application provides a robot navigation method, apparatus, device, and storage medium to address the shortcomings of the aforementioned related technologies. The technical solution is as follows:

[0006] In a first aspect, this application provides a robot navigation method, comprising:

[0007] In response to navigation commands containing the target location, obtain pre-defined layered scene map data;

[0008] Based on the layered scene map data, the navigation task of moving the robot from the initial position to the target position is divided into multiple consecutive navigation sub-tasks;

[0009] Path planning is performed separately for each navigation subtask, including:

[0010] Based on the point cloud data of the preset area collected by the robot at the current moment, the terrain feature distribution and the predicted path of dynamic obstacles in the preset area are determined, and a terrain cost distribution map is constructed.

[0011] The objective function is constructed based on the travel cost of the robot through the undetermined path. The robot's speed parameters and navigation path at the current moment are determined by minimizing the objective function in combination with the terrain cost distribution map.

[0012] Based on the robot's speed parameters and navigation path at each moment, the robot executes the corresponding navigation sub-task until it reaches the target position.

[0013] In one alternative of the first aspect, the layered scene map data includes a floor grid map and floor point cloud data for each floor within the scene;

[0014] The process of determining the robot's initial position includes:

[0015] Acquire the robot's initial observation data, and determine the floor label of the floor where the robot is located based on the comparison results of the initial observation data and the environmental features in the scene;

[0016] Based on the floor labels, extract the corresponding floor point cloud data and floor raster map from the layered scene map data;

[0017] The robot's initial pose is determined by matching the floor point cloud data using an adaptive Monte Carlo localization algorithm, and the robot's initial position in the floor grid map is obtained.

[0018] In one alternative of the first aspect, the layered scene map data further includes a pre-constructed topology map, which describes the connection path between every two navigation nodes within the scene;

[0019] The navigation task of moving the robot from its initial position to its target position based on the layered scene map data is divided into multiple consecutive navigation sub-tasks, including:

[0020] Using the initial position as the initial node and the target position as the target node, the shortest route sequence from the initial node to the target node is obtained by traversing the topology graph.

[0021] The navigation task is divided into navigation subtasks between every two adjacent navigation nodes according to the routing order of the navigation nodes in the shortest route sequence, resulting in multiple consecutive navigation subtasks.

[0022] In one alternative embodiment of the first aspect, determining the terrain feature distribution and predicted path of dynamic obstacles within the preset area based on point cloud data of the preset area collected by the robot at the current moment includes:

[0023] Based on the current floor where the robot is located, extract the corresponding floor grid map from the layered scene map data;

[0024] The point cloud data of the preset area is mapped to the corresponding grid in the floor grid map, the height distribution characteristics of the point cloud in each grid are statistically analyzed, and the terrain feature distribution in the preset area is determined based on the height distribution characteristics.

[0025] Cluster analysis is performed on point cloud data of a preset area, and obstacles in the preset area are identified based on the cluster analysis results.

[0026] The obstacles identified at the current moment are matched with the obstacles identified at the previous moment, and the obstacles that are successfully matched are identified as dynamic obstacles.

[0027] Predict the trajectory of each dynamic obstacle, mark the coordinates covered by the trajectory in the floor grid map, and obtain the predicted path of the dynamic obstacle.

[0028] In one alternative to the first aspect, the construction yields a terrain cost distribution map, comprising:

[0029] Calculate the slope cost, roughness cost, and height difference cost for each grid cell in the floor grid map based on the terrain feature distribution;

[0030] The terrain feature cost of the corresponding raster is calculated based on the maximum value among slope cost, roughness cost, and elevation difference cost.

[0031] The obstacle cost for each grid cell in the floor grid map is calculated based on the predicted path of the dynamic obstacles;

[0032] The terrain feature cost and obstacle cost are filled into the corresponding grid cells to generate a terrain cost distribution map containing the terrain feature cost and obstacle cost of each grid cell;

[0033] The slope cost is calculated based on the tilt angle of the normal vector of the point cloud in the neighborhood centered on each grid cell; the roughness cost is calculated based on the height variance of the point cloud within the grid cell; and the height difference cost is calculated based on the average height difference between each grid cell and all adjacent grid cells. The obstacle cost is maximized at the current position of the dynamic obstacle and decays along the predicted path.

[0034] In one alternative to the first aspect, constructing the objective function based on the travel cost of the robot traversing the undetermined path includes:

[0035] A motion smoothing term is constructed based on the rate of change of the robot's velocity parameters; a path length term is constructed based on the length of the path to be determined; an obstacle avoidance term is constructed by combining the distance between the grids and obstacles traversed by the path to be determined and the obstacle cost; and a terrain adaptation term is constructed based on the terrain feature cost of the grids traversed by the path to be determined.

[0036] The passage cost is obtained by weighted fusion of motion smoothness, path length, obstacle avoidance and terrain adaptation terms, and the objective function is constructed accordingly.

[0037] The process of minimizing the objective function by combining the terrain cost distribution map to determine the robot's velocity parameters and navigation path at the current moment includes:

[0038] Based on the grids traversed by the undetermined path, the obstacle cost and terrain feature cost of the corresponding grids are extracted from the terrain cost distribution map. The obstacle avoidance term and terrain adaptation term are calculated by combining the obstacle cost and terrain feature cost.

[0039] The grids traversed by the path to be determined, the length of the path to be determined, and the robot's speed parameters are adjusted to minimize the objective function. The minimum value of the objective function is obtained by iterative solution.

[0040] Output the velocity parameter sequence corresponding to the minimum value, and extract the linear velocity and angular velocity of the first time step in the velocity parameter sequence as the velocity parameters at the current time.

[0041] Output the path to be determined corresponding to the minimum value as the navigation path at the current moment.

[0042] In one alternative embodiment of the first aspect, before executing the corresponding navigation subtask based on the robot's velocity parameters and navigation path at each moment, the method further includes:

[0043] Based on the terrain cost distribution map, the average value of terrain feature costs within a preset distance on the navigation path is determined, and the terrain type within the preset distance on the navigation path is determined according to the numerical range in which the average value of terrain feature costs falls.

[0044] Adjust the robot's gait and speed parameters according to the terrain type;

[0045] The process of executing corresponding navigation subtasks based on the robot's velocity parameters and navigation path at each moment includes:

[0046] Inverse kinematics calculations are performed based on the adjusted velocity and gait parameters to obtain the motion commands for each actuator of the robot.

[0047] The robot is controlled to move along the navigation path according to the motion instructions in order to perform the corresponding navigation sub-tasks.

[0048] Secondly, this application also provides a robot navigation device, comprising:

[0049] The initialization module is used to obtain preset layered scene map data in response to navigation commands containing the target location;

[0050] The task decomposition module is used to divide the navigation task of the robot moving from the initial position to the target position into multiple consecutive navigation sub-tasks based on the layered scene map data.

[0051] The navigation path planning module is used to plan paths for each navigation subtask separately, including:

[0052] Based on the point cloud data of the preset area collected by the robot at the current moment, the terrain feature distribution and the predicted path of dynamic obstacles in the preset area are determined, and a terrain cost distribution map is constructed.

[0053] The objective function is constructed based on the travel cost of the robot through the undetermined path. The robot's speed parameters and navigation path at the current moment are determined by minimizing the objective function in combination with the terrain cost distribution map.

[0054] The motion module is used to execute corresponding navigation sub-tasks based on the robot's velocity parameters and navigation path at each moment until the robot reaches the target position.

[0055] Thirdly, this application also provides an electronic device, including a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor executes the program to implement the method provided by the first aspect of this application or any implementation thereof.

[0056] Fourthly, this application also provides a non-transitory computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the method provided by the first aspect of this application or any implementation thereof.

[0057] The beneficial effects of the technical solution provided in this application include at least the following:

[0058] This application provides a robot navigation method, apparatus, device, and storage medium that effectively overcomes the shortcomings of traditional two-dimensional grid maps that lose terrain features by constructing a terrain cost distribution map that integrates elevation information from point cloud data. This technical solution enables the robot to accurately distinguish complex terrains such as stairs, slopes, and gravel roads during navigation planning, thereby avoiding misjudging passable areas as obstacles, significantly reducing unnecessary detours and planning failures, and improving the accuracy and reliability of path planning in three-dimensional space.

[0059] Secondly, the navigation task is decomposed based on layered scene map data, and a global environmental representation capable of traversing multiple floors is established for the robot by combining it with a topology diagram. By identifying and associating navigation nodes such as elevators and stairs, global topology path planning is achieved in multi-floor scenarios. This enables the robot to autonomously operate across floors, significantly expanding its reach and operational flexibility in complex building structures.

[0060] Finally, by deeply coupling terrain accessibility analysis with dynamic gait adjustment mechanism, the robot can adaptively adjust gait parameters based on real-time perceived terrain features (such as roughness and slope), thereby ensuring motion stability on different terrains, effectively preventing slippage, instability, and even rollover accidents, while enhancing obstacle avoidance and passage capabilities in dynamic environments, and improving the overall safety and efficiency of navigation. Attached Figure Description

[0061] To more clearly illustrate the technical solutions in this application or related technologies, the drawings used in the description of the embodiments or related technologies will be briefly introduced below. Obviously, the drawings described below are some embodiments of this application. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0062] Figure 1 This is one of the flowcharts illustrating a robot navigation method provided in this application embodiment;

[0063] Figure 2 This is a schematic diagram of the topology provided in the embodiments of this application;

[0064] Figure 3 This is a second schematic flowchart of a robot navigation method provided in an embodiment of this application;

[0065] Figure 4 This is a schematic diagram of the structure of a robot navigation device provided in an embodiment of this application;

[0066] Figure 5 This is a schematic diagram of the structure of the electronic device provided in the embodiments of this application. Detailed Implementation

[0067] To make the objectives, technical solutions, and advantages of this application clearer, the technical solutions of this application will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of this application, not all embodiments. Based on the embodiments of this application, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of this application.

[0068] The terms "comprising" and "having," and any variations thereof, in the specification, claims, and accompanying drawings of this application are intended to cover non-exclusive inclusion. For example, a process, method, system, product, or apparatus that includes a series of steps or modules is not limited to the steps or modules listed, but may optionally include steps or modules not listed, or may optionally include other steps or modules inherent to such process, method, product, or apparatus.

[0069] It should be noted that the terms "first" and "second" used in this application are merely to distinguish similar objects and do not represent a specific ordering of the objects. It is understood that "first" and "second" can be interchanged in a specific order or sequence where permitted. It should be understood that the objects distinguished by "first" and "second" can be interchanged where appropriate so that the embodiments of this application described herein can be implemented in an order other than those described or illustrated herein.

[0070] It should be noted that the robot described in this application is a legged robot with four or more legs. The legged robot can control multiple legs through actuators such as motors to achieve the robot's movement.

[0071] There are still some shortcomings in the navigation technology for legged robots. For example, the feasibility analysis method using two-dimensional grid maps lacks quantitative assessment of ground roughness, step height difference, and slipperiness risk. Furthermore, for legged robots, terrain with a certain height and slope (depending on the robot's hardware parameters, such as a height of 20cm and a slope of 20 degrees) is a passable area, but it is often misjudged as an insurmountable wall in two-dimensional maps. Therefore, the current technology cannot solve the navigation problem for legged robots.

[0072] The present application will now be described in detail with reference to specific embodiments.

[0073] Next, combine Figure 1 This application describes a robot navigation method provided by an embodiment. For details, please refer to [link to relevant documentation]. Figure 1 , Figure 1A flowchart illustrating a robot navigation method provided in an embodiment of this application is shown. Figure 1 As shown, the method includes the following steps:

[0074] S101, in response to a navigation command containing the target location, acquires preset layered scene map data;

[0075] S102, Based on the layered scene map data, the navigation task of the robot moving from the initial position to the target position is divided into multiple consecutive navigation sub-tasks;

[0076] S103, perform path planning for each navigation subtask separately;

[0077] S104, based on the robot's speed parameters and navigation path at each moment, execute the corresponding navigation sub-task until the robot reaches the target position.

[0078] In some embodiments, in S101, the user can set a navigation target for the robot, input the target location coordinates and the target location floor label, and the robot can receive the navigation instruction and trigger the action of obtaining the preset layered scene map data, actively reading the floor grid map, floor point cloud data and topology diagram of each floor in the pre-built scene.

[0079] Next, the robot needs to be initialized, identifying its initial position, including:

[0080] First, the initial observation data of the robot is obtained, and the floor label of the floor where the robot is located is determined based on the comparison results of the initial observation data and the environmental features in the scene.

[0081] The robot's initial observation data includes, but is not limited to, air pressure measurements, WiFi signal strength values, and real-time point cloud data captured by the robot. By comparing the air pressure measurements and WiFi signal strength values ​​with historically recorded air pressure measurements for each floor and WiFi signal strength values ​​for each location, the posterior probability of the current observation data belonging to each floor can be estimated. This allows us to determine the floor where the robot is located and identify the floor label.

[0082] Furthermore, based on the floor labels, the corresponding floor point cloud data and floor grid map are extracted from the layered scene map data.

[0083] Finally, based on the adaptive Monte Carlo localization algorithm, the floor point cloud data is matched to determine the robot's initial pose and locate the robot's initial position in the floor grid map.

[0084] Understandably, the Adaptive Monte Carlo Localization (AMCL) algorithm can be used to disperse a particle swarm (e.g., 500-2000 particles) on the current floor map, calculate the matching score between the point cloud data corresponding to the particle swarm and the real-time point cloud data, and determine successful localization when the particle swarm converges and the highest score is greater than a preset threshold (e.g., 0.85), and then set the state of the particle with the highest weight. As the robot's initial pose, where, Represents the coordinates of the initial position. Indicates the initial attitude.

[0085] Further, in step S102, the navigation task of moving the robot from the initial position to the target position is divided into multiple consecutive navigation sub-tasks.

[0086] It should be noted that the topology diagram can be constructed based on the travel paths in the scene, the connecting passages between floors, and the pre-set navigation points. The form of the topology diagram is the same as that of a knowledge graph, and can be represented as follows: In this context, V represents each navigation node, E represents the connection path between navigation nodes or the connection channel across layers, and each triple represents the connection relationship between every two navigation nodes with a connection path.

[0087] For example, floor connection points such as elevator entrances and stairwells can be used as navigation nodes. Alternatively, navigation nodes can be set manually, such as setting navigation nodes in corridors or at room entrances. This application does not limit this, and the number of navigation nodes other than floor connection points can be increased according to actual needs.

[0088] S102 specifically includes:

[0089] Using the initial position as the initial node and the target position as the target node, the shortest route sequence from the initial node to the target node is obtained by traversing the topology graph.

[0090] The navigation task is divided into navigation subtasks between every two adjacent navigation nodes according to the routing order of the navigation nodes in the shortest route sequence, resulting in multiple consecutive navigation subtasks.

[0091] For example, Figure 2 A schematic diagram of the topology is shown, such as... Figure 2 As shown, navigation nodes can be set. Let the initial node be denoted as The initial node is located on the 1st floor, and the target node is denoted as... The target node is located on the second floor, where The node is located in the living room on the first floor. The node represents the staircase entrance / exit located on the 1st floor leading to the 2nd floor. The node is located at the entrance to the staircase leading from the 2nd floor to the 1st floor. Located in the second-floor corridor, Located at the entrance to the room on the second floor, That is to say, it is based on the initial node. The node at the staircase entrance to the first floor .

[0092] Based on the above examples, the initial node To the target node The shortest route sequence includes Passing through nodes in sequence It can , , , and Each of these is treated as a navigation subtask, where... As subtasks spanning multiple floors, this results in sequential navigation subtasks that can be executed serially; this is merely an example of an embodiment of this application.

[0093] Next, as Figure 3 As shown, Figure 3 This illustration shows a flowchart of a robot navigation method provided in an embodiment of this application. Path planning can be performed separately for each navigation subtask. S103 includes:

[0094] S301, Based on the point cloud data of the preset area collected by the robot at the current moment, determine the terrain feature distribution and the predicted path of dynamic obstacles in the preset area, and construct a terrain cost distribution map;

[0095] S302, construct an objective function based on the travel cost of the robot through the undetermined path, and determine the robot's speed parameters and navigation path at the current moment by minimizing the objective function in combination with the terrain cost distribution map.

[0096] First, execute the steps in S301, which specifically include:

[0097] S3011, Extract the floor grid map of the corresponding floor from the layered scene map data according to the current floor where the robot is located.

[0098] S3012, map the point cloud data of the preset area to the corresponding grid in the floor grid map, count the height distribution characteristics of the point cloud in each grid, and determine the terrain feature distribution within the preset area based on the height distribution characteristics.

[0099] The preset area can be understood as a local scrolling window area centered on the robot (e.g., The robot can collect point cloud data within a preset area using a point cloud camera. Based on the robot's current position and pose, the point cloud data can be aligned with a two-dimensional grid map, and then mapped to the corresponding grid cell in the floor grid map.

[0100] Specifically, the height distribution characteristics can be obtained from the elevation information recorded in each point cloud within the raster, including the maximum height, minimum height, height variance, and average height of the point cloud within the corresponding raster.

[0101] Furthermore, the distribution of terrain features can be determined based on the height distribution characteristics, including three terrain indicators: slope, roughness, and height difference.

[0102] Slope: Select the neighborhood centered on each grid cell (e.g., the current grid cell and its 8 adjacent grid cells). The neighborhood is fitted with a local microplane of the point cloud using the least squares method, and the angle between the plane normal vector and the opposite direction of gravity is calculated. You can then obtain the slope of the corresponding grid.

[0103] Roughness: Calculated based on the height variance of the point cloud within the raster, for example, using the formula:

[0104] ;

[0105] in, For roughness, The coefficient representing the roughness calculation This represents the height variance.

[0106] Height difference: The height difference is calculated by taking the average of the differences between the average height of the point cloud in each grid cell and the average height of all adjacent grid cells.

[0107] S3013 performs cluster analysis based on point cloud data of a preset area to identify independent object clusters, and can then identify obstacles in the preset area based on the cluster analysis results.

[0108] S3014, perform nearest neighbor matching between the obstacle identified at the current time and the obstacle identified at the previous time, and identify the successfully matched obstacle as a dynamic obstacle (such as a pedestrian, a pet, etc.).

[0109] S3015 uses a linear Kalman filter model to determine the motion state of each dynamic obstacle. To predict the trajectory of each dynamic obstacle, the coordinates covered by the trajectory are marked in the floor grid map to obtain the predicted path of the dynamic obstacle.

[0110] Specifically, It can be understood as the motion state of a dynamic obstacle at a predicted moment. The motion trajectory includes a temporal sequence of the motion state at each predicted moment. The motion state is specifically set as the coordinates and velocity in the reference coordinate system corresponding to the floor grid map.

[0111] in, This indicates the dynamic obstacle in the reference coordinate system at the corresponding prediction time. x-coordinate of the axis This indicates the dynamic obstacle in the reference coordinate system at the corresponding prediction time. The ordinate of the axis, This indicates the dynamic obstacle along the reference coordinate system at the corresponding prediction time. Lateral velocity of the shaft, This indicates the dynamic obstacle along the reference coordinate system at the corresponding prediction time. The longitudinal velocity of the shaft.

[0112] Furthermore, a terrain cost distribution map is constructed based on the predicted path of terrain features and dynamic obstacles, including the following steps:

[0113] S3016, Calculate the slope cost, roughness cost, and height difference cost of each grid cell in the floor grid map based on the terrain feature distribution.

[0114] Specifically, the slope cost is positively correlated with the slope value of the grid. The greater the slope, the greater the slope cost. If the slope is greater than the slope limit threshold (e.g., 35°), the slope cost is set to infinity. The slope limit threshold can be set according to the robot's climbing ability. The stronger the climbing ability, the larger the slope limit threshold can be set.

[0115] The roughness cost is positively correlated with the roughness value of the grid; the larger the value, the more uneven the ground.

[0116] The height difference cost can be compared with the robot's maximum leg-raising height. If the height difference is less than or equal to the maximum leg-raising height, the height difference cost is calculated based on the difference between the maximum leg-raising height and the height difference. The larger the difference between the maximum leg-raising height and the height difference, the smaller the height difference cost. If the height difference is greater than the maximum leg-raising height, the height difference cost is set to infinity.

[0117] S3017 calculates the terrain feature cost of the corresponding raster based on the maximum value among slope cost, roughness cost, and height difference cost.

[0118] S3018, calculate the obstacle cost of each grid in the floor grid map based on the predicted path of the dynamic obstacle.

[0119] The obstacle cost is set to its maximum value at the current position of the dynamic obstacle. For example, the obstacle cost at the current moment can be set to infinity. The area of ​​obstacle cost can be marked by the grid along the predicted path, and the obstacle cost decays along the predicted path over time.

[0120] By setting obstacle costs, a "temporal potential field" can be formed in space to guide the robot to avoid obstacles in advance, effectively reducing sudden stops and path oscillations.

[0121] S3019, fill the terrain feature cost and obstacle cost into the corresponding grid, and generate a terrain cost distribution map containing the terrain feature cost and obstacle cost of each grid;

[0122] It should be noted that terrain feature cost and obstacle cost can be divided into two layers as needed to quickly read the two types of data.

[0123] Furthermore, S302 includes:

[0124] S3021, a motion smoothing term is constructed based on the rate of change of the robot's speed parameters, a path length term is constructed based on the length of the path to be determined, an obstacle avoidance term is constructed by combining the distance between the grid and the obstacle and the obstacle cost of the path to be determined, and a terrain adaptation term is constructed based on the terrain feature cost of the grid through which the path to be determined passes.

[0125] S3022, based on the motion smoothness term, path length term, obstacle avoidance term and terrain adaptation term, a weighted fusion is performed to obtain the passage cost, and the objective function is constructed.

[0126] Among them, the motion smoothing term can minimize the robot's speed abrupt changes during path planning, avoiding the problem of robot motion imbalance caused by speed abrupt changes; the path length term can select the shortest possible path during path planning; the obstacle avoidance term can help the robot maintain a distance from obstacles on the navigation path to avoid collisions; and the terrain adaptation term can adaptively select grids with lower terrain costs during path planning, ensuring that the robot can move on a more flat navigation path.

[0127] S3023, based on the grids traversed by the undetermined path, extract the obstacle cost and terrain feature cost of the corresponding grid from the terrain cost distribution map, and calculate the obstacle avoidance term and terrain adaptation term by combining the obstacle cost and terrain feature cost.

[0128] S3024, adjust the grids traversed by the path to be determined, the length of the path to be determined, and the robot's speed parameters to minimize the objective function, and calculate the minimum value of the objective function through iterative solution.

[0129] Specifically, calculations can be performed using nonlinear model predictive control (MPC) algorithms or time elastic band (TEB) algorithms.

[0130] S3025, output the velocity parameter sequence corresponding to the minimum value, and extract the linear velocity and angular velocity of the first time step in the velocity parameter sequence as the velocity parameters at the current moment.

[0131] S3026, output the path to be determined corresponding to the minimum value as the navigation path at the current moment.

[0132] Understandably, the navigation path obtained is the optimal navigation path calculated based on the point cloud data collected by the robot at the current moment. Based on the navigation path, the landing point of the robot at the next moment can be determined. The robot can repeatedly execute steps S3021-S3025 to update the path planning results, thereby continuously updating the optimal landing point (i.e. the grid covered by the robot's foot) and updating the navigation path in real time.

[0133] Understandably, the velocity parameters obtained in each planning step are the sequence of velocity parameters of the robot moving along the navigation path, that is, the predicted velocity parameters of the robot at each moment during its operation along the navigation path. It is only necessary to extract the linear velocity and angular velocity of the first time step in the velocity parameter sequence as the velocity parameters at the current moment. The robot can repeatedly execute steps S3021-S3025 based on the real-time collected point cloud data to update the path planning results, thereby continuously updating the velocity parameter sequence and updating the velocity parameters at each moment in real time.

[0134] The optimization results ensure that the robot can not only avoid walls and pedestrians, but also automatically bypass high-risk areas such as piles of rubble and steep slopes, and adjust its path in real time.

[0135] In some embodiments, prior to S104, the robot's gait and speed parameters can be dynamically adjusted based on the planning results, including:

[0136] S401, based on the terrain cost distribution map, determine the average value of terrain feature costs within a preset distance on the navigation path, and determine the terrain type within the preset distance on the navigation path according to the numerical range in which the average value of terrain feature costs falls.

[0137] The preset distance can be set to 10m, but this application embodiment does not limit this.

[0138] Specifically, if the value of the terrain feature cost is less than the first threshold If the terrain feature cost is greater than or equal to the first threshold, it indicates that the terrain is flat. And less than the second threshold If the value of the terrain feature cost is greater than or equal to the second threshold, it indicates that the terrain is rugged. This indicates that the terrain is either a staircase or a steep slope.

[0139] S402, adjust the robot's gait and speed parameters according to the terrain type.

[0140] Specifically, gait parameters include cadence (the frequency of steps) and leg lift height. For flat terrain, the cadence can be increased, for example, set to 2.0Hz, and the leg lift height can be decreased (for example, from 15cm to 10cm) to achieve rapid movement of the robot. For rugged terrain, the cadence can be decreased and the leg lift height increased, for example, the cadence can be decreased to 1.5Hz and the leg lift height increased to 12cm to prevent the feet from tripping. For stairs or steep slopes, the cadence can be further decreased and the leg lift height increased based on the gait for rugged terrain. It is also necessary to reduce the linear velocity and adjust the body posture to conform to the ground slope, lower the body's center of gravity height, and ensure body stability.

[0141] In some embodiments, in S104, inverse kinematics calculations can be performed based on the adjusted velocity parameters and gait parameters to obtain motion commands for each actuator of the robot.

[0142] The robot is controlled to move along the navigation path according to the motion instructions in order to perform the corresponding navigation sub-tasks.

[0143] The following are apparatus embodiments of this application, which can be used to execute the method embodiments of this application. For details not disclosed in the apparatus embodiments of this application, please refer to the method embodiments of this application.

[0144] Please see below. Figure 4 The image below is a schematic diagram of a robot navigation device provided as an exemplary embodiment of this application. The device includes:

[0145] The initialization module is used to obtain preset layered scene map data in response to navigation commands containing the target location;

[0146] The task decomposition module is used to divide the navigation task of the robot moving from the initial position to the target position into multiple consecutive navigation sub-tasks based on the layered scene map data.

[0147] The navigation path planning module is used to plan paths for each navigation subtask separately, including:

[0148] Based on the point cloud data of the preset area collected by the robot at the current moment, the terrain feature distribution and the predicted path of dynamic obstacles in the preset area are determined, and a terrain cost distribution map is constructed.

[0149] The objective function is constructed based on the travel cost of the robot through the undetermined path. The robot's speed parameters and navigation path at the current moment are determined by minimizing the objective function in combination with the terrain cost distribution map.

[0150] The motion module is used to execute corresponding navigation sub-tasks based on the robot's velocity parameters and navigation path at each moment until the robot reaches the target position.

[0151] It should be noted that the device provided in the above embodiments is only illustrated by the division of the above functional modules when executing a robot navigation method. In practical applications, the above functions can be assigned to different functional modules as needed, that is, the internal structure of the device can be divided into different functional modules to complete all or part of the functions described above. In addition, the device and method embodiments provided in the above embodiments belong to the same concept, and the implementation process can be found in the method embodiments, which will not be repeated here.

[0152] This application also provides an electronic device, including a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor executes the program to implement the steps of any of the methods described above.

[0153] Please see Figure 5 This is a structural block diagram of an electronic device provided in an embodiment of this application.

[0154] like Figure 5 As shown, the electronic device includes a processor and a memory.

[0155] In this embodiment, the processor is the control center of the computer system, and can be a processor of a physical machine or a processor of a virtual machine. The processor may include one or more processing cores, such as a 4-core processor, an 8-core processor, etc. The processor can be implemented using at least one hardware form of DSP (Digital Signal Processing), FPGA (Field-Programmable Gate Array), or PLA (Programmable Logic Array).

[0156] A processor can also include a main processor and a coprocessor. The main processor is used to process data in the wake-up state and is also called the CPU (Central Processing Unit). The coprocessor is a low-power processor used to process data in the standby state.

[0157] The memory may include one or more computer-readable storage media, which may be non-transitory. The memory may also include high-speed random access memory and non-volatile memory, such as one or more disk storage devices or flash memory devices. In some embodiments of this application, the non-transitory computer-readable storage media in the memory are used to store at least one instruction, which is executed by a processor to implement the methods in the embodiments of this application.

[0158] In some embodiments, the electronic device further includes a peripheral device interface and at least one peripheral device. The processor, memory, and peripheral device interface are connected via a bus or signal line. Each peripheral device is connected to the peripheral device interface via a bus, signal line, or circuit board. Specifically, the peripheral device includes: a display screen, a camera, and audio circuitry. The peripheral device interface can be used to connect at least one I / O (Input / Output) related peripheral device to the processor and memory.

[0159] In some embodiments of this application, the processor, memory, and peripheral device interfaces are integrated on the same chip or circuit board; in other embodiments of this application, any one or two of the processor, memory, and peripheral device interfaces can be implemented on separate chips or circuit boards. This application does not specifically limit the implementation in this regard.

[0160] The electronic device structural block diagrams shown in the embodiments of this application do not constitute a limitation on the electronic device. The electronic device may include more or fewer components than shown, or combine certain components, or use different component arrangements.

[0161] This application also provides a computer-readable storage medium storing a computer program thereon, which, when executed by a processor, implements the steps of the methods in any of the foregoing embodiments. The computer-readable storage medium may include, but is not limited to, any type of disk, including floppy disks, optical disks, DVDs, CD-ROMs, microdrives, as well as magneto-optical disks, ROMs, RAMs, EPROMs, EEPROMs, DRAMs, VRAMs, flash memory devices, magnetic cards or optical cards, nanosystems (including molecular memory ICs), or any type of medium or device suitable for storing instructions and / or data.

[0162] Through the above description of the embodiments, those skilled in the art can clearly understand that each embodiment can be implemented by means of software plus necessary general-purpose hardware platforms, and of course, it can also be implemented by hardware. Based on this understanding, the above technical solutions, in essence or the parts that contribute to the related technology, can be embodied in the form of software products. This computer software product can be stored in a computer-readable storage medium, such as ROM / RAM, magnetic disk, optical disk, etc., and includes several instructions to cause a computer device (which may be a personal computer, server, or network device, etc.) to execute the methods described in the various embodiments or some parts of the embodiments.

[0163] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of this application, and are not intended to limit them. Although this application has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that modifications can still be made to the technical solutions described in the foregoing embodiments, or equivalent substitutions can be made to some of the technical features. Such modifications or substitutions do not cause the essence of the corresponding technical solutions to deviate from the spirit and scope of the technical solutions of the embodiments of this application.

Claims

1. A robot navigation method, characterized in that, include: In response to navigation commands containing the target location, obtain pre-defined layered scene map data; Based on the layered scene map data, the navigation task of moving the robot from the initial position to the target position is divided into multiple consecutive navigation sub-tasks; Path planning is performed separately for each navigation subtask, including: Based on the point cloud data of the preset area collected by the robot at the current moment, the terrain feature distribution and the predicted path of dynamic obstacles in the preset area are determined, and a terrain cost distribution map is constructed. A motion smoothing term is constructed based on the rate of change of the robot's velocity parameters; a path length term is constructed based on the length of the path to be determined; an obstacle avoidance term is constructed by combining the distance between the grids and obstacles traversed by the path to be determined and the obstacle cost; and a terrain adaptation term is constructed based on the terrain feature cost of the grids traversed by the path to be determined. The passage cost is obtained by weighted fusion of motion smoothness, path length, obstacle avoidance and terrain adaptation terms, and the objective function is constructed accordingly. Based on the grids traversed by the undetermined path, the obstacle cost and terrain feature cost of the corresponding grids are extracted from the terrain cost distribution map. The obstacle avoidance term and terrain adaptation term are calculated by combining the obstacle cost and terrain feature cost. The grids traversed by the path to be determined, the length of the path to be determined, and the robot's speed parameters are adjusted to minimize the objective function. The minimum value of the objective function is obtained by iterative solution. Output the velocity parameter sequence corresponding to the minimum value, and extract the linear velocity and angular velocity of the first time step in the velocity parameter sequence as the velocity parameters at the current time. Output the path to be determined corresponding to the minimum value as the navigation path at the current moment; Based on the robot's speed parameters and navigation path at each moment, the robot executes the corresponding navigation sub-task until it reaches the target position.

2. The robot navigation method according to claim 1, characterized in that, The layered scene map data includes a floor grid map and floor point cloud data for each floor within the scene; The process of determining the robot's initial position includes: Acquire the robot's initial observation data, and determine the floor label of the floor where the robot is located based on the comparison results of the initial observation data and the environmental features in the scene; Based on the floor labels, extract the corresponding floor point cloud data and floor raster map from the layered scene map data; The robot's initial pose is determined by matching the floor point cloud data using an adaptive Monte Carlo localization algorithm, and the robot's initial position in the floor grid map is obtained.

3. The robot navigation method according to claim 2, characterized in that, The layered scene map data also includes a pre-constructed topology map, which is used to describe the connection path between every two navigation nodes in the scene; The navigation task of moving the robot from its initial position to its target position based on the layered scene map data is divided into multiple consecutive navigation sub-tasks, including: Using the initial position as the initial node and the target position as the target node, the shortest route sequence from the initial node to the target node is obtained by traversing the topology graph. The navigation task is divided into navigation subtasks between every two adjacent navigation nodes according to the routing order of the navigation nodes in the shortest route sequence, resulting in multiple consecutive navigation subtasks.

4. The robot navigation method according to claim 3, characterized in that, The determination of terrain feature distribution and predicted paths of dynamic obstacles within a preset area based on point cloud data collected by the robot at the current moment includes: Based on the current floor where the robot is located, extract the corresponding floor grid map from the layered scene map data; The point cloud data of the preset area is mapped to the corresponding grid in the floor grid map, the height distribution characteristics of the point cloud in each grid are statistically analyzed, and the terrain feature distribution in the preset area is determined based on the height distribution characteristics. Cluster analysis is performed on point cloud data of a preset area, and obstacles in the preset area are identified based on the cluster analysis results. The obstacles identified at the current moment are matched with the obstacles identified at the previous moment, and the obstacles that are successfully matched are identified as dynamic obstacles. Predict the trajectory of each dynamic obstacle, mark the coordinates covered by the trajectory in the floor grid map, and obtain the predicted path of the dynamic obstacle.

5. A robot navigation method according to claim 4, characterized in that, The construction yields a terrain cost distribution map, including: Calculate the slope cost, roughness cost, and height difference cost for each grid cell in the floor grid map based on the terrain feature distribution; The terrain feature cost of the corresponding raster is calculated based on the maximum value among slope cost, roughness cost, and elevation difference cost. The obstacle cost for each grid cell in the floor grid map is calculated based on the predicted path of the dynamic obstacles; The terrain feature cost and obstacle cost are filled into the corresponding grid cells to generate a terrain cost distribution map containing the terrain feature cost and obstacle cost of each grid cell; The slope cost is calculated based on the tilt angle of the normal vector of the point cloud in the neighborhood centered on each grid cell; the roughness cost is calculated based on the height variance of the point cloud within the grid cell; and the height difference cost is calculated based on the average height difference between each grid cell and all adjacent grid cells. The obstacle cost is maximized at the current position of the dynamic obstacle and decays along the predicted path.

6. The robot navigation method according to claim 5, characterized in that, Before executing the corresponding navigation subtask based on the robot's velocity parameters and navigation path at each moment, the method further includes: Based on the terrain cost distribution map, the average value of terrain feature costs within a preset distance on the navigation path is determined, and the terrain type within the preset distance on the navigation path is determined according to the numerical range in which the average value of terrain feature costs falls. Adjust the robot's gait and speed parameters according to the terrain type; The process of executing corresponding navigation subtasks based on the robot's velocity parameters and navigation path at each moment includes: Inverse kinematics calculations are performed based on the adjusted velocity and gait parameters to obtain the motion commands for each actuator of the robot. The robot is controlled to move along the navigation path according to the motion instructions in order to perform the corresponding navigation sub-tasks.

7. A robot navigation device based on the robot navigation method according to any one of claims 1-6, characterized in that, The device includes: The initialization module is used to obtain preset layered scene map data in response to navigation commands containing the target location; The task decomposition module is used to divide the navigation task of the robot moving from the initial position to the target position into multiple consecutive navigation sub-tasks based on the layered scene map data. The navigation path planning module is used to plan paths for each navigation subtask separately, including: Based on the point cloud data of the preset area collected by the robot at the current moment, the terrain feature distribution and the predicted path of dynamic obstacles in the preset area are determined, and a terrain cost distribution map is constructed. The objective function is constructed based on the travel cost of the robot through the undetermined path. The robot's speed parameters and navigation path at the current moment are determined by minimizing the objective function in combination with the terrain cost distribution map. The motion module is used to execute corresponding navigation sub-tasks based on the robot's velocity parameters and navigation path at each moment until the robot reaches the target position.

8. An electronic device comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that, When the processor executes the program, it implements the steps of the method as described in any one of claims 1 to 6.

9. A non-transitory computer-readable storage medium having a computer program stored thereon, characterized in that, When the computer program is executed by a processor, it implements the steps of the method as described in any one of claims 1 to 6.

Images