A method and system for navigation decision of a field pasture quadruped grazing robot

Abstract

This invention discloses a navigation decision-making method and system for a quadruped herding robot in a wild pasture, belonging to the field of mobile robot navigation technology. First, by integrating multi-source terrain geometric features such as slope, roughness, and elevation difference, and introducing observation quality factors and directional consistency penalties, a Bayesian log-probability model is used for temporal probability updates. Then, combined with a three-level decision logic of "soft threshold - counting lag - hard constraint," a continuous passable probability is output, achieving a refined quantitative assessment of risks in complex local terrains such as potholes, steep slopes, and vegetation obstruction. Based on this, an adaptive multi-objective cost function based on situational awareness is constructed, dynamically integrating three types of situational factors: environmental uncertainty, motion risk, and proximity risk. The weights between safety and efficiency are adjusted in real time, thereby achieving an online trade-off between path length, travel time, and passage safety, ultimately generating a locally optimal navigation path for the quadruped robot in a natural grassland pasture environment.

Description

Technical Field

[0001] This invention relates to the field of mobile robot navigation technology, and more specifically to a navigation decision-making method and system for a quadruped herding robot in a field pasture. Background Technology

[0002] Currently, quadruped robots have been explored for autonomous grazing operations in wild pastures. They have superior mobility on unstructured terrain. Existing research usually uses environmental representations such as two-dimensional grids and 2.5D elevation maps, and employs methods such as geometric feature thresholding, graph search, or probabilistic learning to assess drivability and plan paths, which to some extent enables robots to navigate autonomously in grassland, slope, and other scenarios.

[0003] However, natural grasslands and pastures are characterized by significant topographic relief, fragmented small-scale terrain (such as pits, steep slopes, and haystacks), and vegetation that causes occlusion and noise on the perception system. This makes it difficult for existing methods to accurately assess local small-scale terrain risks and easily leads to misjudgments of drivability. At the same time, fixed-weight cost functions cannot dynamically balance safety and drivability in long-term, wide-area operations, resulting in quadruped robots still facing problems of poor reliability and insufficient adaptability in autonomous navigation in complex grassland environments.

[0004] Therefore, how to propose a navigation decision-making method and system for quadruped herding robots in wild pastures, to achieve accurate assessment of the drivability of complex small terrains and dynamic optimization of multi-objective paths, and to improve the overall safety, environmental adaptability and long-term operational stability of quadruped robots in the complex and unstructured environment of natural grasslands, is a problem that urgently needs to be solved by those skilled in the art. Summary of the Invention

[0005] In view of this, the present invention provides a navigation decision-making method and system for a quadruped herding robot in a field pasture, which improves the adaptability and stability of the quadruped herding robot's path planning and ensures the safety and smoothness of the robot's autonomous mobile operation in the field.

[0006] To achieve the above objectives, the present invention adopts the following technical solution: On one hand, this invention discloses a navigation decision-making method for a quadruped herding robot in a wild pasture, comprising the following steps: S1. Acquire point cloud data of the environment surrounding the quadruped robot and construct a local elevation map; S2. Based on the local elevation map, extract multi-source terrain geometric features, integrate observation quality factor and direction consistency penalty, adopt Bayesian log probability update model, output continuous passability probability, and map the continuous passability probability to passability determination result; S3. Construct a multi-objective cost function, which includes a distance term, a terrain risk term, and a time term; wherein, the weight coefficients of the distance term, the time term, and the terrain risk term are dynamically adjusted by environmental uncertainty factors, motion risk factors, and proximity risk factors; S4. Construct a local planning window centered on the current position of the quadruped herding robot, and search for the path that minimizes the multi-objective cost function within the passability grid area defined by the passability determination result as the optimal navigation path.

[0007] Preferably, S1 includes: Environmental point cloud data is collected by a lidar mounted on a quadruped herding robot; The environmental point cloud data is preprocessed by the edge computing unit, and a local elevation map is constructed with the robot body as the origin. The local elevation map uses regular raster encoding of terrain elevation information.

[0008] Preferably, S2 includes: S21. In the local elevation map, roughness, elevation difference and maximum slope are extracted as multi-source terrain geometric features; S22. Based on the principal component analysis method, extract the first principal axis of the local terrain point cloud set as the terrain mobility direction, calculate the angle between the robot's motion direction and the terrain mobility direction, and construct a direction consistency penalty term based on the angle; S23. Obtain the point density, surface consistency, and slope stability of the local point cloud, and then perform weighted fusion to obtain the observation quality factor; S24. Based on the multi-source terrain geometric features, the directional consistency penalty term and the observation quality factor, a Bayesian log-probability update model is used to update the temporal probability of each grid in the local elevation map and output the continuous passability probability. S25. Based on the continuous passability probability, and combined with the slope, elevation difference and roughness, the passability determination result is output in grid units through a three-level decision logic of soft threshold-counting lag-hard constraint.

