Elevation map path planning method and system based on terrain adaptive potential energy field

Abstract

The application provides a height map path planning method and system based on a terrain adaptive potential field, and relates to the technical field of path planning. The method comprises the following steps: calculating the slope and roughness of the position of an intelligent agent, setting a terrain adaptive gain, dynamically adjusting the slope and roughness, and combining a basic repulsive field to obtain a terrain adaptive repulsive field and a composite potential field; determining the movement direction of the intelligent agent based on the composite potential field, and determining the next position of the intelligent agent; in the process of the intelligent agent moving to the next position, judging whether the next position of the intelligent agent needs to be locally re-planned based on a preset environment change judgment rule; when re-planning is needed, the terrain adaptive repulsive field and the dynamic obstacle repulsive field are recalculated, the composite potential field is updated, and the next position of the intelligent agent is determined again. The application can effectively perceive the terrain, adaptively adjust the planning strategy, and take into account high precision, high efficiency and strong robustness.

Description

Technical Field

[0001] This invention belongs to the field of path planning technology, and in particular relates to a path planning method and system based on terrain adaptive potential energy field elevation map. Background Technology

[0002] Traditional methods face significant challenges in tasks involving path planning in complex terrain using two-dimensional elevation maps (2.5D elevation maps):

[0003] First, such algorithms are prone to getting stuck in local optima. That is, due to the choice of initial path points or search direction, the algorithm may converge to a non-global optimal path too early, resulting in the final planned path length or cost not being optimal.

[0004] Secondly, complex elevation maps contain a large amount of obstacle and undulating terrain information, requiring classic methods such as the A* algorithm and Dijkstra's algorithm to evaluate a massive number of map grid nodes. As the map size and terrain complexity increase, the computational cost increases dramatically, and its time complexity often fails to meet real-time requirements, limiting its feasibility in large-scale or dynamic application scenarios.

[0005] Finally, real-world environments often involve dynamic changes, such as dynamic obstacles or real-time terrain changes. However, the traditional methods mentioned above are mainly designed for static scenarios and lack efficient environmental adaptability. When the environment changes, the global planning process usually needs to be restarted, resulting in sluggish path adjustment response and making it difficult to meet the real-time planning requirements of dynamic environments. Summary of the Invention

[0006] To overcome the shortcomings of the prior art, this invention provides a method and system for elevation map path planning based on terrain adaptive potential energy field. It constructs a terrain adaptive repulsive field model with terrain perception capability, which can effectively perceive the terrain, adaptively adjust the planning strategy, and take into account high accuracy, high efficiency and strong robustness.

[0007] To achieve the above objectives, one or more embodiments of the present invention provide the following technical solutions:

[0008] The first aspect of this invention provides a path planning method for elevation maps based on terrain-adaptive potential energy fields.

[0009] The elevation map path planning method based on terrain-adaptive potential field includes the following steps:

[0010] Obtain the elevation map and the location of the agent on the elevation map;

[0011] Calculate the slope and roughness of the agent's position, set the terrain adaptive gain, dynamically adjust the slope and roughness, and combine it with the basic repulsive field to obtain the terrain adaptive repulsive field.

[0012] Based on the terrain-adaptive repulsive field, a composite potential energy field is obtained by combining the target gravitational field and the dynamic obstacle repulsive field;

[0013] The direction of motion of the agent is determined based on the composite potential energy field, and the next position of the agent is determined.

[0014] As the agent moves to the next position, it determines whether a local replanning of the agent's next position is needed based on a preset environmental change discrimination rule.

[0015] When replanning is required, the terrain-adaptive repulsive field and the dynamic obstacle repulsive field are recalculated, and the composite potential field is updated.

[0016] Based on the updated composite potential field, the agent's next position is determined.

[0017] A second aspect of the present invention provides an elevation map path planning system based on terrain-adaptive potential energy fields.

[0018] A terrain-adaptive potential field-based elevation map path planning system includes:

[0019] The data acquisition module is configured to acquire an elevation map and the location of the agent on the elevation map;

[0020] The terrain adaptation module is configured to: calculate the slope and roughness of the agent's position, set the terrain adaptation gain, dynamically adjust the slope and roughness, and combine it with the basic repulsive field to obtain the terrain-adaptive repulsive field.

[0021] The composite potential energy field calculation module is configured to: obtain the composite potential energy field based on the terrain-adaptive repulsive field, combined with the target gravitational field and the dynamic obstacle repulsive field;

[0022] The overall planning module is configured to: determine the agent's direction of motion and determine the agent's next position based on the composite potential energy field;

[0023] The local replanning judgment module is configured to: determine whether local replanning of the agent's next position is needed based on preset environmental change judgment rules during the process of the agent moving to the next position;

[0024] The composite potential field update module is configured to recalculate the terrain-adaptive repulsive field and the dynamic obstacle repulsive field and update the composite potential field when replanning is required.

[0025] The local replanning execution module is configured to: redetermine the agent's next position based on the updated composite potential field.

[0026] The above one or more technical solutions have the following beneficial effects:

[0027] This invention provides a method and system for elevation map path planning based on a terrain-adaptive potential energy field. The path planning problem is modeled as the motion process of an agent in a continuous potential energy field. A navigation path is generated by constructing a dynamically perceptive potential energy field model and calculating its negative gradient streamlines. Specifically, the potential energy field consists of a global target gravitational field, a terrain-adaptive repulsive field, and a dynamic obstacle repulsive field. The gravitational field guides the agent towards the target point, while the repulsive field generates avoidance effects based on terrain features and obstacle distribution. Path generation is achieved by solving for the streamlines formed by the negative gradient direction of the potential energy field. The agent moves towards the target point along these streamlines in a steepest descent manner.

[0028] The core innovation of this invention lies in constructing a terrain-adaptive repulsive field model with terrain perception capabilities. This model dynamically adjusts the intensity coefficient of the terrain repulsive field by analyzing the elevation and slope data of the agent's location in real time. Specifically, when the slope exceeds a safety threshold, the slope repulsive gain increases exponentially with increasing slope; when the terrain roughness increases, the roughness repulsive gain is linearly positively correlated. This dynamic adjustment mechanism enables the potential energy field to accurately reflect the actual passage risk of the terrain, providing a physically meaningful continuous field environment for subsequent path generation.

[0029] In the path generation mechanism, the algorithm of this invention uses a negative gradient streamline calculation method instead of the traditional discrete search. The negative gradient vector field is obtained by solving the spatial partial derivative of the composite potential field, and a numerical integration method (such as the Euler method or the Runge-Kutta method) is applied to iteratively generate a sequence of path points starting from the starting point. This process directly utilizes the continuity and smoothness of the gradient flow, avoiding the local optima problem caused by node sampling in discrete search algorithms, while reducing the computational complexity from O(n log n) of the traditional algorithm to the order of O(n).

[0030] To adapt to dynamic environmental changes, the algorithm framework of this invention supports incremental updates and local replanning of the composite potential field. When a sudden change in terrain elevation or obstacle displacement is detected, only the repulsive field components of the affected area need to be updated, and the local gradient flow calculation is restarted from the current position while freezing the planned path. This mechanism quickly locates the changed area through spatial hash mapping, and combined with motion consistency constraints, ensures a smooth transition in path updates, achieving a real-time response capability with a replanning latency of less than 50ms.

[0031] Advantages of additional aspects of the invention will be set forth in part in the description which follows, and in part will be obvious from the description, or may be learned by practice of the invention. Attached Figure Description

[0032] The accompanying drawings, which form part of this invention, are used to provide a further understanding of the invention. The illustrative embodiments of the invention and their descriptions are used to explain the invention and do not constitute an improper limitation of the invention.

[0033] Figure 1 This is a flowchart of the method in Example 1.

[0034] Figure 2 This is a flowchart of the potential energy field construction process in Example 1.

[0035] Figure 3 This is a schematic diagram of gradient flow path generation in Example 1.

[0036] Figure 4 This is a schematic diagram of the incremental update mechanism in Example 1.

[0037] Figure 5 This is a system hardware architecture diagram for Example 1. Detailed Implementation

[0038] It should be noted that the following detailed descriptions are exemplary and intended to provide further illustration of the invention. Unless otherwise specified, all technical and scientific terms used herein have the same meaning as commonly understood by one of ordinary skill in the art to which this invention pertains.

[0039] It should be noted that the terminology used herein is for the purpose of describing particular implementations only and is not intended to limit the exemplary implementations of the present invention.

[0040] Where there is no conflict, the embodiments and features in the embodiments of the present invention can be combined with each other.

[0041] Example 1

[0042] like Figure 1 As shown in the figure, this embodiment discloses an elevation map path planning method based on terrain adaptive potential energy field.

[0043] The elevation map path planning method based on terrain-adaptive potential field includes the following steps:

[0044] Obtain the elevation map and the location of the agent on the elevation map;

[0045] Calculate the slope and roughness of the agent's position, set the terrain adaptive gain, dynamically adjust the slope and roughness, and combine it with the basic repulsive field to obtain the terrain adaptive repulsive field.

[0046] Based on the terrain-adaptive repulsive field, a composite potential energy field is obtained by combining the target gravitational field and the dynamic obstacle repulsive field;

[0047] The direction of motion of the agent is determined based on the composite potential energy field, and the next position of the agent is determined.

[0048] As the agent moves to the next position, it determines whether a local replanning of the agent's next position is needed based on a preset environmental change discrimination rule.

[0049] When replanning is required, the terrain-adaptive repulsive field and the dynamic obstacle repulsive field are recalculated, and the composite potential field is updated.

[0050] Based on the updated composite potential field, the agent's next position is determined.

[0051] The technical solution of this embodiment will now be explained in detail.

[0052] (I) Terrain-Adaptive Potential Field Modeling

[0053] (1) Elevation map preprocessing

[0054] Input 2.5D elevation map data and perform real-time analysis and structured processing of terrain features, specifically including:

[0055] 1. Slope Calculation

[0056] The slope value is calculated using the gradient magnitude of the elevation matrix:

[0057] ;

[0058] in, Represents the planar coordinates of the agent's current position; This represents the elevation gradient in the x-direction; This represents the elevation gradient in the y-direction; for abbreviation, Represents coordinate points The elevation value at that location. This represents the slope value of the agent's current position.

