Robot dynamic path planning method and system for unstructured environment

Abstract

The application provides a robot dynamic path planning method and system for unstructured environment, and the method comprises the following steps: maintaining a local elevation feature grid map with a preset range and with the robot itself as the center in real time, so as to determine the motion mode of the robot in the next control cycle as a smooth mode, a shock absorption mode or an obstacle crossing mode; calling the constraint term weight parameter value of the cost function corresponding to the motion mode, wherein the cost function is calculated according to the weighted sum of the heading term, the distance term, the speed term, the traffic degree term and the heading vertical constraint term; obtaining a plurality of simulated trajectory sequences of the robot in the next control cycle based on the DWA search, taking a set of speed value corresponding to the simulated trajectory sequence with the minimum cost calculated based on the cost function as the optimal solution, and generating a speed control instruction for the next control cycle and issuing the speed control instruction. The application realizes a control technology for realizing the adaptive obstacle avoidance and obstacle crossing of the robot.

Description

Technical Field

[0001] This invention relates to the field of robot navigation technology, and in particular to a method and system for dynamic path planning of robots in unstructured environments. Background Technology

[0002] With the rapid development of robotics technology, wheel-legged robots (quadruped robots) have been widely used in substation inspection, building security, and disaster relief due to their combination of the efficiency of wheeled locomotion and the obstacle-crossing ability of legged locomotion. In these practical application scenarios, robots often face complex unstructured environments with multiple floors and obstacles, which poses a significant challenge to their autonomous localization, navigation, and path planning capabilities.

[0003] Path planning is a core component of autonomous robot navigation. Its main task is to find a collision-free and optimal trajectory from the starting point to the target point in an environment with obstacles. Among local path planning algorithms, the Dynamic Window Approach (DWA) is widely used in various mobile robot systems due to its low computational cost, strong real-time performance, and ability to simultaneously consider speed and obstacle avoidance requirements. Traditional DWA algorithms mainly rely on planar distance information obtained from sensors (such as 2D LiDAR) to binarize the environment into "free space" and "obstacle space." By simulating multiple velocity trajectories and evaluating their distances to obstacles, target heading deviations, and speed magnitudes, the optimal control command is selected.

[0004] However, the existing DWA algorithm has limitations in terrain physics perception, attitude control for crossing layers and obstacles, and is difficult to meet the high-precision motion control requirements of four-wheeled robots. Summary of the Invention

[0005] This invention provides a dynamic path planning method and system for robots in unstructured environments, which addresses the shortcomings of existing path planning algorithms in effectively handling robot navigation in complex three-dimensional environments. It realizes a path planning method that can integrate three-dimensional terrain features and dynamically adjust the trajectory evaluation strategy according to terrain accessibility.

[0006] This invention provides a dynamic path planning method for robots in unstructured environments, comprising:

[0007] The local elevation feature grid map centered on the robot itself and within a preset range is maintained in real time. Based on the local elevation feature grid map, the robot's motion mode in the next control cycle is determined to be either smooth mode, shock absorption mode, or obstacle crossing mode.

[0008] According to the motion mode, the constraint weight parameter value of the corresponding cost function is called so that the robot cruises at high speed in the smooth mode, decelerates in the shock absorption mode, and cuts into the terrain in the forward direction in the obstacle crossing mode. The cost function is calculated based on the weighted sum of the heading, distance, speed, passability and heading vertical constraint terms.

[0009] Multiple simulated trajectory sequences for the robot in the next control cycle are obtained by searching using the dynamic window method. The set of velocity values ​​corresponding to the simulated trajectory sequence with the minimum cost calculated based on the cost function is taken as the optimal solution, and the velocity control command for the next control cycle is generated and issued.

[0010] According to the robot dynamic path planning method for unstructured environments provided by the present invention, the step of determining whether the robot's motion mode in the next control cycle is a smooth mode, a shock-absorbing mode, or an obstacle-crossing mode based on the local elevation feature grid map specifically includes:

[0011] Based on the local elevation feature raster map, the passage cost of each raster is calculated, a cost map is generated, and the height distribution characteristics are statistically analyzed based on the raster elevation values ​​to determine whether it is a unimodal or bimodal distribution.

[0012] If the terrain in front of the robot is identified as having a single-peak distribution, the height variance is higher than the preset threshold, and the robot's roll and pitch angles are both within the preset safe range, then the motion mode for the current cycle is determined to be the shock absorption mode.

[0013] If the terrain in front of the robot is identified as having a bimodal distribution, or if the robot's roll or pitch angles exceed the preset safety range, the motion mode for the current cycle is determined to be obstacle-crossing mode.

[0014] In other cases, the motion pattern of the current cycle is determined to be a steady pattern.

[0015] According to the present invention, a robot dynamic path planning method for unstructured environments includes the following steps: calculating the passage cost of each grid cell based on the local elevation feature grid map, generating a cost map, and statistically analyzing the height distribution characteristics (unimodal or bimodal) based on the grid elevation values.

[0016] Traverse the valid grids in the local elevation feature grid map, obtain the height set of the point cloud inside each valid grid, and statistically obtain the height histogram of each grid.

[0017] Based on the height histogram, the ground roughness and terrain slope of each grid are calculated, and the distribution characteristics of the height histogram of the grid are marked as unimodal or bimodal.

[0018] Cost maps corresponding to local elevation feature raster maps are generated based on the ground roughness, terrain slope, and distribution characteristics of the effective raster area, so that rasters with high ground roughness and terrain slope have higher costs, and rasters with bimodal distribution are given a preset maximum cost value.

[0019] According to the present invention, a dynamic path planning method for robots in unstructured environments is provided, wherein the traversability term of the cost function is calculated based on the sum of grid cost values ​​of the grid corresponding to the trajectory; and the heading vertical constraint term is calculated based on the normal vector angle of the tangential angle at the end of the trajectory and the elevation gradient of the terrain ahead.

[0020] The robot dynamic path planning method for unstructured environments provided by the present invention further includes:

[0021] The system acquires the point cloud data stream collected in real time by the lidar on the robot, acquires the robot's current angular velocity and acceleration read in real time by the inertial measurement unit on the robot, and calculates the robot's real-time pitch angle and roll angle.