[0009] Preferably, S24 includes: Based on the multi-source terrain geometric features and the direction consistency penalty term, the observation likelihood under the traversable class and the observation likelihood under the intraversable class are calculated, and the log-likelihood ratio is obtained; wherein, the likelihood confidence is adjusted according to the observation quality factor. The log-likelihood ratio is used to perform time-series cumulative updates of the log-probability, which is then mapped to a continuous passable probability using the sigmoid function.

[0010] Preferably, S25 includes: First-level soft threshold comparison: The continuous passability probability is compared with a preset soft threshold. When the continuous passability probability is higher than the soft threshold, favorable evidence is generated; when it is lower than the soft threshold, unfavorable evidence is generated. Level 2 counting lag: The number of consecutive unfavorable evidence or consecutive favorable evidence is accumulated by a counter. When the number of consecutive unfavorable evidence exceeds a preset counting threshold, it is determined to be impassable. When the number of consecutive favorable evidence exceeds a preset counting threshold, it is determined to be permissible. Level 3 hard limit: When any of the features of slope, elevation difference or roughness exceeds the corresponding preset hard threshold, it is determined that the road is impassable and the counter is reset.

[0011] Preferably, the multi-objective cost function The calculation formula is as follows: ; In the formula, For distance, , This is the range gain coefficient. The Euclidean distance between adjacent nodes; This is a terrain risk item; For time terms, , This is the time gain coefficient. The estimated running time for the robot to navigate through the current edge; , , These are the weighting coefficients for the corresponding distance, terrain risk, and time items, respectively.

[0012] Preferably, the range gain coefficient Terrain risk items Time gain coefficient The calculation formulas are as follows: ; ; ; In the formula, This is the uncertainty adjustment coefficient. As an uncertainty factor, This is the proximity risk adjustment coefficient. For proximity risk factors, This is the risk weighting coefficient for the probability of passage. Let be the probability of continuous passage. This is the slope risk weighting coefficient. For slope penalty items, This is the roughness risk weighting coefficient. For roughness penalty term, This is the time adjustment factor for sports risk. As a risk factor for exercise, This is an adjustment factor for the time of near-risk.

[0013] Preferably, the uncertainty factor The calculation formula is: ; In the formula, This is a truncation function. For probability entropy, For observation correction factors, The effective observation ratio; The proximity risk factor The calculation formula is: ; In the formula, To mitigate the risk of obstacles, For local terrain risks, This serves as the maximum reference value for local terrain risk. This is the obstacle risk weighting coefficient. This refers to the terrain risk weighting coefficient. The sports risk factors The calculation formula is: ; In the formula, The linear velocity of the machine body. The maximum linear velocity under safe operating conditions for the robot. For the tilt angle, For the maximum safe tilt angle, For speed risk weighting coefficient, This represents the attitude risk weighting coefficient.

[0014] On the other hand, the present invention also discloses a navigation and decision-making system for a quadruped herding robot in a wild pasture, comprising: The data acquisition and mapping module is used to acquire point cloud data of the environment surrounding the quadruped robot and construct a local elevation map. The accessibility assessment module is used to extract multi-source terrain geometric features based on the local elevation map, integrate the observation quality factor and the direction consistency penalty, adopt a Bayesian log probability update model, output the continuous accessibility probability, and map the continuous accessibility probability to the accessibility judgment result. A cost function construction module is used to construct a multi-objective cost function, which includes a distance term, a terrain risk term, and a time term; wherein, the weight coefficients of the distance term, the time term, and the terrain risk term are dynamically adjusted by environmental uncertainty factors, motion risk factors, and proximity risk factors; The local path planning module is used to construct a local planning window centered on the current position of the quadruped herding robot, and search for the path that minimizes the multi-objective cost function within the passability grid area defined by the passability determination result as the optimal navigation path.

[0015] As can be seen from the above technical solution, compared with the prior art, this invention discloses a navigation decision-making method and system for a quadruped herding robot in a wild pasture. First, a local elevation map is constructed and multi-source terrain geometric features are extracted. An observation quality factor and a direction consistency penalty are integrated, and a Bayesian log-probability update model is used to output continuous traversability probabilities. Then, a stable traversability judgment result is obtained through a three-level decision logic. Based on this, a multi-objective cost function including distance, terrain risk, and time terms is constructed. The weights of each cost term are dynamically adjusted using environmental uncertainty factors, motion risk factors, and proximity risk factors. Finally, the optimal navigation path is searched within the traversable area. This invention achieves refined risk assessment of complex local terrain such as potholes, steep slopes, and vegetation obstruction through temporal probability accumulation and observation quality adjustment, avoiding decision oscillations caused by single misjudgments. Simultaneously, through adaptive multi-objective trade-offs based on situational awareness, the path length and travel time are dynamically optimized while ensuring safety, thereby significantly improving the reliability, adaptability, and overall efficiency of the quadruped robot's autonomous navigation in unstructured environments such as natural grasslands and pastures. Attached Figure Description

[0016] To more clearly illustrate the technical solutions in the embodiments of the present 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 only embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on the provided drawings without creative effort.

[0017] Figure 1 A flowchart of the method provided by the present invention; Figure 2 This is a schematic diagram of a data acquisition platform; Figure 3 This is a flowchart of the feasibility assessment process; Figure 4 This is a flowchart of the adaptive weight update process based on situational awareness. Detailed Implementation