[0059] 2. Roughness Calculation

[0060] The degree of terrain undulation is quantified by the standard deviation of local neighborhood elevation. (typically) ) Calculate the standard deviation of elevation:

[0061]

[0062] in, Indicates the local index coordinates of the neighborhood grid; Indicates the neighborhood range; Indicates the total number of grid cells in the neighborhood; The average elevation of the neighborhood is expressed by the following formula: ; Indicates index coordinate point The elevation value at that location. This represents the roughness value of the agent's current position.

[0063] 3. Feasibility domain labeling:

[0064] Determine the passable area based on slope and roughness threshold:

[0065]

[0066] in, Safety slope angle threshold (typical value) ), This represents the maximum permissible roughness. Indicates the current position The feasible region labeling results are as follows: 1 indicates that the region is feasible, and 0 indicates that the region is not feasible.

[0067] Technical effect: Through real-time generated slope field Roughness field and feasible region markers A terrain feature perception matrix is ​​constructed to provide a decision-making basis for dynamic parameter adjustment of the potential energy field. Among them, slope detection prevents the risk of overturning, roughness quantification characterizes the degree of terrain turbulence, and feasible region constraint delineates the boundary of the path search space.

[0068] (2) Adaptive repulsive field modeling

[0069] Based on the terrain feature parameters generated from the preprocessing of elevation maps, a repulsive field model that dynamically responds to terrain risks is constructed, as detailed below:

[0070] 1. Construction of the basic repulsive field:

[0071] Define the basic repulsive field function for obstacle avoidance:

[0072]

[0073] The basic repulsion function Defined as:

[0074]

[0075] In the formula: Indicates the current position To the Obstacle Center The Euclidean distance; Indicates the first The center coordinates of the obstacles Indicates the first The x-coordinate of the center of each obstacle Indicates the first The ordinate of the center of each obstacle; Indicates the total number of static obstacles; Indicates the repulsive force intensity coefficient; This indicates the radius of the repulsive field. Indicates the current position The basic repulsive field; This represents the fundamental repulsive force function.

[0076] 2. Terrain-Adaptive Gain Calculation:

[0077] By dynamically adjusting the repulsive force intensity based on slope and roughness, a gain function is constructed:

[0078] Slope gain function (to address overturning risk):

[0079]

[0080] in, Slope gain coefficient ( ), Nonlinear adjustment index (typical value) ), The safe slope threshold (value) ); Current location The slope gain function.

[0081] Roughness gain function (to cope with the risk of bumps):

[0082]

[0083] in, Roughness gain coefficient ( ); Current location The roughness gain function.

[0084] Composite gain function:

[0085]

[0086] in, Current location Terrain-adaptive gain.

[0087] 3. Adaptive repulsive field synthesis

[0088] The terrain-adaptive gain is coupled to the base repulsive field to form the final terrain-adaptive repulsive field:

[0089]

[0090] in, It is a terrain-adaptive repulsive field.

[0091] The technical effects of adaptive repulsive field modeling:

[0092] When the slope exceeds the safety threshold It exhibits a secondary growth, significantly increasing the repulsive force intensity in steep slope regions; as roughness increases, The avoidance weight for uneven terrain is linearly increased. The two are coupled multiplicatively to achieve a coordinated response to the risks of complex terrain.

[0093] (3) Definition of a complete potential energy field

[0094] Based on goal orientation and terrain risk constraints, a composite potential energy field model integrating multiple physics fields is constructed, and its mathematical definition is:

[0095]

[0096] in, Indicates the target gravitational field; This represents a terrain-adaptive repulsive field. Represents the repulsive force field of a dynamic obstacle; Indicates the current position exist The composite potential energy field at any given moment. express time. Indicates the current position The target gravitational field; Indicates the current position Terrain-adaptive repulsive field; Indicates the current position exist The dynamic obstacle repulsive field at any given moment.

[0097] The target gravitational field drives global navigation, the terrain-adaptive repulsive field responds to static terrain risks, and the dynamic obstacle repulsive field completes time-varying threat avoidance.

[0098] 1. Target gravitational field model

[0099] The agent is guided to move toward the target using a quadratic potential energy function:

[0100]

[0101] In the formula: The coordinates of the target point; This is the gravitational gain coefficient (typical value range 0.5 to 2.0).

[0102] 2. Terrain-Adaptive Repulsive Field Model

[0103] Dynamically coupled terrain features and obstacle avoidance:

[0104]

[0105] in For terrain-adaptive gain, Based on the fundamental repulsive force function, This represents the total number of static obstacles.

[0106] 3. Dynamic obstacle repulsive field model

[0107] Real-time response to time-varying threats from moving obstacles; defining a time-varying repulsive field to address these threats:

[0108]

[0109] The repulsive force function of a single obstacle is defined as:

[0110]

[0111] In the formula: Indicates the current position of the agent To the The time-varying distance of a dynamic obstacle; Indicates the first A dynamic obstacle at any time coordinates Indicates the first A dynamic obstacle at any time x-coordinate Indicates the first A dynamic obstacle at any time The ordinate; Indicates the total number of dynamic obstacles; Indicates the dynamic repulsion coefficient; Indicates the radius of influence of dynamic obstacles. This represents the repulsive force function of a single obstacle.