[0022] After performing voxel grid downsampling and height pass-through filtering on the collected point cloud data stream, the point cloud data is converted to a preset robot coordinate system.

[0023] According to the robot dynamic path planning method for unstructured environments provided by the present invention, the step of real-time maintenance of a local elevation feature grid map centered on the robot and within a preset range specifically includes:

[0024] A local scrolling grid map is established with the robot's center as the origin and follows the robot's movement. The local scrolling grid map discretizes the horizontal ground within a preset range centered on the robot itself into a two-dimensional grid of fixed size, and maintains the point cloud height set within this range in real time.

[0025] This invention also provides a robot dynamic path planning system for unstructured environments, comprising:

[0026] The data acquisition module is used to maintain a local elevation feature grid map centered on the robot itself within a preset range in real time, and to determine the robot's motion mode in the next control cycle as a smooth mode, a shock absorption mode, or an obstacle crossing mode based on the local elevation feature grid map.

[0027] The parameter determination module is used to call the constraint weight parameter value of the corresponding cost function according to the motion mode, so that the robot cruises at high speed in the smooth mode, decelerates in the shock absorption mode, and cuts into the terrain in the forward direction in the obstacle crossing mode. The cost function is calculated based on the weighted sum of the heading, distance, speed, passability and heading vertical constraint terms.

[0028] The path generation module is used to search for multiple simulated trajectory sequences of the robot in the next control cycle based on the dynamic window method. It takes the set of velocity values ​​corresponding to the simulated trajectory sequence with the minimum cost calculated based on the cost function as the optimal solution and generates the velocity control command for the next control cycle.

[0029] The present invention 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 robot dynamic path planning method for unstructured environments as described above.

[0030] The present invention also provides a non-transitory computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the robot dynamic path planning method for unstructured environments as described above.

[0031] The present invention also provides a computer program product, including a computer program that, when executed by a processor, implements the robot dynamic path planning method for unstructured environments as described above.

[0032] The present invention provides a robot dynamic path planning method and system for unstructured environments. By using point cloud data, IMU data and the elevation features of the terrain ahead, the evaluation function of the robot's local path planning is dynamically configured during the robot's movement. This introduces environmental physical attributes as key constraints for path planning to the traditional dynamic window method. At the same time, it endows the four-wheeled robot with the "seeking advantages and avoiding disadvantages" ability similar to that of a living organism: it prioritizes efficient cruising on flat roads, while in unstructured environments such as stairs, ramps or gravel roads, it can actively plan a steady-state path that cuts vertically and slows down, effectively avoiding the risk of rollover caused by oblique obstacle crossing. Attached Figure Description

[0033] To more clearly illustrate the technical solutions in this invention or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are some embodiments of this invention. For those skilled in the art, other drawings can be obtained from these drawings without creative effort.

[0034] Figure 1 This is a flowchart illustrating the robot dynamic path planning method for unstructured environments provided by the present invention.

[0035] Figure 2 This is a schematic diagram of the structure of the robot dynamic path planning system for unstructured environments provided by the present invention;

[0036] Figure 3This is a schematic diagram of the structure of the electronic device provided by the present invention. Detailed Implementation

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

[0038] The following is combined Figure 1 This invention introduces a dynamic path planning method for robots in unstructured environments, such as... Figure 1 As shown, it includes:

[0039] Step 101: Maintain a local elevation feature grid map centered on the robot and within a preset range in real time, and determine the robot's motion mode in the next control cycle as a smooth mode, a shock absorption mode, or an obstacle crossing mode based on the local elevation feature grid map.

[0040] The robot uses a 3D LiDAR sensor mounted on it to collect point cloud data streams of the environment in real time. An inertial measurement unit (IMU) installed inside the robot body reads the robot's current angular velocity and acceleration data at high frequency and calculates the robot's real-time pitch and roll angles.

[0041] Based on the collected point cloud data stream, a local area map data is maintained in real time with the robot's center as the center and a preset range as the area. After rasterization and processing, it is converted into a local elevation feature raster map to record the height of the point cloud falling into each grid and calculate the average height value of each grid as the ground elevation estimate of that grid.

[0042] In this way, the real-time maintained local elevation feature grid map can obtain the real-time terrain elevation features in front of the robot, and thus determine the motion mode of the next control cycle based on the real-time terrain elevation features in front of the robot.

[0043] This invention equips the robot with three motion modes: a smooth mode, a shock-absorbing mode, and an obstacle-crossing mode. The smooth mode controls the robot to travel on flat ground and encourages it to move at a speed close to its maximum cruising speed. The shock-absorbing mode controls the robot to travel over rough surfaces such as grass or gravel roads and encourages it to decelerate to ensure stable movement. The obstacle-crossing mode controls the robot to travel over structured surfaces with a certain height, such as stairs or steps, to strictly constrain the robot to enter and pass over obstacles from the front, ensuring that the robot's path when entering an obstacle is aligned with the normal vector of the obstacle terrain.

[0044] Optionally, the elevation variance of all grids in the local elevation feature grid map is calculated, and the road surface ahead is determined based on the variance magnitude, so as to select the vibration reduction mode for uneven road surfaces (i.e., those with large variance).

[0045] Optionally, the height distribution characteristics of all grids in the local elevation feature grid map are statistically analyzed. If a bimodal distribution appears, it indicates that there are structured obstacles such as stairs in the robot's field of vision. In this case, to ensure the robot's passage stability, the obstacle crossing mode is selected.

[0046] Alternatively, in other cases, if the robot's posture stability is considered to be guaranteed, a smooth mode is selected to encourage the robot to cruise at high speed to complete the inspection task.

[0047] Step 102: According to the motion mode, call the constraint term weight parameter value of the corresponding cost function so that the robot cruises at high speed in the smooth mode, decelerates in the shock absorption mode, and cuts into the terrain in the forward direction in the obstacle crossing mode. The cost function is calculated based on the weighted sum of the heading term, distance term, speed term, passability term and heading vertical constraint term.