[0018] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0019] On one hand, embodiments of the present invention disclose a navigation decision-making method for a quadruped herding robot in a wild pasture, such as... Figure 1 As shown, it includes the following steps: S1. Acquire point cloud data of the environment surrounding the quadruped robot and construct a local elevation map.

[0020] To verify the characteristics of various typical grassland micro-topography features such as pits, steep slopes and dense grass in natural grazing environments, a quadruped robot equipped with lidar was used to collect data in natural grassland pastures.

[0021] This embodiment collects data on grassy scenes with strong occlusion and uncertain visibility, steep slope scenes that are easily affected by posture constraints and terrain slope, and pit scenes with strong local abrupt changes and micro-topographic undulations.

[0022] In response to the complex and ever-changing environmental conditions of grasslands and pastures, such as Figure 2 The design of the data acquisition platform shown comprehensively considers key performance indicators such as the size of the robot chassis, climbing ability, movement speed, obstacle crossing performance, and endurance, while also taking into account economic cost factors. Ultimately, a quadruped robot was selected as the mobile platform. Figure 2 (a) integrates core modules such as edge computing development board, LiDAR and 4G router. Figure 2 (b) Among them, the edge computing module, as the core computing unit of the platform, undertakes the tasks of point cloud data storage and mapping, and at the same time receives user instructions and sends control instructions to the robot chassis; the Hesai XT-16 LiDAR, as the main perception module, is responsible for collecting high-resolution point cloud data to obtain grassland environmental features; and the 4G router ensures remote communication and data transmission between the robot platform and the user terminal.

[0023] In this embodiment, the XT-16 LiDAR has a horizontal field of view of 360°, a vertical field of view of 30° (-15°~+15°), a wavelength of 905nm, supports ROS / ROS2 and CUDA, and is suitable for deployment on the JetsonOrinNX edge computing platform. Table 1 shows the coverage area, total trajectory length, acquisition time, and total point cloud data for three typical scenarios. The acquired data includes a global 3D point cloud map (PCD), robot motion trajectory (PCD), and optimized pose parameters (TXT), all of which are stored on the quadruped robot platform via the JetsonOrinNX computing module.

[0024] Input validation is performed to ensure that the point cloud and voxel mesh are usable, and a local elevation map is constructed around the node to be evaluated.

[0025] Table 1 Data Acquisition Parameters

[0026] S2. Based on the local elevation map, extract multi-source terrain geometric features, integrate the observation quality factor and the direction consistency penalty, adopt the Bayesian log probability update model, output the continuous passability probability, and map the continuous passability probability to the passability judgment result.