[0112] (ii) Path generation algorithm for negative gradient streamlines

[0113] (1) Gradient field calculation

[0114] Based on the constructed composite potential field The specific steps for calculating the negative gradient vector field generated by the driving path are as follows:

[0115] 1. Definition of the gradient of the total potential energy field

[0116]

[0117] The path motion direction is the negative gradient direction:

[0118]

[0119] This represents the Hamiltonian operator (gradient operator). Represents the navigation force vector field; This represents the gradient of the composite potential field; This represents the gradient of the target's gravitational field; The gradient represents the terrain-adaptive repulsive field. This represents the gradient of the repulsive force field of a dynamic obstacle.

[0120] 2. Analytical Calculation of Gradient Field

[0121]

[0122] 3. Terrain-Adaptive Repulsive Field Gradient Calculation

[0123] Use the product rule to process the gain factor With the fundamental repulsive field Coupling relationship:

[0124]

[0125] The partial derivative of the gain factor is:

[0126]

[0127] The partial derivative of the fundamental repulsive field is:

[0128]

[0129] Indicates the first A dynamic obstacle coordinate.

[0130] 4. Calculation of gradient in dynamic obstacle field

[0131]

[0132] The gradient function of a single obstacle :

[0133]

[0134] Directional gradient The same calculation applies.

[0135] (2) Gradient streamline integral

[0136] Based on the calculated negative gradient vector field Continuous paths are generated using numerical integration methods. The specific steps are as follows:

[0137] 1. Streamline Integral Initialization

[0138] set up:

[0139]

[0140] Indicates the starting position of the agent; Indicates the x and y coordinates of the starting position; Indicates the spatial step size (fixed step size, not time step size). This indicates the upper limit of iteration safety (to prevent infinite loops).

[0141] 2. Streamline Iterative Calculation

[0142] The first-order Euler method is used for recursive calculation of the path point sequence:

[0143]

[0144] In the formula: For the first Step path point coordinates, Indicates the first x-coordinate of the path point Indicates the first The ordinate of the path point; For the first Step path point coordinates; For the first The negative gradient vector at the current timestamp and the coordinates of the step path point; The magnitude of the negative gradient vector (normalization ensures a constant step size). For the current timestamp ( (Environmental sampling cycle); The start time, This indicates the current iteration step.

[0145] 3. Termination Condition Determination

[0146] Iteration stops when any of the following conditions are met:

[0147]

[0148] This is the convergence threshold; The coordinates of the target point, Represents the x-coordinate of the target point. This represents the ordinate of the target point.

[0149] 4. Path sequence output

[0150] Generate an ordered set of path points:

[0151]

[0152] in, This is the first waypoint; This is the second path point; This is the (m+1)th path point, i.e., the termination point.

[0153] End point satisfy:

[0154]

[0155] Indicates the first There are path points.

[0156] (III) Dynamic Environment Adaptive Planning Mechanism

[0157] (1) Change detection

[0158] Real-time monitoring of terrain and obstacle status changes provides a trigger for local replanning. The specific detection logic is as follows:

[0159] 1. Quantitative Detection of Topographic Changes

[0160] Determine terrain changes by comparing time-series elevation map data:

[0161]

[0162]

[0163] In the formula: For a moment Elevation value; For a moment Elevation value; Represents the elevation variation matrix; The Frobenius norm of the elevation change matrix (characterizing the overall change). Topographic change threshold (typical value) ). This indicates the result of elevation changes.

[0164] 2. Obstacle motion detection

[0165] Displacement analysis for dynamic obstacles:

[0166]

[0167] Conditions for determining changes in obstacles:

[0168]

[0169] in, Indicates the first The position of a dynamic obstacle changes; Indicates the first A dynamic obstacle at any time The coordinates; Indicates the first A dynamic obstacle at any time The coordinates; Indicates time A collection of obstacles; Indicates time A collection of obstacles; This represents the displacement threshold. This indicates the result of changes in the obstacle. Represents the set of added obstacles; Represents the set of obstacles that have disappeared. Indicates the displacement of the obstacle; This indicates an increase in obstacles; This indicates that the obstacle has disappeared.

[0170] 3. Event-triggered functions generate replanning instructions based on changes in the overall environment:

[0171]

[0172] (2) Incremental update

[0173] When the event is triggered function At that time, a local potential field update operation is performed, and the specific process is as follows:

[0174] 1. Local update of terrain repulsion field

[0175] Based on terrain change detection results Dynamically adjust the affected areas:

[0176]

[0177] Update terrain gain factor:

[0178]

[0179] in, To locally update the sensitivity threshold; Indicates the updated terrain gain; This indicates the slope calculated based on the updated elevation. This represents the roughness calculated based on the updated elevation. This represents the updated terrain-adaptive repulsive field. This indicates a partially updated region.

[0180] 2. Incremental Update of Dynamic Obstacle Field

[0181] Based on obstacle change detection results , Reconstructing the potential energy field of the obstacle:

[0182]

[0183] Single obstacle repulsive force field function:

[0184]