[0048] In this embodiment, an improved DWA algorithm is used to determine the robot's local travel path. The cost of each candidate trajectory searched by the DWA algorithm is calculated based on a pre-built cost function, and the travel path with the minimum cost is selected for the robot in the next control cycle.

[0049] The cost function includes a heading term, a distance term, a speed term, a passageway term, and a heading vertical constraint term.

[0050] The heading cost is used to calculate the angle deviation between the end of the candidate trajectory and the line connecting the target point. The larger the deviation, the higher the score, guiding the robot to move towards the target.

[0051] The distance term (Dist_Cost) is used to calculate the Euclidean distance of all points on the candidate trajectory from the nearest obstacle (impassable area). The closer the distance, the heavier the penalty, to ensure obstacle avoidance safety.

[0052] The velocity term (Vel_Cost) is used to calculate the difference between the current sampling linear velocity and the target cycle velocity. The higher the velocity, the lower the cost, which encourages the robot to run at a speed close to the target cruising speed and improves traffic efficiency.

[0053] The Terrain_Cost term is used to calculate the travel cost of candidate trajectories, determined based on the cost map corresponding to the local elevation feature grid map; the heading vertical constraint term is used to constrain the tangential angle at the end of the candidate trajectory. It can be understood that the target point in local path planning is determined based on the robot's global path.

[0054] Alternatively, a feasible cost function can be expressed as:

[0055] ;

[0056] In the formula, Indicates the value of the general agent. This indicates the heading vertical constraint term. For heading item, For distance terms, For speed term, For the passability item, These are the weighting coefficients for each sub-item.

[0057] Optionally, weight parameter values ​​are pre-configured for each motion mode according to the following weight configuration strategy: based on the weight parameter values ​​of each sub-item in the stable mode, the weight parameter value of the heading vertical constraint item in the stable mode is... Defined as 0, the values ​​of the other four weight parameters can be determined based on the dimensions of the sub-items, robot cruise preferences, etc., while ensuring that the speed weight is maintained. High value, terrain weight The value is set low to encourage high-speed cruising by the robot; in shock absorption mode, the speed weight is reduced. And maintain terrain weight This allows the robot to slightly sacrifice heading accuracy, mainly by reducing speed to reduce body vibration; in obstacle crossing mode, the speed weight is greatly reduced, the terrain weight is increased, and the heading vertical constraint is activated, forcing the robot to adjust its heading to align with the normal vector of the terrain gradient (i.e., cutting into the step directly), strictly preventing diagonal crossing.

[0058] Step 103: Based on the dynamic window method, multiple simulated trajectory sequences of the robot are obtained in the next control cycle. The set of velocity values ​​corresponding to the simulated trajectory sequence with the minimum cost calculated based on the cost function is taken as the optimal solution, and the velocity control command for the next control cycle is generated and issued.