[0027] To mitigate accessibility assessment errors caused by loss of micro-topographic features and vegetation obstruction, a Continuous Bayesian Traversability Assessment (CBTA) model is designed, referencing... Figure 3 This method integrates multi-source terrain features such as slope, roughness, and elevation difference, extracts geometric evidence from local elevation maps, and performs temporal Bayesian updates using log-odds methods to reliably determine grassland accessibility. Specifically, it includes: S21. In the local elevation map, extract roughness and elevation difference. ) and maximum slope ( () as a geometric feature of multi-source terrain.

[0028] Taking the robot body as the origin, for each central grid structure Local terrain Collect its point cloud set Simultaneously take Neighborhood Used for slope and PCA analysis.

[0029] The formula for calculating roughness is:

[0030] Height difference The calculation formula is: ; Maximum slope The calculation formula is .

[0031] in It is the elevation difference in the eight adjacent directions. It is the voxel side length.

[0032] S22. Based on the principal component analysis method, extract the first principal axis of the local terrain point cloud set as the terrain mobility direction, calculate the angle between the robot's motion direction and the terrain mobility direction, and construct a direction consistency penalty term based on the angle.

[0033] exist Neighborhood Take the first spindle Let the direction of movement be the most traversable direction of the terrain. The formula for calculating the directional consistency penalty dir_penalty is as follows: .

[0034] S23. Obtain the point density of the local point cloud. Surface consistency With slope stability The observation quality factor is obtained by weighted fusion. The calculation formula is as follows: ; in This is the default weight.

[0035] (1) Point density quality factor Calculation

[0036] Centered on the current analysis node, a 5×5 grid neighborhood is constructed as the local point cloud analysis region, and the number of grid cells with valid elevation observations within the local point cloud region is denoted as . Point density quality factor The calculation formula is:

[0037] in This is a preset point density reference threshold, used to characterize the minimum number of effective observations required for a local point cloud to achieve sufficient coverage. In this embodiment, we take... =30; The value range of is [0,1]. When hour Taking the maximum value of 1 indicates that the point cloud coverage in the current local area is sufficient and the observation support is relatively reliable.

[0038] (2) Surface uniformity quality factor Calculation

[0039] For local point cloud analysis areas 1 effective elevation observation value First, calculate its average elevation. and elevation variance Based on this, the surface consistency quality factor is determined. Mean elevation The calculation formula is:

[0040] Elevation variance The calculation formula is:

[0041] Furthermore, surface consistency quality factor The calculation formula is:

[0042] in, Indicates the first The elevation observation values ​​of each effective grid cell are in meters. This represents the arithmetic mean of the effective elevation observations within the local point cloud analysis area; This represents the elevation variance, used to reflect the degree of undulation of the local land surface. The larger the value, the rougher the terrain surface and the worse the surface uniformity. is the surface uniformity sensitivity coefficient, used to adjust the response intensity of the surface uniformity quality factor to the degree of surface roughness. In this embodiment, is taken as... .

[0043] Surface consistency quality factor The range of values ​​is When the surface is completely flat, that is... hour, Take the maximum value of 1.

[0044] (3) Slope stability quality factor Calculation

[0045] Calculate the local slope angles along the four directions of east, west, south, and north of the current analysis node, and take the maximum slope angle in each of the four directions as the representative slope to determine the slope stability quality factor. The local slope angle in the i-th direction The calculation formula is:

[0046] Representative slope The calculation formula is:

[0047] Furthermore, define the slope consistency quantity. for:

[0048] The slope stability quality factor The calculation formula is:

[0049] in, This is the elevation value of the current analysis node, in meters; For the first Elevation values ​​of adjacent nodes in each direction, in meters; This represents the horizontal distance between adjacent nodes, and its value is equal to the raster voxel resolution, in meters. For the first The slope angle of the terrain in each direction, in radians; The largest slope angle among the four directions is used to characterize the steepest slope in a local area; Used to characterize the impact of local slope changes on stability. The larger the value, the steeper the local slope and the worse the stability. This serves as the lower limit for slope protection, used to prevent numerical degradation caused by extremely small slope angles. In this embodiment, we take... ; This is the minimum guaranteed value of the slope stability quality factor, used to ensure that the basic confidence level is maintained even under extreme terrain conditions. In this embodiment, it is taken as... Slope stability quality factor The range of values ​​is .

[0050] In summary, by analyzing the point density quality factor Surface consistency quality factor and slope stability quality factor Weighted fusion can comprehensively reflect the observation integrity, surface smoothness, and slope availability of the current local point cloud region, thereby obtaining a more stable and reliable observation quality factor. This provides support for subsequent terrain assessment and route planning.

[0051] S24. Based on multi-source terrain geometric features, directional consistency penalty term, and observation quality factor, a Bayesian log-probability update model is used to update the temporal probability of each grid cell in the local elevation map, outputting the continuous passability probability, including: The observation likelihoods for the traversable and inaccessible classes are calculated based on the Gaussian model with multi-source terrain geometric features and directional consistency penalty terms, and the log-likelihood ratio is obtained. In order to suppress observation noise and quantify confidence, the likelihood confidence is adjusted according to the observation quality factor.

[0052] Specifically, to robustly integrate multi-dimensional observations in online scenarios, this embodiment employs a log-odds approach to implement Bayesian updates. Let the event... Indicates passage is permitted; observation features Directional consistency penalty Observation quality The Bayesian posterior is calculated as follows: ; It should be noted that the aforementioned Bayesian posterior formula is used to explain the combined effect of observation likelihood and prior probability on posterior probability under single observation conditions. In the temporal implementation of this embodiment, logarithmic odds are used for recursive updates to facilitate online cumulative calculation and improve numerical stability. Furthermore, the directional consistency penalty adjusts the relative magnitudes of the observation likelihoods of the drivable and inaccessible classes into the log-likelihood ratio. and followed It also participates in the historical logarithmic probability update.

[0053] log-odds makes the information from a single observation linearly additive to history. This avoids the instability of values ​​from probability multiplication and facilitates online accumulation.

[0054] (1) Calculation of observational likelihood

[0055] In this embodiment, for each grid cell, its multi-source terrain geometric feature vector is extracted.

[0056] and directional consistency penalty As the current observation input. Indicates the local maximum slope. Indicates surface roughness. Indicates local elevation difference. This indicates the degree of deviation between the current direction of movement and the easily accessible direction of the terrain. Based on the observed input, this is applied to the accessible class... and impassable classes Under the given conditions, an independent Gaussian likelihood model is established to characterize the degree of support of the current grid for the two types of traffic states.

[0057] Preferably, in passable classes In this context, local terrain typically exhibits gentle slopes, low roughness, and small elevation differences, thus allowing for the observation of likelihood patterns. The calculation formula is:

[0058] In impassable category Below, local terrain typically features steep slopes, high roughness, and significant elevation differences, thus making it impassable. (Observational likelihood class) The calculation formula is:

[0059] in, The expression for the non-normalized Gaussian kernel function is:

[0060] in, and These represent the expected value and standard deviation of the corresponding feature under the corresponding category, respectively.

[0061] In this embodiment, the passable class The parameter values ​​are: ; ;

[0062] Impassable The parameter values ​​are: ; ;

[0063] Among them, the direction consistency penalty term Through multiplicative factors Attenuation is performed on passable similar phenomena through a multiplicative factor. The likelihood of impassability is enhanced so that the evidence of impassability is strengthened as the deviation between the direction of movement and the drivable direction of the local terrain increases. Based on the above two types of observational likelihoods, a log-likelihood ratio is constructed. The calculation formula is as follows:

[0064] in, A lower bound for numerical stability is set to prevent the denominator from being zero or the logarithmic operation from diverging.

[0065] (2) Adjustment of likelihood credibility

[0066] To mitigate the adverse effects of observation noise on accessibility assessment results under conditions such as sparse point clouds, severe local surface undulations, or unstable slopes, an observation quality factor is introduced in this embodiment. Confidence adjustment is performed on the log-likelihood ratio.

[0067] Observational quality factor The point density quality factor in step S23 Surface consistency quality factor and slope stability quality factor The weighted fusion is used to comprehensively characterize the reliability of current observations. When the local point cloud coverage is sufficient, the surface is relatively flat, and the slope change is stable, The value is close to 1; when the point cloud coverage is insufficient or the local terrain features fluctuate greatly, The value approaches the lower bound.

[0068] Preferably, the log-likelihood ratio is adjusted using a linear scaling method to obtain the effective log-likelihood ratio after quality correction. :

[0069] in, This represents valid observational evidence after correction for the observation quality factor. When the time is right, it indicates that the current observation quality is high and the evidence from the current observation is preserved relatively completely; when When the current observation quality is low, the contribution of the observation to the historical logarithmic probability is close to zero, thus automatically suppressing the accumulation of false evidence when point cloud coverage is insufficient or terrain features are unstable.

[0070] (3) Time-series cumulative update

[0071] To obtain the quality-adjusted effective log-likelihood ratio Then, the historical passability probability of the current grid is updated recursively over time. Preferably, the passability probability of the previous time step is first updated. Converted to logarithmic probability form and updated using a soft saturation method, its calculation formula is as follows:

[0072] in, This represents the upper limit of saturation for logarithmic odds. This is achieved by introducing the hyperbolic tangent function. Implementing soft saturation constraints prevents excessive accumulation of log odds due to consecutive same-direction observations, thus avoiding the system losing its responsiveness to subsequent reverse observations. After obtaining the updated log odds, the sigmoid function is used to map them to the continuous passability probability at the current time step. The calculation formula is as follows:

[0073] in, This represents the probability that a grid cell is continuously passable at the current moment. The closer it is to 1, the more likely the current grid cell is to be passable; The closer it is to 0, the more likely the current grid is to be impassable.

[0074] In summary, this implementation method constructs an observation likelihood model based on multi-source terrain features and directional consistency information, adjusts the reliability of the log-likelihood ratio by combining the observation quality factor, and then achieves continuous, stable and noise-resistant time-series estimation of local terrain traffic status through soft-saturated log-probability updates and probability mapping.

[0075] S25. Based on the continuous passability probability, and combined with slope, elevation difference and roughness, the passability determination result is output in grid units through a three-level decision logic of soft threshold-counting lag-hard constraint.

[0076] To make continuous Bayesian passability probability The smooth mapping to discrete "accessible / inaccessible" decisions is achieved by adopting a "soft threshold-counting lag-hard constraint fallback" approach in this embodiment.

[0077] Level 1 soft threshold comparison: The continuous passability probability is compared with a preset soft threshold. When the continuous passability probability is higher than the soft threshold, favorable evidence is generated; when it is lower than the soft threshold, unfavorable evidence is generated. Level 2 counting lag: The number of consecutive unfavorable evidence or consecutive favorable evidence is accumulated by a counter. When the number of consecutive unfavorable evidence exceeds a preset counting threshold, it is determined to be impassable. When the number of consecutive favorable evidence exceeds a preset counting threshold, it is determined to be permissible. Level 3 hard limit: When any feature of slope, elevation difference or roughness exceeds the corresponding preset hard threshold, it is determined to be impassable and the counter is reset.

[0078] In practice, to avoid the oscillation problem caused by hard thresholds, a soft threshold evidence index is calculated. A value of 1 indicates one instance of adverse evidence; A value of 0 indicates one instance of favorable evidence.

[0079] The probability of continuous passage at the current moment. With preset soft threshold The comparisons are made, and corresponding favorable or unfavorable evidence is generated. Specifically, a soft threshold evidence index is defined. for:

[0080] in, For indicator functions; This is a preset soft threshold. When... hour, This indicates that the current observation has produced one piece of unfavorable evidence, suggesting that the grid has a high risk of passage; when hour, This indicates that the current observation provides favorable evidence that the grid is currently more likely to be passable.

[0081] By setting a soft threshold, decision oscillations caused by hard switching of continuous probabilities can be avoided, making the discrete decision-making process insensitive to small probability fluctuations.

[0082] Counters provide historical information, prevent forgetting, and improve decision stability. For the counting threshold, continuous +1 adverse / favorable observation, respectively entering / exiting the impassable state.

[0083] By accumulating the number of consecutive unfavorable or favorable pieces of evidence using a counter, historical information can be used to suppress state reversals caused by a single misjudgment. Let the... The counter for the time is Its update formula is:

[0084] in, The preset counting threshold is used; Used to measure evidence indicators Mapped to :when When the counter increments by 1, it indicates that the progress is increasing in the impassable direction; when When the counter decrements by 1, it indicates that the path has been restored to the passable direction. and Used to constrain the counter Within a certain range, the system avoids the infinite accumulation of values. By retaining historical information of continuous, unidirectional evidence, the counter prevents the system from immediately altering its judgment due to a single isolated anomaly observation, thus improving judgment stability. When the grid is impassable, it is determined that the grid is impassable; when When the grid is open, it is determined that the grid is passable; when When the time is right, it means that the current evidence is not yet sufficient and the situation is in an intermediate transitional state.

[0085] To prevent the robot from entering extremely dangerous areas, in addition to probabilistic judgment and counting lag, a hard constraint mechanism based on terrain extreme characteristics is further implemented. The hard constraint is only determined for the maximum slope and elevation difference. Hard constraint indices are defined. for:

[0086] in, The maximum slope of the current grid is consistent with the definition in step S21; This is the elevation difference of the current grid, consistent with the definition in step S21; This is a hard threshold for slope. The elevation difference hard threshold; It represents the logical "OR".

[0087] when or At that time, regardless of the current consecutive passability probability and counter Regardless of its status, the grid is immediately deemed impassable, and the counter is reset. This mechanism serves as a last line of defense, used to forcibly intercept extremely dangerous terrain features such as steep slopes and potholes.

[0088] Based on the above three-level decision-making logic, the grid-level discrete accessibility determination result is obtained. :

[0089] in, This indicates that the road is impassable. This indicates a low-confidence transition state; This indicates that passage is permitted. When the counter reaches the threshold or triggers a hard limit, the output is... This indicates that the grid does not allow the robot to pass through; when the counter is in the middle range, the output is... This indicates that the current evidence is insufficient, and a higher cost can be assigned in path planning; when the counter reaches zero, the output is... This indicates that the grid allows the robot to pass through.

[0090] In summary, this embodiment obtains instantaneous evidence through soft threshold comparison, suppresses misjudgments caused by single observation noise through counting lag mechanism, and forcibly intercepts extremely dangerous areas through hard restriction mechanism targeting maximum slope and elevation difference, thereby achieving stable determination of the drivability of local terrain.

[0091] S3. Construct a multi-objective cost function, which includes a distance term, a terrain risk term, and a time term; wherein, the weight coefficients of the distance term, the time term, and the terrain risk term are dynamically adjusted by environmental uncertainty factors, motion risk factors, and proximity risk factors.

[0092] Terrain navigability assessment models provide a risk basis for path planning. However, in actual navigation, robots not only need to avoid impassable areas but also need to comprehensively consider factors such as path distance, motion state, and nearby obstacles to achieve a dynamic trade-off between safety and efficiency. This embodiment designs an adaptive cost function based on situational awareness. By introducing situational weighting and distance gain, the path planning decision is allowed to self-balance between safety and efficiency while maintaining maximum safety.

[0093] To ensure safe and autonomous navigation, the path planner can comprehensively consider both shorter time and shorter path. This embodiment proposes an adaptive cost function based on situational awareness, which maps the environment and robot state to a [0,1] risk index by introducing three types of situational factors and dynamically adjusts the weights accordingly: environmental uncertainty... Increase safety weight and decrease distance weight when the risk of motion is high; At high speeds (high speed / large tilt), steering and terrain constraints increase; proximity risks also increase. At higher altitudes (obstacle proximity / bad terrain), safety costs are significantly increased and distance parameters are suppressed; see [link to relevant documentation]. Figure 4 .

[0094] Let from adjacent nodes To the node The total cost is Then the formula for calculating the multi-objective cost function is:

[0095] in, For distance, This is a terrain risk item. For time; , , These are the weighting coefficients for the corresponding distance, terrain risk, and time items, respectively.

[0096] Distance Term The calculation formula is:

[0097] in, This is the range gain coefficient. Adjacent nodes With nodes The Euclidean distance between them.

[0098] Terrain risk items The calculation formula is:

[0099] in, This is the risk weighting coefficient for the probability of passage; The continuous passability probability is output in step S24; This is the slope risk weighting coefficient. This is a slope penalty item; This is the roughness risk weighting coefficient. This is a roughness penalty term.

[0100] Preferably, a slope penalty item The calculation formula is:

[0101] Where roughness is the surface roughness of the current raster; This is the reference value for roughness normalization.

[0102] Time item The calculation formula is:

[0103] in, This is the time gain coefficient. This is the estimated running time for the robot to navigate through the current edge.

[0104] Furthermore, the range gain coefficient The calculation formula is:

[0105] in, This is the uncertainty adjustment coefficient. For uncertainty factors; This is the proximity risk adjustment coefficient. These are nearby risk factors.

[0106] Time gain coefficient The calculation formula is:

[0107] in, This is the time adjustment factor for sports risk; This is an adjustment factor for the time of near-risk.

[0108] To achieve the above dynamic adjustment, this embodiment further defines an environmental uncertainty factor. Proximity risk factors and sports risk factors as follows.

[0109] Uncertainty factors The calculation formula is:

[0110] in, This is a truncation function used to constrain the calculation results within a preset range to ensure that the values ​​of the uncertainty factors are within the valid range. Probability of continuous accessibility The probability entropy of is calculated using the following formula:

[0111] in, This is the maximum value of the binary probability entropy, used to normalize the probability entropy; This is the observation correction coefficient, used to measure the effect of the proportion of effective observations on the suppression of uncertainty factors; The effective observation ratio. When When the value is close to 0.5 and the effective observation ratio is low, the uncertainty is higher.

[0112] Proximity risk factors Obstacle risk and local terrain risks Obtained by weighting and then cropped. Within the interval, the calculation formula is:

[0113] in, The distance between the robot and the nearest obstacle; Let be the obstacle risk function, and its range of values ​​is: and followed It increases while monotonically decreasing; Local terrain risks; This is the maximum reference value for local terrain risk, used for normalization of local terrain risk. This is the distance obstacle risk weighting coefficient, used to adjust the contribution of distance obstacle risk to the proximity risk factor; This is the terrain risk weighting coefficient, used to adjust the contribution of local terrain risk to adjacent risk factors. (Motion risk factor) The calculation formula is:

[0114] in, The linear velocity of the machine body; The maximum linear velocity under safe operating conditions for the robot; For attitude tilt angle; For the maximum safe tilt angle; This refers to the speed risk weighting coefficient. This represents the attitude risk weighting coefficient.

[0115] In summary, this embodiment constructs a multi-objective cost function consisting of a distance term, a terrain risk term, and a time term, and dynamically adjusts each cost component by combining environmental uncertainty factors, motion risk factors, and proximity risk factors, thereby achieving a comprehensive trade-off between path length, travel time, and traffic safety, and providing a cost basis for subsequent local path search.

[0116] S4. Construct a local planning window centered on the current position of the quadruped herding robot. Within the passable grid area defined by the passability determination result, search for the path that minimizes the multi-objective cost function as the optimal navigation path.

[0117] On the other hand, the present invention also discloses a navigation and decision-making system for a quadruped herding robot in a wild pasture, comprising: The data acquisition and mapping module is used to acquire point cloud data of the environment surrounding the quadruped robot and construct a local elevation map. The accessibility assessment module is used to extract multi-source terrain geometric features based on local elevation maps, integrate observation quality factors and directional consistency penalties, adopt a Bayesian log-probability update model, output continuous accessibility probabilities, and map continuous accessibility probabilities to accessibility judgment results. The cost function construction module is used to construct a multi-objective cost function, which includes a distance term, a terrain risk term, and a time term. The weight coefficients of the distance term, time term, and terrain risk term are dynamically adjusted by environmental uncertainty factors, motion risk factors, and proximity risk factors. The local path planning module is used to construct a local planning window centered on the current position of the quadruped herding robot. Within the passable grid area defined by the passability determination result, it searches for the path that minimizes the multi-objective cost function as the optimal navigation path.

[0118] The various embodiments in this specification are described in a progressive manner, with each embodiment focusing on its differences from other embodiments. Similar or identical parts between embodiments can be referred to interchangeably. For the apparatus disclosed in the embodiments, since they correspond to the methods disclosed in the embodiments, the description is relatively simple; relevant parts can be referred to the method section.

[0119] The above description of the disclosed embodiments enables those skilled in the art to make or use the invention. Various modifications to these embodiments will be readily apparent to those skilled in the art, and the general principles defined herein may be implemented in other embodiments without departing from the spirit or scope of the invention. Therefore, the invention is not to be limited to the embodiments shown herein, but is to be accorded the widest scope consistent with the principles and novel features disclosed herein.

Claims

1. A navigation and decision-making method for a quadruped herding robot in a wild pasture, characterized in that, Includes the following steps: S1. Acquire point cloud data of the environment surrounding the quadruped robot and construct a local elevation map; S2. Based on the local elevation map, extract multi-source terrain geometric features, integrate observation quality factor and direction consistency penalty, adopt Bayesian log probability update model, output continuous passability probability, and map the continuous passability probability to passability determination result; S3. Construct a multi-objective cost function, which includes a distance term, a terrain risk term, and a time term; wherein, the weight coefficients of the distance term, the time term, and the terrain risk term are dynamically adjusted by environmental uncertainty factors, motion risk factors, and proximity risk factors; S4. Construct a local planning window centered on the current position of the quadruped herding robot, and search for the path that minimizes the multi-objective cost function within the passability grid area defined by the passability determination result as the optimal navigation path.

2. The navigation and decision-making method for a quadruped herding robot in a field pasture according to claim 1, characterized in that, S1 includes: Environmental point cloud data is collected by a lidar mounted on a quadruped herding robot; The environmental point cloud data is preprocessed by the edge computing unit, and a local elevation map is constructed with the robot body as the origin. The local elevation map uses regular raster encoding of terrain elevation information.

3. The navigation and decision-making method for a quadruped herding robot in a field pasture according to claim 2, characterized in that, S2 include: S21. In the local elevation map, roughness, elevation difference and maximum slope are extracted as multi-source terrain geometric features; S22. Based on the principal component analysis method, extract the first principal axis of the local terrain point cloud set as the terrain mobility direction, calculate the angle between the robot's motion direction and the terrain mobility direction, and construct a direction consistency penalty term based on the angle; S23. Obtain the point density, surface consistency, and slope stability of the local point cloud, and then perform weighted fusion to obtain the observation quality factor; S24. Based on the multi-source terrain geometric features, the directional consistency penalty term and the observation quality factor, a Bayesian log-probability update model is used to update the temporal probability of each grid in the local elevation map and output the continuous passability probability. S25. Based on the continuous passability probability, and combined with the slope, elevation difference and roughness, the passability determination result is output in grid units through a three-level decision logic of soft threshold-counting lag-hard constraint.

4. The navigation and decision-making method for a quadruped herding robot in a field pasture according to claim 3, characterized in that, S24 includes: Based on multi-source terrain geometric features and directional consistency penalty terms, the observation likelihoods under the traversable class and the intraversable class are calculated, and the log-likelihood ratio is calculated; wherein, the likelihood confidence is adjusted according to the observation quality factor. The log-likelihood ratio is used to perform time-series cumulative updates of the log-probability, which is then mapped to a continuous passable probability using the sigmoid function.

5. The navigation and decision-making method for a quadruped herding robot in a field pasture according to claim 3, characterized in that, S25 includes: First-level soft threshold comparison: The continuous passability probability is compared with a preset soft threshold. When the continuous passability probability is higher than the soft threshold, favorable evidence is generated; when it is lower than the soft threshold, unfavorable evidence is generated. Level 2 counting lag: The number of consecutive unfavorable evidence or consecutive favorable evidence is accumulated by a counter. When the number of consecutive unfavorable evidence exceeds a preset counting threshold, it is determined to be impassable. When the number of consecutive favorable evidence exceeds a preset counting threshold, it is determined to be permissible. Level 3 hard limit: When any of the features of slope, elevation difference or roughness exceeds the corresponding preset hard threshold, it is determined that the road is impassable and the counter is reset.

6. The navigation and decision-making method for a quadruped herding robot in a field pasture according to claim 1, characterized in that, The multi-objective cost function The calculation formula is as follows: ； In the formula, For distance term, and , This is the range gain coefficient. The Euclidean distance between adjacent nodes; This is a terrain risk item; It is a time term, and , This is the time gain coefficient. The estimated running time for the robot to navigate through the current edge; , , These are the weighting coefficients for the corresponding distance, terrain risk, and time items, respectively.

7. A navigation and decision-making method for a quadruped herding robot in a field pasture according to claim 6, characterized in that, Range gain coefficient Terrain risk items Time gain coefficient The calculation formulas are as follows: ； ； ； In the formula, This is the uncertainty adjustment coefficient. As an uncertainty factor, This is the proximity risk adjustment coefficient. For proximity risk factors, This is the risk weighting coefficient for the probability of passage. Let be the probability of continuous passage. This is the slope risk weighting coefficient. For slope penalty items, This is the roughness risk weighting coefficient. For roughness penalty term, This is the time adjustment factor for sports risk. As a risk factor for exercise, This is an adjustment factor for the time of near-risk.

8. The navigation and decision-making method for a quadruped herding robot in a field pasture according to claim 7, characterized in that, The uncertainty factor The calculation formula is: ； In the formula, This is a truncation function. For probability entropy, For observation correction factors, The effective observation ratio; The proximity risk factor The calculation formula is: ； In the formula, To mitigate the risk of obstacles, For local terrain risks, This serves as the maximum reference value for local terrain risk. This is the obstacle risk weighting coefficient. This refers to the terrain risk weighting coefficient. The sports risk factors The calculation formula is: ； In the formula, The linear velocity of the machine body. The maximum linear velocity under safe operating conditions for the robot. For the tilt angle, For the maximum safe tilt angle, For speed risk weighting coefficient, This represents the attitude risk weighting coefficient.

9. A navigation and decision-making system for a quadruped herding robot in a wild pasture, characterized in that, include: The data acquisition and mapping module is used to acquire point cloud data of the environment surrounding the quadruped robot and construct a local elevation map. The accessibility assessment module is used to extract multi-source terrain geometric features based on the local elevation map, integrate the observation quality factor and the direction consistency penalty, adopt a Bayesian log probability update model, output the continuous accessibility probability, and map the continuous accessibility probability to the accessibility judgment result. A cost function construction module is used to construct a multi-objective cost function, which includes a distance term, a terrain risk term, and a time term; wherein, the weight coefficients of the distance term, the time term, and the terrain risk term are dynamically adjusted by environmental uncertainty factors, motion risk factors, and proximity risk factors; The local path planning module is used to construct a local planning window centered on the current position of the quadruped herding robot, and search for the path that minimizes the multi-objective cost function within the passability grid area defined by the passability determination result as the optimal navigation path.

Images