[0185] in, This represents the repulsive force field of the original dynamic obstacle; Indicates the disappearance of obstacles The repulsive field; Indicates newly added obstacles The repulsive field; Indicates the current position of the agent To the new obstacles The distance; This indicates the center coordinates of the newly added obstacle. This represents a repulsive force field from a single obstacle. This represents the updated dynamic obstacle repulsion field. Indicates the current position exist New obstacles are constantly being added. The repulsive field.

[0186] 3. Reconstruction of the composite potential field

[0187] Merging and updating items generates a real-time potential energy field:

[0188]

[0189] This represents the updated composite potential field.

[0190] (3) Supporting technical support

[0191] 1. Optimization of computational efficiency

[0192] 1) Spatial hash mapping

[0193] Discretize the continuous space into grid cells to accelerate neighborhood search:

[0194]

[0195] Indicates the minimum coordinates of the map boundary. Represents the minimum x-coordinate of the map boundary. Indicates the minimum ordinate of the map boundary; Indicates spatial resolution (balancing accuracy and efficiency). Represents coordinate points The raster index.

[0196] Obstacle / terrain data is stored in a hash table:

[0197]

[0198] When calculating the gradient, it is only necessary to search the adjacent cells (3×3 neighborhood) of the grid where the current position is located. Represents a list of obstacles; This represents a hash table.

[0199] 2) Local update region constraints

[0200] Limiting the calculation scope during dynamic reprogramming:

[0201]

[0202] Parameter description:

[0203] Represents the set of indices for changing obstacles; Indicates the amount of security boundary expansion; This represents the local update region. Potential field updates and gradient calculations occur only within the local update region. Execute internally.

[0204] 2. Motion smoothness constraints

[0205] To ensure that the path conforms to the robot's kinematics, a historical path constraint mechanism is introduced, specifically implemented as follows:

[0206] 1) Construction of historical path smoothing items

[0207] Define a smooth energy function based on a sequence of historical path points:

[0208]

[0209] In the formula: The first in the historical path Coordinates of path points ; The first in the historical path The coordinates of the path points. Smoothing weighting coefficients (typical value range) ); This represents the number of historical path points. It is a smooth energy function.

[0210] 2) Reconstruction of the composite potential energy field

[0211] Superimpose the smoothing term onto the original potential field:

[0212]

[0213] in This represents the original dynamic potential energy field. This represents a composite potential field with a smoothing term superimposed on it. Represents a sequence of path points. .

[0214] 3) Analytical calculation of smooth gradient

[0215] The path optimization direction is constrained by the gradient of the smoothing term:

[0216]

[0217] This represents the gradient of the historical path smoothing term. This gradient term is introduced as a kinematic constraint during local reprogramming.

[0218] The accompanying drawings for this embodiment will now be described in detail:

[0219] Figure 2 The construction logic of the terrain-adaptive potential field was revealed.

[0220] The process begins with the input of a 2.5D elevation map, and then performs slope calculation (using the central difference method) and roughness calculation (based on the standard deviation of a 2m × 2m neighborhood) in parallel. The slope gain α_slope is generated according to the formula 1 + 0.8 × (Slope - 30°)^2, and the roughness gain is calculated according to 1 + 1.2 × Roughness. The two are multiplied together to obtain the composite gain factor α.

[0221] Simultaneously, a basic repulsive field (with an effective radius of 3.0m) is generated based on the obstacle location information. The composite gain α is multiplied by the basic repulsive field to form a terrain-adaptive repulsive field, which is then superimposed with the target gravitational field (gain coefficient k_att=1.5) to finally output a composite potential energy field U that accurately reflects the terrain risk.

[0222] This process enables a mathematical mapping from raw terrain data to a navigation field with clearly defined physical meaning.

[0223] Figure 3 The principle of continuous path generation is explained:

[0224] This sequence comprises three core modules: the control unit calculates the navigation force vector based on the composite potential energy field, performs iterative position updates, and outputs the final path sequence through a convergence determination mechanism. The control unit initiates a gradient calculation request to the potential energy field module to obtain its current position. At any moment Potential field gradient Then the negative gradient direction is calculated. and according to spatial step size Update path point coordinates:

[0225]

[0226] in Environmental sampling cycle Dynamically generated. After the location is updated, the system detects the termination condition in real time: when the current path point... With the target point The Euclidean distance is less than the convergence threshold. Or the number of iterations reaches the safe limit. When the time is reached, the calculation terminates and an ordered set of path points is output. .

[0227] This control mechanism establishes a mathematical mapping relationship between path generation and the potential field gradient, and its technical effects are manifested in three aspects: 1. By fixing the spatial step size 1. Ensure uniformity of path point spacing; 2. Normalize gradient vectors. 3. Eliminate the impact of magnitude fluctuations on the path; 4. Double termination conditions ensure the real-time performance of the algorithm, including a distance threshold. Control path precision, iteration limit To prevent computation from getting stuck in an infinite loop, output the path sequence. It can directly drive the motion control system to perform terrain-adaptive navigation.

[0228] Figure 4 This illustrates the dynamic environment adaptation process:

[0229] The detection module scans the environment every 50ms: terrain update is triggered when the Frobenius norm of the terrain elevation change matrix is ​​greater than 0.2m; obstacle update is triggered when obstacle displacement is greater than 0.5m or obstacles are added / disappeared.