[0059] In this embodiment, the Dynamic Window Method (DWA) employs the following planning loop, based on the kinematic model of the quadruped robot, in the velocity space ( A dynamic window is defined in the linear velocity-angular velocity space, and the range of this window is limited by the following three conditions:

[0060] Maximum and minimum linear and angular velocity limits for the robot;

[0061] The maximum acceleration and deceleration limits determined by the motor performance are used to limit the speed range that the robot can reach in the next time step.

[0062] The safe braking distance limit under the current environment is used to determine the maximum speed at which a robot can decelerate to zero when encountering a sudden obstacle.

[0063] Within a defined dynamic window, a gridded sampling method is used to generate multiple candidate velocity pairs. For each candidate velocity pair, the robot's motion equations are combined with discrete-time integration methods, such as Euler integrals or Runge-Kutta method, to deduce the simulated motion trajectory of the robot in the next control cycle and generate a series of trajectory sets composed of discrete pose points, representing multiple simulated trajectory sequences.

[0064] Based on this, the total cost of each simulated trajectory sequence in the next control cycle can be calculated according to the cost function determined by the parameters. The simulated trajectory sequence with the minimum cost is selected as the optimal trajectory, and the set of velocity values ​​corresponding to the trajectory is sent to the robot as the velocity control command for the next control cycle.

[0065] In this way, during the robot's movement, by combining the real-time attitude data of the IMU with the terrain features reflected in the local elevation feature grid map, the weight of the cost function is dynamically intervened by switching the robot's movement mode. Thus, when the area in front of the planned path is mainly an unstructured rough area and the robot's body attitude is stable, the robot is allowed to decelerate and pass through smoothly without forcibly constraining the entry angle, so as to ensure the smoothness and efficiency of passage.

[0066] When a structured step edge is identified ahead of the planned path, or when the IMU detects that the fuselage attitude tilt exceeds a preset threshold, the system automatically triggers an obstacle avoidance protection mechanism. This significantly increases the weight of the terrain adaptability evaluation item in the evaluation function, and simultaneously introduces a heading verticality constraint into the evaluation function, forcing the planner to abandon diagonal paths and instead choose a direction perpendicular to the step edge or contour lines to avoid one side of the wheels being suspended in the air or tipping over. The final selected optimal speed command... The commands are sent to the underlying motion controller, which interprets them as joint torques or foot position commands for the four legs, driving the robot to perform the corresponding movements.

[0067] This invention dynamically configures the evaluation function for the robot's local path planning during the robot's movement by using point cloud data, IMU data, and the elevation features of the terrain ahead. It introduces environmental physical attributes as a key constraint for path planning to the traditional dynamic window method, and at the same time endows the four-wheeled robot with the "seeking advantages and avoiding disadvantages" ability similar to that of a living organism: it prioritizes efficient cruising on flat roads, while in unstructured environments such as stairs, ramps, or gravel roads, it can actively plan a steady-state path that cuts vertically and slows down, effectively avoiding the risk of rollover caused by oblique obstacle crossing.

[0068] In the robot dynamic path planning method for unstructured environments of this invention, the step of determining whether the robot's motion mode in the next control cycle is a stable mode, a shock-absorbing mode, or an obstacle-crossing mode based on the local elevation feature grid map specifically includes:

[0069] Based on the local elevation feature raster map, the passage cost of each raster is calculated, a cost map is generated, and the height distribution characteristics are statistically analyzed based on the raster elevation values ​​to determine whether it is a unimodal or bimodal distribution.

[0070] If the terrain in front of the robot is identified as having a single-peak distribution, the height variance is higher than the preset threshold, and the robot's roll and pitch angles are both within the preset safe range, then the motion mode for the current cycle is determined to be the shock absorption mode.

[0071] If the terrain in front of the robot is identified as having a bimodal distribution, or if the robot's roll or pitch angles exceed the preset safety range, the motion mode for the current cycle is determined to be obstacle-crossing mode.

[0072] In other cases, the motion pattern of the current cycle is determined to be a steady pattern.

[0073] In existing methods, the same treatment is usually applied to rough surfaces caused by different reasons in the robot path. For example, the passage cost of the surface grid is calculated and a higher passage cost is given to the rough surface, forcing the robot to stay away from the rough surface or directly treat the corresponding grid as an obstacle.

[0074] In this invention, in order to enable the robot to cruise in complex three-dimensional spaces and ensure the robot's stable passage through rough terrain such as stairs, an enhanced state machine is embedded in the robot control program. This machine is used to monitor the robot's state and dynamically adjust the weight coefficients and constraint strategies of each sub-item in the cost function.

[0075] Optionally, the passage cost of each grid cell can be calculated based on a real-time maintained local elevation feature raster map, and a real-time cost map can be generated.

[0076] Simultaneously, the elevation values ​​of all grids in the real-time maintained local elevation feature grid map are statistically analyzed, and a real-time grid elevation histogram is constructed to determine the height distribution characteristics of the local terrain for the real-time statistical robot based on the elevation histogram.

[0077] It is understandable that when the height distribution features show a single-peak distribution, it indicates that the real-time terrain is unstructured. If the height variance is small at this time, it is a smooth road surface. If the variance is large at the same time, it is a rough road surface such as a gravel road or a lawn. When the height distribution features of any grid cell show a bimodal distribution, it indicates that there is a structured height change in the robot's field of vision, that is, obstacles such as stairs or steps have entered the robot's field of vision. It needs to be treated carefully in path planning to adjust the robot's heading.

[0078] Therefore, the robot's motion pattern in the next control cycle can be determined based on the passage cost, elevation value, and height distribution characteristics of each grid.

[0079] Specifically, before the start of each planning loop, the attitude data (Roll, Pitch) of the IMU and the classification label (single peak / double peak) of the terrain ahead of the current optimal path are read synchronously.

[0080] If the robot's current roll and pitch angles are both within the preset safe angle range (in this embodiment, both angles are less than 5°), and the real-time maintained local elevation feature grid map does not show a bimodal distribution, it indicates that the robot's field of vision is flat.

[0081] At this point, the height variance of all grids in front of the robot's center position in the real-time maintained local elevation feature grid map is calculated. If the height variance is less than a preset threshold, it indicates that the ground in front of the robot is flat, and the motion mode of the robot's next control cycle is determined to be a smooth cycle mode; otherwise, it indicates that the ground in front of the robot is rough, and the motion mode of the robot's next control cycle is determined to be a shock-absorbing mode.

[0082] If the robot exceeds the preset safe range for either roll angle or pitch angle, or if the real-time maintained local elevation feature grid map shows a bimodal distribution, it indicates that the robot is experiencing posture imbalance and is at risk of tipping over, or if there are stairs, steps, or other terrain that needs to be crossed within the robot's field of vision. Therefore, the robot's movement mode for the next cycle is determined to be obstacle crossing mode.

[0083] The robot's motion mode is determined by the above method and the corresponding cost function sub-item weight parameters are called. In obstacle crossing mode, the high terrain weight will force the algorithm to abandon those paths that are fast but cut diagonally into the steps, and instead choose paths with slower linear speed but can cut vertically into the steps. In vibration reduction mode, the robot is allowed to slightly sacrifice heading accuracy, mainly by reducing speed to reduce body vibration, thereby realizing the robot's autonomous steady-state control at the algorithm level.

[0084] In the robot dynamic path planning method for unstructured environments of this invention, the steps of calculating the passage cost of each grid cell based on the local elevation feature grid map, generating a cost map, and statistically analyzing the height distribution characteristics (unimodal or bimodal) based on the grid elevation values ​​specifically include:

[0085] Traverse the valid grids in the local elevation feature grid map, obtain the height set of the point cloud inside each valid grid, and statistically obtain the height histogram of each grid.

[0086] In this embodiment, in order to quantify the traversal difficulty of each grid in the real-time maintained local elevation feature raster map from the perspective of physical attributes, the raster value is calculated from three aspects: terrain classification features, roughness, and slope.

[0087] Specifically, the point cloud data within the robot's field of view is traversed a second time, and for each grid cell containing valid data, the height distribution dataset of its internal point cloud is obtained. , k This represents the number of point clouds within the raster.

[0088] The mean height of all point cloud data within each grid cell is calculated as the grid height. Grid cells containing valid point cloud data are identified as valid grid cells. The program calculates the grid height of each valid grid cell and constructs a height histogram of all valid grid cell regions to analyze the number of peaks and determine the real-time height distribution characteristics.

[0089] Optionally, in this embodiment, the height data histogram is set to a 2cm interval.

[0090] Based on the height histogram, the ground roughness and terrain slope of each grid are calculated, and the distribution characteristics of the height histogram of the grid are marked as unimodal or bimodal.

[0091] Cost maps corresponding to local elevation feature raster maps are generated based on the ground roughness, terrain slope, and distribution characteristics of the effective raster area, so that rasters with high ground roughness and terrain slope have higher costs, and rasters with bimodal distribution are given a preset maximum cost value.

[0092] First, the height distribution characteristics of the grid are determined to be either unimodal or bimodal based on the number of peaks in the height histogram.

[0093] If the height distribution characteristic corresponding to the current local elevation feature raster map is a unimodal distribution, then calculate the height variance of the local elevation feature raster map:

[0094] ;

[0095] In the formula, This represents the height value of the nth point cloud within the raster. This represents the average height of all point clouds within the grid.

[0096] If the variance exceeds a preset variance threshold, which is set to 0.05 in this embodiment, then the roughness cost of the raster will be... Setting it to a higher value (but not the maximum value) prompts the robot to slow down.

[0097] Optionally, variance normalization is performed on all gratings marked as having a unimodal distribution based on variance to determine the roughness cost of each unimodal distribution grating. .

[0098] If the current local elevation feature raster map corresponds to a bimodal elevation distribution, and the elevation difference between the centers of the two peaks is... Within the step height range (set to be in this embodiment) If the grid cell is identified as a step edge, it is immediately marked as a high-risk area and assigned a very high step cost. This forces path planning to be given high priority.

[0099] Furthermore, using a 3×3 Sobel operator or the central difference method, a convolution operation is performed on the elevation map to calculate the gradient of each grid cell in the X and Y axes. :

[0100] ;

[0101] If the slope angle If the slope angle exceeds the robot's maximum climbing angle, then the slope cost... Set it to infinity (to block the path); otherwise, normalize the angle value.

[0102] Calculate the base terrain cost for each grid cell using the following formula:

[0103] ;

[0104] In the formula, and This represents the weighting coefficient for the sub-item.

[0105] Subsequently, a morphological dilation algorithm is invoked, using a circular kernel with a radius equal to the robot's body radius to dilate the cost map. This ensures that if the terrain is treacherous, an area within one body radius around it is marked as a high-risk zone, preventing the robot from scraping its edges or becoming suspended in mid-air.

[0106] In the robot dynamic path planning method for unstructured environments of the present invention, the traversability term of the cost function is calculated based on the sum of grid cost values ​​of the grid corresponding to the trajectory; the heading vertical constraint term is calculated based on the normal vector angle of the tangential angle at the end of the trajectory and the elevation gradient of the terrain ahead.

[0107] Based on the aforementioned cost graph, this embodiment proposes a method for calculating the traffic efficiency term and the heading vertical constraint term of the cost function.

[0108] Optionally, for each simulated trajectory searched by DWA, every point on the trajectory is traversed. And map it to the cost graph, where the grid cost corresponding to the index is... .

[0109] Trafficability item: ;

[0110] In the formula, M is the number of time steps in the next control cycle.

[0111] This item reflects the overall bumpiness and tipping risk of the robot traveling on the trajectory. If the trajectory cuts across the stairs (passing through multiple high-roughness grids), the cumulative cost of this item will be extremely high; if the trajectory cuts vertically (shortest path), the cost of this item will be relatively low.

[0112] Evaluation items for heading verticality constraint:

[0113] ;

[0114] In the formula, To simulate the tangential angle at the end of the trajectory, This is the normal angle of the current terrain elevation gradient (i.e., the direction perpendicular to the edge of the step or the contour line of the ramp). This formula calculates the absolute value of the minimum included angle between two angles, with a value ranging from [0, 1]. π ].