[0230] The update module employs a spatial constraint strategy: it only processes circular regions centered at the point of change with a radius of 5.0m (obstacle radius + 2.0m safety boundary). Terrain update recalculates the local gain factor α, and obstacle update reconstructs the repulsive field components.

[0231] After receiving the incrementally updated potential field, the planning module restarts the gradient flow calculation from the current position, adds a historical path smoothing constraint (weight λ=0.3), and outputs a new path.

[0232] The entire process takes ≤50ms, including ≤10ms for update calculation and ≤40ms for replanning, ensuring real-time response capabilities in dynamic environments.

[0233] Figure 5 The three-layer collaborative architecture of the hardware system of this invention is demonstrated. The environmental perception layer includes a lidar, a depth camera, an IMU (inertial measurement unit), and a dynamic obstacle sensor, which are responsible for collecting terrain elevation data, vehicle pose information, and dynamic obstacle positions, respectively, and transmitting them to the main control unit via a CAN bus (baud rate ≥ 1Mbps).

[0234] The computational control layer uses an embedded GPU platform (such as NVIDIA Jetson AGX Xavier) to run terrain parsing algorithms and potential energy field calculation modules, generating path control commands in real time.

[0235] The execution layer includes steering actuators and drive controllers, which convert path data into vehicle steering angle and speed commands. The target point input is directly injected into the main controller as navigation endpoint parameters, and dynamic obstacle information is updated in real time through the environmental perception layer, forming a closed-loop control system. This architecture provides complete hardware support for algorithm implementation, encompassing sensors, computation, and execution.

[0236] Experimental Section

[0237] This embodiment relies on an onboard computing platform (such as NVIDIA Jetson AGX Xavier) and environmental perception sensors (LiDAR / depth camera). First, configure the system parameters: input the target point coordinates. Set a safe slope threshold Roughness upper limit The radii of the static / dynamic obstacle repulsive fields are respectively and Topographic data is input in the form of a 0.1m×0.1m rasterized elevation map. The slope value of each point is calculated in real time using the central difference method, and the elevation standard deviation is calculated as a roughness index within a 2m×2m neighborhood. The system automatically marks areas that meet the requirements of slope ≤30° and roughness ≤0.3m as feasible regions, forming a traffic capacity benchmark map.

[0238] During the potential field construction phase, the target gravitational field adopts a quadratic function. Its gradient direction always points towards the target point.

[0239] The core innovative module is the terrain-adaptive repulsive field: when the slope exceeds the safety threshold, the repulsive gain increases accordingly. It grows exponentially; the roughness gain, on the other hand, increases proportionally. Linear increase. Composite gain. Dynamically amplify the intensity of the basic repulsive field. Final composite potential energy field. It accurately integrates navigation targets with terrain risk constraints.

[0240] Path generation is achieved through negative gradient streamline integration. Starting from the vehicle's current position... Initially, the algorithm iterates along the negative gradient direction of the potential energy field with a step size of 0.3m. The gradient vector is calculated using numerical differentiation (partial derivative calculation step size 0.01m), and the update formula for each step is as follows: The iteration continues until the distance to the target point is less than 0.1m or the upper limit of 1000 steps is reached. The output path point sequence naturally has continuous and smooth characteristics, avoiding the jagged path of traditional search algorithms.

[0241] A dynamic environmental adaptation mechanism ensures real-time performance. Terrain changes (triggered if the Frobenius norm change in the elevation matrix > 0.2m) and obstacle displacements (movement > 0.5m or addition / disappearance of obstacles) are detected every 50ms. Upon detection, only the affected area is updated: the terrain gain factor and repulsive field are reconstructed within a circular area with a radius of 5.0m centered on the point of change, while a historical path smoothing term is introduced based on motion consistency constraints. Incremental update time is controlled within 10ms, and combined with local replanning, the total system response latency is ≤50ms.

[0242] Example 2

[0243] This embodiment discloses an elevation map path planning system based on terrain-adaptive potential energy field.

[0244] A terrain-adaptive potential field-based elevation map path planning system includes:

[0245] The data acquisition module is configured to acquire an elevation map and the location of the agent on the elevation map;

[0246] The terrain adaptation module is configured to: calculate the slope and roughness of the agent's position, set the terrain adaptation gain, dynamically adjust the slope and roughness, and combine it with the basic repulsive field to obtain the terrain-adaptive repulsive field.

[0247] The composite potential energy field calculation module is configured to: obtain the composite potential energy field based on the terrain-adaptive repulsive field, combined with the target gravitational field and the dynamic obstacle repulsive field;

[0248] The overall planning module is configured to: determine the agent's direction of motion and determine the agent's next position based on the composite potential energy field;

[0249] The local replanning judgment module is configured to: determine whether local replanning of the agent's next position is needed based on preset environmental change judgment rules during the process of the agent moving to the next position;

[0250] The composite potential field update module is configured to recalculate the terrain-adaptive repulsive field and the dynamic obstacle repulsive field and update the composite potential field when replanning is required.

[0251] The local replanning execution module is configured to: redetermine the agent's next position based on the updated composite potential field.