[0115] After the heading verticality constraint evaluation term is activated, its weight is adjusted to a dominant large value. At this time, if the simulated trajectory is oblique (i.e., and (There is a significant deviation) This will significantly increase, leading to a substantial increase in the value of the general agent. Much higher than other paths; conversely, only when ≈ (i.e., when cutting vertically) Approaching 0, this trajectory in the overall evaluation function... It has a significant competitive advantage.

[0116] The robot dynamic path planning method for unstructured environments of this invention also includes:

[0117] The system acquires the point cloud data stream collected in real time by the lidar on the robot, acquires the robot's current angular velocity and acceleration read in real time by the inertial measurement unit on the robot, and calculates the robot's real-time pitch angle and roll angle.

[0118] After performing voxel grid downsampling and height pass-through filtering on the collected point cloud data stream, the point cloud data is converted to a preset robot coordinate system.

[0119] The robot collects point cloud data streams of the environment in real time using a 3D LiDAR on top of the robot, and at the same time reads the current angular velocity and acceleration data at high frequency through the inertial measurement unit inside the body, so as to calculate the robot's real-time pitch and roll angles.

[0120] For the collected raw point cloud data, perform two levels of preprocessing operations:

[0121] The first stage is voxel grid downsampling, which divides the 3D space into tiny voxel cubes and retains only the centroid point within each voxel. This significantly reduces the number of point clouds while preserving the geometric features of the environment, thus reducing the computational load in the subsequent stages.

[0122] Optionally, create a size of A 3D voxel mesh is generated. For all points falling within the same voxel, their geometric centroid is calculated and replaced with the original points. This step can compress the point cloud by more than 70% while preserving the geometric contours of the environment.

[0123] The second stage is height pass-through filtering, which sets the region of interest (ROI) based on the robot's physical height, and removes invalid point cloud data below the ground (such as deep pit noise) and above the robot's height (such as ceiling and tree canopy), retaining only obstacle and terrain point cloud data that affect ground walking.

[0124] Optionally, a height threshold range can be set. In this implementation, the program traverses all point clouds, removes points with z < -0.5m (deep pit noise) or z > 1.0m (ceiling, tree canopy), and retains only ground and obstacle feature points.

[0125] Based on this, a coordinate transformation is performed to convert the preprocessed point cloud data to a preset robot coordinate system, which can be either the robot base coordinate system or the world coordinate system.

[0126] Optionally, read the current robot odometry pose matrix. Point cloud in lidar coordinate system The data is standardized by transforming the data into the robot's base coordinate system through rigid body transformation.

[0127] In the robot dynamic path planning method for unstructured environments of this invention, the step of real-time maintenance of a local elevation feature grid map centered on the robot and within a preset range specifically includes:

[0128] A local scrolling grid map is established with the robot's center as the origin and follows the robot's movement. The local scrolling grid map discretizes the horizontal ground within a preset range centered on the robot itself into a two-dimensional grid of fixed size, and maintains the point cloud height set within this range in real time.

[0129] In this embodiment, the high memory consumption of global mapping is abandoned, and a robot-centric local rolling window strategy is adopted to create a local rolling grid map with the robot center as the origin and following the robot's movement.

[0130] Specifically, an N×N two-dimensional array is allocated in memory as a raster map, with each raster representing a 0.1m×0.1m area in physical space.

[0131] Based on the preprocessed point cloud data, for each point Calculate its index in the map array based on the raster resolution. :

[0132] ;

[0133] ;

[0134] The above method enables data mapping from point cloud to raster.

[0135] Furthermore, maintain a height list or incremental statistics (Sum, Count) in each raster object. When a point falls into a raster, update the average height of that raster. .

[0136] When the robot moves a distance exceeding one grid resolution, the array index shifts. The program clears the data that has moved out of view and initializes the newly entered grid area, ensuring that the map always covers the local area surrounding the robot.

[0137] The computing architecture achieved through the above method is lightweight and efficient, and can make autonomous decisions in complex local environments without relying on high-precision global maps. This significantly improves the robot's passability, stability, and intelligence in practical application scenarios such as building inspection and field exploration.

[0138] The following describes the robot dynamic path planning system for unstructured environments provided by the present invention. The robot dynamic path planning system for unstructured environments described below and the robot dynamic path planning method for unstructured environments described above can be referred to and correspond to each other.

[0139] like Figure 2 As shown, the robot dynamic path planning system for unstructured environments of the present invention includes a data acquisition module 201, a parameter determination module 202, and a path generation module 203.

[0140] The data acquisition module 201 is used to maintain a local elevation feature grid map centered on the robot itself within a preset range in real time, and to determine the robot's motion mode in the next control cycle as a smooth mode, a shock absorption mode, or an obstacle crossing mode based on the local elevation feature grid map.

[0141] The robot uses a 3D LiDAR sensor mounted on it to collect point cloud data streams of the environment in real time. An inertial measurement unit (IMU) installed inside the robot body reads the robot's current angular velocity and acceleration data at high frequency and calculates the robot's real-time pitch and roll angles.

[0142] Based on the collected point cloud data stream, a local area map data is maintained in real time with the robot's center as the center and a preset range as the area. After rasterization and processing, it is converted into a local elevation feature raster map to record the height of the point cloud falling into each grid and calculate the average height value of each grid as the ground elevation estimate of that grid.

[0143] In this way, the real-time maintained local elevation feature grid map can obtain the real-time terrain elevation features in front of the robot, and thus determine the motion mode of the next control cycle based on the real-time terrain elevation features in front of the robot.

[0144] This invention equips the robot with three motion modes: a smooth mode, a shock-absorbing mode, and an obstacle-crossing mode. The smooth mode controls the robot to travel on flat ground and encourages it to move at a speed close to its maximum cruising speed. The shock-absorbing mode controls the robot to travel over rough surfaces such as grass or gravel roads and encourages it to decelerate to ensure stable movement. The obstacle-crossing mode controls the robot to travel over structured surfaces with a certain height, such as stairs or steps, to strictly constrain the robot to enter and pass over obstacles from the front, ensuring that the robot's path when entering an obstacle is aligned with the normal vector of the obstacle terrain.

[0145] Optionally, the elevation variance of all grids in the local elevation feature grid map is calculated, and the road surface ahead is determined based on the variance magnitude, so as to select the vibration reduction mode for uneven road surfaces (i.e., those with large variance).

[0146] Optionally, the height distribution characteristics of all grids in the local elevation feature grid map are statistically analyzed. If a bimodal distribution appears, it indicates that there are structured obstacles such as stairs in the robot's field of vision. In this case, to ensure the robot's passage stability, the obstacle crossing mode is selected.

[0147] Alternatively, in other cases, if the robot's posture stability is considered to be guaranteed, a smooth mode is selected to encourage the robot to cruise at high speed to complete the inspection task.

[0148] The parameter determination module 202 is used to call the constraint weight parameter value of the corresponding cost function according to the motion mode, so that the robot cruises at high speed in the smooth mode, decelerates in the shock reduction mode, and cuts into the terrain in the forward direction in the obstacle crossing mode. The cost function is calculated based on the weighted sum of the heading term, distance term, speed term, passability term and heading vertical constraint term.

[0149] In this embodiment, an improved DWA algorithm is used to determine the robot's local travel path. The cost of each candidate trajectory searched by the DWA algorithm is calculated based on a pre-built cost function, and the travel path with the minimum cost is selected for the robot in the next control cycle.

[0150] Alternatively, a feasible cost function can be expressed as:

[0151] ;

[0152] In the formula, Indicates the value of the general agent. This indicates the heading vertical constraint term. These are the weighting coefficients for each sub-item.

[0153] Optionally, weight parameter values ​​are pre-configured for each motion mode according to the following weight configuration strategy: based on the weight parameter values ​​of each sub-item in the stable mode, the weight parameter value of the heading vertical constraint item in the stable mode is... Defined as 0, the values ​​of the other four weight parameters can be determined based on the dimensions of the sub-items, robot cruise preferences, etc., while ensuring that the speed weight is maintained. High value, terrain weight The value is set low to encourage high-speed cruising by the robot; in shock absorption mode, the speed weight is reduced. And maintain terrain weight This allows the robot to slightly sacrifice heading accuracy, mainly by reducing speed to reduce body vibration; in obstacle crossing mode, the speed weight is greatly reduced, the terrain weight is increased, and the heading vertical constraint is activated, forcing the robot to adjust its heading to align with the normal vector of the terrain gradient (i.e., cutting into the step directly), strictly preventing diagonal crossing.

[0154] The path generation module 203 is used to search for multiple simulated trajectory sequences of the robot in the next control cycle based on the dynamic window method, take the set of velocity values ​​corresponding to the simulated trajectory sequence with the minimum cost calculated based on the cost function as the optimal solution, and generate the velocity control command for the next control cycle.

[0155] In this embodiment, the Dynamic Window Method (DWA) employs the following planning loop, based on the kinematic model of the quadruped robot, in the velocity space ( A dynamic window is defined in the linear velocity-angular velocity space, and the range of this window is limited by the following three conditions:

[0156] Maximum and minimum linear and angular velocity limits for the robot;

[0157] The maximum acceleration and deceleration limits determined by the motor performance are used to limit the speed range that the robot can reach in the next time step.

[0158] The safe braking distance limit under the current environment is used to determine the maximum speed at which a robot can decelerate to zero when encountering a sudden obstacle.

[0159] Within a defined dynamic window, a gridded sampling method is used to generate multiple candidate velocity pairs. For each candidate velocity pair, the robot's motion equations are combined with discrete-time integration methods, such as Euler integrals or Runge-Kutta method, to deduce the simulated motion trajectory of the robot in the next control cycle and generate a series of trajectory sets composed of discrete pose points, representing multiple simulated trajectory sequences.

[0160] Based on this, the total cost of each simulated trajectory sequence in the next control cycle can be calculated according to the cost function determined by the parameters. The simulated trajectory sequence with the minimum cost is selected as the optimal trajectory, and the set of velocity values ​​corresponding to the trajectory is sent to the robot as the velocity control command for the next control cycle.

[0161] In this way, during the robot's movement, by combining the real-time attitude data of the IMU with the terrain features reflected in the local elevation feature grid map, the weight of the cost function is dynamically intervened by switching the robot's movement mode. Thus, when the area in front of the planned path is mainly an unstructured rough area and the robot's body attitude is stable, the robot is allowed to decelerate and pass through smoothly without forcibly constraining the entry angle, so as to ensure the smoothness and efficiency of passage.

[0162] When a structured step edge is identified ahead of the planned path, or when the IMU detects that the fuselage attitude tilt exceeds a preset threshold, the system automatically triggers an obstacle avoidance protection mechanism. This significantly increases the weight of the terrain adaptability evaluation item in the evaluation function, and simultaneously introduces a heading verticality constraint into the evaluation function, forcing the planner to abandon diagonal paths and instead choose a direction perpendicular to the step edge or contour lines to avoid one side of the wheels being suspended in the air or tipping over. The final selected optimal speed command... The commands are sent to the underlying motion controller, which interprets them as joint torques or foot position commands for the four legs, driving the robot to perform the corresponding movements.

[0163] This invention dynamically configures the evaluation function for the robot's local path planning during the robot's movement by using point cloud data, IMU data, and the elevation features of the terrain ahead. It introduces environmental physical attributes as a key constraint for path planning to the traditional dynamic window method, and at the same time endows the four-wheeled robot with the "seeking advantages and avoiding disadvantages" ability similar to that of a living organism: it prioritizes efficient cruising on flat roads, while in unstructured environments such as stairs, ramps, or gravel roads, it can actively plan a steady-state path that cuts vertically and slows down, effectively avoiding the risk of rollover caused by oblique obstacle crossing.

[0164] Figure 3 An example is a schematic diagram of the physical structure of an electronic device, such as... Figure 3 As shown, the electronic device may include: a processor 310, a communication interface 320, a memory 330, and a communication bus 340, wherein the processor 310, the communication interface 320, and the memory 330 communicate with each other through the communication bus 340. The processor 310 can call logic instructions in the memory 330 to execute a robot dynamic path planning method for unstructured environments. This method includes: maintaining a local elevation feature grid map centered on the robot within a preset range in real time; determining the robot's motion mode for the next control cycle as a smooth mode, a damping mode, or an obstacle-crossing mode based on the local elevation feature grid map; calling the constraint weight parameter values ​​of the corresponding cost function according to the motion mode, so that the robot cruises at high speed in the smooth mode, decelerates in the damping mode, and enters the terrain forward in the obstacle-crossing mode; wherein the cost function is calculated based on a weighted sum of heading, distance, speed, traversability, and vertical heading constraints; obtaining multiple simulated trajectory sequences for the robot in the next control cycle based on DWA search; selecting the set of speed values ​​corresponding to the simulated trajectory sequence with the lowest cost calculated based on the cost function as the optimal solution; and generating and issuing speed control commands for the next control cycle.

[0165] Furthermore, the logical instructions in the aforementioned memory 330 can be implemented as software functional units and, when sold or used as independent products, can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of the present invention, essentially, or the part that contributes to the prior art, or a part of the technical solution, can be embodied in the form of a software product. This computer software product is stored in a storage medium and includes several instructions to cause a computer device (which may be a personal computer, server, or network device, etc.) to execute all or part of the steps of the methods described in the various embodiments of the present invention. The aforementioned storage medium includes various media capable of storing program code, such as USB flash drives, portable hard drives, read-only memory (ROM), random access memory (RAM), magnetic disks, or optical disks.

[0166] On the other hand, the present invention also provides a computer program product, which includes a computer program that can be stored on a non-transitory computer-readable storage medium. When the computer program is executed by a processor, the computer can execute the robot dynamic path planning method for unstructured environments provided by the above methods. The method includes: maintaining a local elevation feature grid map centered on the robot within a preset range in real time; determining the robot's motion mode in the next control cycle as a smooth mode, a shock-absorbing mode, or an obstacle-crossing mode based on the local elevation feature grid map; calling the constraint weight parameter values ​​of the corresponding cost function according to the motion mode, so that the robot cruises at high speed in the smooth mode, decelerates in the shock-absorbing mode, and cuts into the terrain in the forward direction in the obstacle-crossing mode, wherein the cost function is calculated based on the weighted sum of the heading, distance, speed, passability, and vertical heading constraints; obtaining multiple simulated trajectory sequences of the robot in the next control cycle based on DWA search; taking the set of speed values ​​corresponding to the simulated trajectory sequence with the minimum cost calculated based on the cost function as the optimal solution; and generating and issuing the speed control command for the next control cycle.

[0167] In another aspect, the present invention also provides a non-transitory computer-readable storage medium storing a computer program thereon. When executed by a processor, the computer program implements a robot dynamic path planning method for unstructured environments provided by the methods described above. This method includes: maintaining a local elevation feature grid map centered on the robot within a preset range in real time; determining the robot's motion mode in the next control cycle as a smooth mode, a shock-absorbing mode, or an obstacle-crossing mode based on the local elevation feature grid map; calling the constraint weight parameter values ​​of the corresponding cost function according to the motion mode, so that the robot cruises at high speed in the smooth mode, decelerates in the shock-absorbing mode, and cuts into the terrain in the forward direction in the obstacle-crossing mode, wherein the cost function is calculated based on the weighted sum of the heading, distance, speed, passability, and vertical heading constraints; obtaining multiple simulated trajectory sequences of the robot in the next control cycle based on DWA search; taking the set of speed values ​​corresponding to the simulated trajectory sequence with the minimum cost calculated based on the cost function as the optimal solution; and generating and issuing speed control commands for the next control cycle.

[0168] The device embodiments described above are merely illustrative. The units described as separate components may or may not be physically separate. The components shown as units may or may not be physical units; that is, they may be located in one place or distributed across multiple network units. Some or all of the modules can be selected to achieve the purpose of this embodiment according to actual needs. Those skilled in the art can understand and implement this without any creative effort.

[0169] 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 part that contributes to the prior art, can be embodied in the form of a software product. 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.

[0170] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, and not to limit them; although the present invention 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; and these 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 the present invention.

Claims

1. A dynamic path planning method for robots in unstructured environments, characterized in that, include: The local elevation feature grid map centered on the robot itself and within a preset range is maintained in real time. Based on the local elevation feature grid map and the robot's roll angle and pitch angle data at the current moment, the robot's motion mode in the next control cycle is determined to be either smooth mode, shock absorption mode, or obstacle crossing mode. According to the motion mode, the constraint weight parameter value of the corresponding cost function is called so that the robot cruises at high speed in the smooth mode, decelerates in the shock absorption mode, and cuts into the terrain in the forward direction in the obstacle crossing mode. The cost function is calculated based on the weighted sum of the heading, distance, speed, passability and heading vertical constraint terms. The robot obtains multiple simulated trajectory sequences for the next control cycle based on the dynamic window method. The set of velocity values ​​corresponding to the simulated trajectory sequence with the minimum cost calculated based on the cost function is taken as the optimal solution, and the velocity control command for the next control cycle is generated and issued.

2. The robot dynamic path planning method for unstructured environments according to claim 1, characterized in that, The step of determining whether the robot's motion mode in the next control cycle is a smooth mode, a damping mode, or an obstacle-crossing mode based on the local elevation feature grid map and the robot's roll and pitch angle data at the current moment specifically includes: Based on the local elevation feature raster map, the passage cost of each raster is calculated, a cost map is generated, and the height distribution characteristics are statistically analyzed based on the raster elevation values ​​to determine whether it is a unimodal or bimodal distribution. If the terrain in front of the robot is identified as having a single-peak distribution, the height variance is higher than the preset threshold, and the robot's roll and pitch angles are both within the preset safe range, then the motion mode for the current cycle is determined to be the shock absorption mode. If the terrain in front of the robot is identified as having a bimodal distribution, or if the robot's roll or pitch angles exceed the preset safety range, the motion mode for the current cycle is determined to be obstacle-crossing mode. In other cases, the motion pattern of the current cycle is determined to be a steady pattern.

3. The robot dynamic path planning method for unstructured environments according to claim 2, characterized in that, The steps of calculating the passage cost of each grid cell based on the local elevation feature grid map, generating a cost map, and statistically analyzing the height distribution characteristics based on the grid elevation values ​​to determine whether it is a unimodal or bimodal distribution specifically include: Traverse the valid grids in the local elevation feature grid map, obtain the height set of the point cloud inside each valid grid, and statistically obtain the height histogram of each grid. Based on the height histogram, the ground roughness and terrain slope of each grid are calculated, and the distribution characteristics of the height histogram of the grid are marked as unimodal or bimodal. Cost maps corresponding to local elevation feature raster maps are generated based on the ground roughness, terrain slope, and distribution characteristics of the effective raster area, so that rasters with high ground roughness and terrain slope have higher costs, and rasters with bimodal distribution are given a preset maximum cost value.

4. The robot dynamic path planning method for unstructured environments according to claim 3, characterized in that, The traversability term of the cost function is calculated based on the sum of the grid costs of the corresponding grid cells of the trajectory; the heading vertical constraint term is calculated based on the normal vector angle of the tangential angle at the end of the trajectory and the elevation gradient of the terrain ahead.

5. The robot dynamic path planning method for unstructured environments according to claim 1, characterized in that, Also includes: The system acquires the point cloud data stream collected in real time by the lidar on the robot, acquires the robot's current angular velocity and acceleration read in real time by the inertial measurement unit on the robot, and calculates the robot's real-time pitch angle and roll angle. After performing voxel grid downsampling and height pass-through filtering on the collected point cloud data stream, the point cloud data is converted to a preset robot coordinate system.

6. The robot dynamic path planning method for unstructured environments according to claim 5, characterized in that, The steps of real-time maintenance of a local elevation feature grid map centered on the robot and within a preset range specifically include: A local scrolling grid map is established with the robot's center as the origin and follows the robot's movement. The local scrolling grid map discretizes the horizontal ground within a preset range centered on the robot itself into a two-dimensional grid of fixed size, and maintains the point cloud height set within this range in real time.

7. A robot dynamic path planning system for unstructured environments, characterized in that, include: The data acquisition module is used to maintain a local elevation feature grid map centered on the robot itself within a preset range in real time. Based on the local elevation feature grid map and the robot's roll angle and pitch angle data at the current moment, the module determines the robot's motion mode in the next control cycle as a smooth mode, a shock absorption mode, or an obstacle-crossing mode. The parameter determination module is used to call the constraint weight parameter value of the corresponding cost function according to the motion mode, so that the robot cruises at high speed in the smooth mode, decelerates in the shock absorption mode, and cuts into the terrain in the forward direction in the obstacle crossing mode. The cost function is calculated based on the weighted sum of the heading, distance, speed, passability and heading vertical constraint terms. The path generation module is used to search for multiple simulated trajectory sequences of the robot in the next control cycle based on the dynamic window method. It takes the set of velocity values ​​corresponding to the simulated trajectory sequence with the minimum cost calculated based on the cost function as the optimal solution and generates the velocity control command for the next control cycle.

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 robot dynamic path planning method for unstructured environments 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 the processor, it implements the robot dynamic path planning method for unstructured environments as described in any one of claims 1 to 6.

10. A computer program product, comprising a computer program, characterized in that, When the computer program is executed by the processor, it implements the robot dynamic path planning method for unstructured environments as described in any one of claims 1 to 6.

Images