[0252] Those skilled in the art will understand that the modules or steps of the present invention described above can be implemented using general-purpose computer devices. Optionally, they can be implemented using computer-executable program code, thereby allowing them to be stored in a storage device for execution by a computer device, or they can be fabricated as separate integrated circuit modules, or multiple modules or steps can be fabricated as a single integrated circuit module. The present invention is not limited to any particular combination of hardware and software.

[0253] While the specific embodiments of the present invention have been described above in conjunction with the accompanying drawings, this is not intended to limit the scope of protection of the present invention. Those skilled in the art should understand that various modifications or variations that can be made by those skilled in the art without creative effort based on the technical solutions of the present invention are still within the scope of protection of the present invention.

Claims

1. A terrain-adaptive potential field-based elevation map path planning method, characterized in that, Includes the following steps: Obtain the elevation map and the location of the agent on the elevation map; Calculate the slope and roughness of the agent's position, set the terrain adaptive gain, dynamically adjust the slope and roughness, and combine it with the basic repulsive field to obtain the terrain adaptive repulsive field. Based on the terrain-adaptive repulsive field, a composite potential energy field is obtained by combining the target gravitational field and the dynamic obstacle repulsive field; The direction of motion of the agent is determined based on the composite potential energy field, and the next position of the agent is determined. As the agent moves to the next position, it determines whether a local replanning of the agent's next position is needed based on a preset environmental change discrimination rule. When replanning is required, the terrain-adaptive repulsive field and the dynamic obstacle repulsive field are recalculated, and the composite potential field is updated. Based on the updated composite potential field, the next position of the agent is determined. The slope and roughness of the agent's location are calculated, a terrain-adaptive gain is set, the slope and roughness are dynamically adjusted, and combined with the basic repulsive field, a terrain-adaptive repulsive field is obtained, specifically including: By analyzing the elevation map, terrain feature parameters, including the elevation matrix, are obtained; The slope value is solved based on the spatial partial derivative of the elevation matrix. The roughness value is calculated based on the elevation standard deviation within the local neighborhood. Based on the slope value, set the slope gain to cope with the risk of overturning; Based on the roughness value, set the roughness gain to cope with the risk of bumps; The terrain gain is obtained by multiplying the slope gain and the roughness gain. By coupling the terrain gain to the base repulsive field, a terrain-adaptive repulsive field is obtained. The fundamental repulsive field is defined as follows: In the formula: Represents the fundamental repulsive field function; Represents the fundamental repulsive force function; Indicates the total number of static obstacles; Indicates the repulsive force intensity coefficient; Indicates the current position To the Obstacle Center The Euclidean distance; Indicates the radius of the repulsive field; Indicates the first The center coordinates of each obstacle; The slope gain is specifically as follows: in, This is the slope gain coefficient. It is a non-linear adjustment index. The safe slope threshold; For slope gain; Indicates the current position of the agent The slope value; The roughness gain is specifically: in, This is the roughness gain coefficient; For roughness gain; The roughness value represents the current position of the agent; The terrain gain is specifically as follows: The terrain-adaptive repulsive force field is specifically as follows: in, Terrain-adaptive repulsive field; The basic repulsive field; This is the terrain-adaptive gain.

2. The elevation map path planning method based on terrain adaptive potential energy field as described in claim 1, characterized in that, Based on the terrain-adaptive repulsive field, combined with the target gravitational field and the dynamic obstacle repulsive field, a composite potential energy field is obtained, which specifically includes: The target gravitational field is obtained based on the quadratic potential energy function; Based on the distance between the dynamic obstacle and the agent, and the radius of influence of the dynamic obstacle, the repulsive field of the dynamic obstacle is obtained; The target gravitational field, the dynamic obstacle repulsive field, and the terrain-adaptive repulsive field are summed to obtain the composite potential energy field.

3. The elevation map path planning method based on terrain-adaptive potential energy field as described in claim 1, characterized in that, Determining the agent's direction of motion and its next position based on the composite potential field includes: Calculate the negative gradient direction of the composite potential field and use it as the negative gradient vector field. Based on the negative gradient vector field, the direction of motion of the agent is obtained, and a continuous path is generated by numerical integration to determine the next position of the agent. The method of generating continuous paths using numerical integration specifically includes: Set the starting position of the agent, define the integration step size and the maximum number of iterations, and perform streamline integration initialization; Based on the agent's initial position and the defined integration step size, the first-order Euler method is used to recursively calculate the path point sequence. Determine whether the termination condition has been met based on the distance between the waypoint and the end point, as well as the step limit. Output all path points after the termination condition is met.

4. The elevation map path planning method based on terrain adaptive potential energy field as described in claim 1, characterized in that, The environmental change discrimination rules include terrain change discrimination rules and obstacle movement discrimination rules, wherein: The terrain change discrimination rule is as follows: Set terrain change threshold; Based on the elevation matrix, calculate the elevation time series variation matrix within a set time interval; Calculate the Frobenius norm of the elevation time series variation matrix and compare it with the terrain change threshold; When the Frobenius norm of the elevation time series change matrix is ​​greater than the terrain change threshold, it is determined that the terrain has changed, triggering a local replanning for the agent's next location; or, The obstacle motion discrimination rule is as follows: Set the displacement threshold; Calculate the distance a dynamic obstacle travels within a set time interval; When a dynamic obstacle moves a distance greater than a displacement threshold within a set time interval, it is determined that the obstacle has moved, triggering a local replanning of the agent's next position. Determine if any new obstacles have been added or if any obstacles have disappeared; When a new obstacle is added or a new obstacle is removed, a local replanning of the agent's next location is triggered.

5. The elevation map path planning method based on terrain adaptive potential energy field as described in claim 4, characterized in that, When replanning is required, the terrain-adaptive repulsive field and the dynamic obstacle repulsive field are recalculated, and the composite potential field is updated, where: Recalculate the terrain-adaptive repulsive field, specifically including: Determine the local update sensitivity threshold; Identify the coordinates of points in the elevation time-series change matrix that are greater than the local update sensitivity threshold to determine the area affected by dynamic adjustment; Recalculate and update the slope gain and roughness gain in the dynamically adjusted affected area; Based on the updated slope gain and roughness gain, the updated terrain gain is obtained; Based on the updated terrain gain, the terrain-adaptive repulsive field is recalculated; or, Recalculate the repulsive field of dynamic obstacles, specifically including: Statistics on newly added and removed obstacles; Calculate the repulsive field of newly added obstacles and the repulsive field of disappeared obstacles; Based on the original dynamic obstacle repulsion field, the repulsion fields of the disappeared obstacles are removed and the repulsion fields of the newly added obstacles are added to obtain the updated dynamic obstacle repulsion field.

6. The elevation map path planning method based on terrain adaptive potential energy field as described in claim 1, characterized in that, When redetermining the agent's next position based on the updated composite potential field, a motion consistency constraint is introduced: Define a smoothing energy function based on the historical path point sequence as a smoothing term; The smoothing term is superimposed onto the original composite potential field to reconstruct the composite potential field; When redetermining the agent's next position, the path optimization direction is constrained by the gradient of the smoothing term; The smoothing energy function is specifically as follows: In the formula: The first in the historical path Coordinates of path points , This represents the number of historical path points. The first in the historical path Coordinates of the path points; For smoothing weighting coefficients; It is a smooth energy function.

7. The elevation map path planning method based on terrain adaptive potential energy field as described in claim 5, characterized in that, This also includes adding local update region constraints when determining the region affected by dynamic adjustments: ； in, For a set of indexes of changing obstacles; For the safety boundary extension amount; Indicates the radius of the repulsive field; Indicates the first One obstacle center; This indicates a partially updated region.

8. A terrain-adaptive potential field-based elevation map path planning system, characterized in that, include: The data acquisition module is configured to acquire an elevation map and the location of the agent on the elevation map; The terrain adaptation module is configured to: calculate the slope and roughness of the agent's position, set the terrain adaptation gain, dynamically adjust the slope and roughness, and combine it with the basic repulsive field to obtain the terrain-adaptive repulsive field. The composite potential energy field calculation module is configured to: obtain the composite potential energy field based on the terrain-adaptive repulsive field, combined with the target gravitational field and the dynamic obstacle repulsive field; The overall planning module is configured to: determine the agent's direction of motion and determine the agent's next position based on the composite potential energy field; The local replanning judgment module is configured to: determine whether local replanning of the agent's next position is needed based on preset environmental change judgment rules during the process of the agent moving to the next position; The composite potential field update module is configured to recalculate the terrain-adaptive repulsive field and the dynamic obstacle repulsive field and update the composite potential field when replanning is required. The local replanning execution module is configured to: redetermine the agent's next position based on the updated composite potential field; The slope and roughness of the agent's location are calculated, a terrain-adaptive gain is set, the slope and roughness are dynamically adjusted, and combined with the basic repulsive field, a terrain-adaptive repulsive field is obtained, specifically including: By analyzing the elevation map, terrain feature parameters, including the elevation matrix, are obtained; The slope value is solved based on the spatial partial derivative of the elevation matrix. The roughness value is calculated based on the elevation standard deviation within the local neighborhood. Based on the slope value, set the slope gain to cope with the risk of overturning; Based on the roughness value, set the roughness gain to cope with the risk of bumps; The terrain gain is obtained by multiplying the slope gain and the roughness gain. By coupling the terrain gain to the base repulsive field, a terrain-adaptive repulsive field is obtained. The fundamental repulsive field is defined as follows: In the formula: Represents the fundamental repulsive field function; Represents the fundamental repulsive force function; Indicates the total number of static obstacles; Indicates the repulsive force intensity coefficient; Indicates the current position To the Obstacle Center The Euclidean distance; Indicates the radius of the repulsive field; Indicates the first The center coordinates of each obstacle; The slope gain is specifically as follows: in, This is the slope gain coefficient. It is a non-linear adjustment index. The safe slope threshold; For slope gain; Indicates the current position of the agent The slope value; The roughness gain is specifically: in, This is the roughness gain coefficient; For roughness gain; The roughness value represents the current position of the agent; The terrain gain is specifically as follows: The terrain-adaptive repulsive force field is specifically as follows: in, Terrain-adaptive repulsive field; The basic repulsive field; This is the terrain-adaptive gain.

Images