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

Abstract

The invention provides an elevation map path planning method and system based on a terrain adaptive potential energy field, and relates to the technical field of path planning. Comprising the steps of calculating the gradient and roughness of the position of an intelligent agent, setting a terrain adaptive gain, dynamically adjusting the gradient and roughness, and obtaining a terrain adaptive repulsive force field and a composite potential energy field in combination with a basic repulsive force field; determining the motion direction of the agent based on the composite potential energy field, and determining the next position of the agent; in the process that the intelligent agent moves to the next position, whether local re-planning needs to be carried out on the next position of the intelligent agent or not is judged based on a preset environment change judgment rule; and when re-planning is needed, recalculating the terrain adaptive repulsive force field and the dynamic obstacle repulsive force field, updating the composite potential energy field, and re-determining the next position of the intelligent body. According to the invention, the terrain can be effectively sensed, the planning strategy is adaptively adjusted, and high precision, high efficiency and strong robustness are considered.

Description

Technical Field

[0001] The present invention belongs to the technical field of path planning, and in particular relates to a method and system for path planning based on an elevation map of a terrain-adaptive potential energy field. Background Art

[0002] Traditional methods face significant challenges in complex terrain path planning using two-dimensional elevation maps (2.5D elevation maps): First, such algorithms are generally prone to falling into local optimal solutions. That is, due to the selection of initial path points or search directions, the algorithm may converge to a non-global optimal path too early, resulting in the final planned path length or cost not being optimal.

[0003] Secondly, complex elevation maps contain numerous obstacles and undulating terrain, requiring classic methods like the A* algorithm and the Dijkstra algorithm to evaluate a vast number of map grid nodes. As map size and terrain complexity increase, computational overhead increases dramatically, making it difficult to meet real-time requirements, limiting their feasibility in large-scale or dynamic applications.

[0004] Finally, the actual environment often has dynamic changing factors, such as dynamic obstacles or real-time terrain changes, but the above traditional methods are mainly designed for static scenes and lack efficient environmental adaptability. When the environment changes, the global planning process usually needs to be restarted, resulting in delayed path adjustment response, which makes it difficult to meet the real-time planning requirements of dynamic environments. Summary of the Invention

[0005] To overcome the deficiencies of the above-mentioned prior art, the present invention provides an elevation map path planning method and system based on a terrain-adaptive potential field, constructs a terrain-adaptive repulsive field model with terrain perception capabilities, can effectively perceive the terrain, adaptively adjust the planning strategy, and take into account high precision, high efficiency, and strong robustness.

[0006] To achieve the above objectives, one or more embodiments of the present invention provide the following technical solutions: A first aspect of the present invention provides an elevation map path planning method based on terrain adaptive potential energy field.

[0007] The elevation map path planning method based on terrain adaptive potential field includes the following steps: Get the elevation map and the agent's position 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 them with the basic repulsion field to obtain the terrain adaptive repulsion field; Based on the terrain adaptive repulsion field, the target gravity field and the dynamic obstacle repulsion field are combined to obtain a composite potential energy field; Determine the direction of movement of the agent based on the composite potential energy field and determine the next position of the agent; When the agent moves to the next position, it determines whether it is necessary to perform local replanning of the next position of the agent based on the preset environment change judgment rules; When replanning is required, the terrain adaptive repulsion field and the dynamic obstacle repulsion field are recalculated and the composite potential energy field is updated; Based on the updated composite potential energy field, the next position of the agent is re-determined.

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

[0009] The elevation map path planning system based on terrain adaptive potential field includes: The data acquisition module is configured to: acquire the elevation map and the position of the intelligent 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 them with the basic repulsion field to obtain the terrain adaptive repulsion field; The composite potential energy field calculation module is configured to: obtain a composite potential energy field based on the terrain adaptive repulsion field, the target gravity field, and the dynamic obstacle repulsion field; The overall planning module is configured to: determine the movement direction of the agent based on the composite potential energy field and determine the next position of the agent; The local replanning judgment module is configured to: determine whether it is necessary to perform local replanning on the next position of the intelligent agent based on a preset environment change judgment rule during the process of the intelligent agent moving to the next position; The composite potential energy field update module is configured to: when replanning is required, recalculate the terrain adaptive repulsion field and the dynamic obstacle repulsion field, and update the composite potential energy field; The local replanning execution module is configured to re-determine the next position of the agent based on the updated composite potential energy field.

[0010] One or more of the above technical solutions have the following beneficial effects: The application provides a height map path planning method and system based on a terrain adaptive potential field, models a path planning problem as a motion process of an intelligent agent in a continuous potential field, generates a navigation path by constructing a potential field model of a dynamically perceived terrain and calculating a negative gradient streamline thereof. Specifically, the potential field is composed of a global target gravitational field, a terrain adaptive repulsive field and a dynamic obstacle repulsive field, wherein the gravitational field guides the intelligent agent to move towards a target point, and the repulsive field generates an avoidance effect according to terrain features and obstacle distribution. The path is generated by solving a streamline formed in a negative gradient direction of the potential field, and the intelligent agent moves towards the target point in a fastest descending manner along the streamline.

[0011] The core innovation of the application lies in constructing a terrain adaptive repulsive field model with terrain perception capability. The model dynamically adjusts the strength coefficient of the terrain repulsive field by analyzing height and slope data of a position where the intelligent agent is located in real time. Specifically, when the slope exceeds a safety threshold, the slope repulsive gain increases exponentially with the increase of the slope; when the terrain roughness increases, the roughness repulsive gain is positively correlated with the roughness in a linear manner. This dynamic adjustment mechanism enables the potential field to accurately reflect the actual passing risk of the terrain, and provides a continuous field environment with clear physical meaning for subsequent path generation.

[0012] In the path generation mechanism, the algorithm of the application adopts a negative gradient streamline calculation method instead of a traditional discrete search. The spatial partial derivative of the composite potential field is solved to obtain a negative gradient vector field, and a numerical integration method (such as Euler method or Runge Kutta method) is applied to iteratively generate a path point sequence from the starting point. This process directly utilizes the continuity and smoothness of the gradient flow, avoids the local optimal problem caused by node sampling in the discrete search algorithm, and reduces the calculation complexity from O(n log n) of the traditional algorithm to O(n) order.

[0013] To adapt to dynamic environmental changes, the algorithm framework of the application supports incremental updating and local re-planning of the composite potential field. When a terrain height mutation or obstacle displacement is detected, only the repulsive field components of the affected area need to be updated, and local gradient flow calculation is restarted from the current position on the basis of freezing the planned path. This mechanism quickly locates the changed area through spatial hash mapping, guarantees smooth transition of path updating by combining motion consistency constraints, and realizes a real-time response capability with a re-planning delay of less than 50 ms.

[0014] Advantages of additional aspects of the application will be partially given in the following description, partially will become obvious from the following description, or will be understood by practice of the application. BRIEF DESCRIPTION OF DRAWINGS

[0015] The accompanying drawings, which constitute a part of the present invention, are used to provide a further understanding of the present invention. The exemplary embodiments of the present invention and their descriptions are used to explain the present invention and do not constitute improper limitations on the present invention.

[0016] Figure 1 This is a flow chart of the method of embodiment 1.

[0017] Figure 2 This is a flow chart of potential energy field construction in Example 1.

[0018] Figure 3 Schematic diagram of gradient flow path generation in Example 1.

[0019] Figure 4 Schematic diagram of the incremental update mechanism of Example 1.

[0020] Figure 5 This is a diagram of the system hardware architecture of Example 1. DETAILED DESCRIPTION

[0021] It should be noted that the following detailed descriptions are exemplary and intended to provide further explanation of the present invention. Unless otherwise specified, all technical and scientific terms used herein have the same meaning as commonly understood by those skilled in the art to which the present invention belongs.

[0022] It should be noted that the terms used herein are for describing particular embodiments only and are not intended to limit the exemplary embodiments according to the present invention.

[0023] In the absence of conflict, the embodiments of the present invention and the features thereof may be combined with each other.

[0024] Example 1 like Figure 1 As shown, this embodiment discloses an elevation map path planning method based on terrain adaptive potential field.

[0025] The elevation map path planning method based on terrain adaptive potential field includes the following steps: Get the elevation map and the agent's position 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 them with the basic repulsion field to obtain the terrain adaptive repulsion field; Based on the terrain adaptive repulsion field, the target gravity field and the dynamic obstacle repulsion field are combined to obtain a composite potential energy field; Determine the direction of movement of the agent based on the composite potential energy field and determine the next position of the agent; When the agent moves to the next position, it determines whether it is necessary to perform local replanning of the next position of the agent based on the preset environment change judgment rules; When re-planning is needed, the terrain adaptive repulsive force field and the dynamic obstacle repulsive force field are recalculated, and the composite potential field is updated; Based on the updated composite potential field, the next position of the intelligent agent is re-determined.

[0026] Next, the technical solutions of the embodiment will be explained in detail.

[0027] (I) Terrain adaptive potential field modeling (1) Elevation map preprocessing Input 2.5D elevation map data, real-time analyze and structure the terrain features, including: 1. Slope calculation The slope value is calculated by the gradient modulus of the elevation matrix: ; Wherein, represents the current position of the intelligent agent plane coordinates; represents the x direction elevation gradient; represents the y direction elevation gradient; is the abbreviation of , and represents the elevation value at the coordinate point . represents the slope value of the current position of the intelligent agent.

[0028] 2. Roughness calculation The terrain fluctuation degree is quantified by the local neighborhood elevation standard deviation, and the elevation standard deviation is calculated in the local neighborhood (typically ):

[0029] Wherein, represents the local index coordinates of the neighborhood grid; represents the neighborhood range; represents the total number of grids in the neighborhood; represents the average elevation of the neighborhood, and the calculation formula is: ; represents the elevation value at the index coordinate point . represents the roughness value of the current position of the intelligent agent.

[0030] 3. Feasible region marking: According to the slope and roughness threshold value to determine the passable area:

[0031] Wherein, is the safety slope angle threshold value (typical value ), is the maximum allowable roughness. Indicates current location The feasible region marking result at , 1 means the region is feasible, and 0 means the region is not feasible.

[0032] Technical effect: Slope field generated in real time , roughness field and feasible region marking This constitutes a terrain feature perception matrix, providing a decision-making basis for dynamic parameter adjustment of the potential energy field. Slope detection prevents the risk of overturning during movement, roughness quantifies the degree of terrain bumpiness, and feasible region constraints define the boundaries of the path search space.

[0033] (2) Adaptive repulsive field modeling Based on the terrain characteristic parameters generated by elevation map preprocessing, a repulsion field model that dynamically responds to terrain risks is constructed. The specific implementation is as follows: 1. Construction of basic repulsive field: Define the basic repulsive field function for obstacle avoidance:

[0034] The basic repulsive force function Defined as:

[0035] Where: Indicates current location To Obstacle Center The Euclidean distance of Indicates the The center coordinates of the obstacles, Indicates the The horizontal coordinate of the obstacle center, Indicates the The vertical coordinate of the obstacle center; Indicates the total number of static obstacles; represents the repulsive force strength coefficient; Represents the radius of the repulsive field. Indicates current location The basic repulsive field of represents the basic repulsion function.

[0036] 2. Terrain Adaptive Gain Calculation: The repulsive force strength is dynamically adjusted by slope and roughness to construct a gain function: Slope gain function (to deal with overturning risk):

[0037] in, is the slope gain coefficient ( ), is the nonlinear regulation index (typical value ), is the safety slope threshold (value ); Current location The slope gain function.

[0038] Roughness gain function (to cope with bumpy risk):

[0039] in, is the roughness gain coefficient ( ); Current location Roughness gain function.

[0040] Composite gain function:

[0041] in, Current location Terrain adaptation gain.

[0042] 3. Adaptive repulsive field synthesis The terrain adaptive gain is coupled to the basic repulsion field to form the final terrain adaptive repulsion field:

[0043] in, It is a terrain adaptive repulsion field.

[0044] Technical effects of adaptive repulsive field modeling: When the slope exceeds the safety threshold, It grows quadratically, significantly increasing the repulsive force strength in the steep slope area; when the roughness increases, The avoidance weight of uneven terrain is linearly increased. The two achieve a coordinated response to complex terrain risks through multiplicative coupling.

[0045] (3) Complete potential energy field definition Based on goal orientation and terrain risk constraints, a composite potential energy field model integrating multiple physical fields is constructed, and its mathematical definition is:

[0046] in, represents the target gravitational field; represents the terrain adaptive repulsion field; represents the dynamic obstacle repulsion field; Indicates current location exist The composite potential energy field at each moment. express time. Indicates current location The target gravitational field; Indicates current location Terrain-adaptive repulsive field; Indicates current location exist Dynamic obstacle repulsion field at all times.

[0047] The target gravitational field drives global navigation, the terrain adaptive repulsion field responds to static terrain risks, and the dynamic obstacle repulsion field avoids time-varying threats.

[0048] 1. Target gravitational field model Use the quadratic potential energy function to guide the agent to move towards the target:

[0049] Where: is the target point coordinate; is the gravitational gain coefficient (typical values ​​range from 0.5 to 2.0).

[0050] 2. Terrain-adaptive repulsive field model Dynamically coupled terrain features and obstacle avoidance:

[0051] in is the terrain adaptive gain, is the basic repulsion function, is the total number of static obstacles.

[0052] 3. Dynamic obstacle repulsion field model Respond to the time-varying threat of moving obstacles in real time, deal with the threat of moving obstacles, and define a time-varying repulsive field:

[0053] The single obstacle repulsion function is defined as:

[0054] Where: Indicates the current position of the agent To The time-varying distance of a dynamic obstacle; Indicates the A dynamic obstacle at time The coordinates of Indicates the A dynamic obstacle at time The horizontal axis, Indicates the A dynamic obstacle at time The vertical coordinate of Indicates the total number of dynamic obstacles; represents the dynamic repulsion coefficient; Indicates the dynamic obstacle influence radius. represents the single obstacle repulsion function.

[0055] (2) Negative gradient streamline path generation algorithm (1) Gradient field calculation Based on the constructed composite potential energy field , calculate the negative gradient vector field generated by the driving path, the specific steps are as follows: 1. Definition of total potential energy field gradient

[0056] The path movement direction is the negative gradient direction:

[0057] Represents Hamilton operator (gradient operator); represents the navigation force vector field; represents the gradient of the composite potential energy field; represents the gradient of the target gravitational field; represents the gradient of the terrain adaptive repulsive field; Represents the gradient of the repulsive field of a dynamic obstacle.

[0058] 2. Analytical calculation of gravitational field gradient

[0059] 3. Terrain-adaptive repulsive field gradient calculation Using the product rule to process gain factors and basic repulsive field Coupling relationship:

[0060] Among them, the partial derivative of the gain factor is:

[0061] The partial derivative of the basic repulsive field is:

[0062] Indicates the dynamic obstacles coordinate.

[0063] 4. Dynamic obstacle field gradient calculation

[0064] The single obstacle gradient function :

[0065] Directional Gradient Calculate similarly.

[0066] (2) Gradient streamline integral Based on the calculated negative gradient vector field , a numerical integration method is used to generate a continuous path. The specific steps are as follows: 1. Streamline integral initialization set up:

[0067] Indicates the starting position of the agent; Indicates the horizontal and vertical coordinates of the starting position; Indicates the spatial step size (fixed step size, not time step size); Represents an upper safety limit for iterations (to prevent infinite loops).

[0068] 2. Streamline Iteration Calculation Use the first-order Euler method to recurse the path point sequence:

[0069] Where: For the Step path point coordinates, Indicates the The horizontal coordinate of the step path point, Indicates the The vertical coordinate of the step path point; For the Step path point coordinates; For the The negative gradient vector at the step point coordinates and the current timestamp; is the modulus of the negative gradient vector (normalization ensures constant step length); is the current timestamp ( is the environmental sampling period); is the starting time, Indicates the current iteration step number.

[0070] 3. Termination condition determination Iteration stops when any of the following conditions are met:

[0071] is the convergence threshold; is the target point coordinate, Indicates the horizontal coordinate of the target point, Indicates the vertical coordinate of the target point.

[0072] 4. Path sequence output Generate an ordered set of path points:

[0073] in, is the first waypoint; is the second waypoint; is the m+1th path point, i.e. the end point.

[0074] End point satisfy:

[0075] Indicates the waypoints.

[0076] (3) Dynamic Environment Adaptive Planning Mechanism (1) Change Detection Real-time monitoring of terrain and obstacle status changes provides a trigger basis for local replanning. The specific detection logic is as follows: 1. Quantitative detection of terrain changes Determine terrain changes by comparing elevation map time series data:

[0077]

[0078] Where: For the moment The elevation value of For the moment The elevation value of represents the elevation change matrix; The Frobenius norm of the elevation change matrix (characterizing the overall change); is the terrain change threshold (typical value ). Indicates the result of elevation change.

[0079] 2. Obstacle motion detection Displacement analysis for dynamic obstacles:

[0080] Obstacle change determination conditions:

[0081] in, Indicates the Dynamic obstacle position changes; Indicates the A dynamic obstacle at time coordinates; Indicates the A dynamic obstacle at time coordinates; Indicates time The set of obstacles; Indicates time The set of obstacles; Indicates the displacement threshold. Indicates the result of obstacle change. Indicates the added obstacle set; Indicates the set of disappeared obstacles. Indicates obstacle displacement; Indicates an increase in obstacles; Indicates that the obstacle has disappeared.

[0082] 3. The event trigger function generates re-planning instructions based on the environmental change status:

[0083] (2) Incremental update When the event triggers the function When , the local potential field update operation is performed, and the specific process is as follows: 1. Local update of terrain repulsion field Based on the results of terrain change detection , dynamically adjust the affected area:

[0084] Update terrain gain factor:

[0085] in, is the local update sensitivity threshold; Indicates the updated terrain gain; Indicates the slope calculated based on the updated elevation; Indicates the roughness calculated based on the updated elevation; Represents the updated terrain adaptive repulsion field. Represents a local update region.

[0086] 2. Incremental update of dynamic obstacle field Based on the obstacle change detection results 、 , reconstruct the obstacle potential energy field:

[0087] Single obstacle repulsion field function:

[0088] in, represents the original dynamic obstacle repulsion field; Indicates the disappearance of obstacles repulsive field; Indicates a new obstacle repulsive field; Indicates the current position of the agent To add obstacles distance; Indicates the center coordinates of the newly added obstacle. Represents the repulsion field of a single obstacle. Represents the updated dynamic obstacle repulsion field. Indicates current location exist New obstacles added all the time repulsive field.

[0089] 3. Reconstruction of composite potential energy field Combine the update terms to generate a real-time potential field:

[0090] Represents the updated composite potential energy field.

[0091] (3) Assistive technology support 1. Computational efficiency optimization 1) Spatial hash map Discretize the continuous space into grid cells to speed up neighborhood searches:

[0092] Indicates the minimum coordinates of the map boundary, Indicates the minimum horizontal coordinate of the map boundary. Indicates the minimum vertical coordinate of the map boundary; Indicates spatial resolution (balancing accuracy and efficiency); Indicates coordinate points The raster index of the grid.

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

[0094] When calculating the gradient, only the adjacent cells (3×3 neighborhood) of the grid where the current position is located need to be retrieved. Represents a list of obstacles; Represents a hash table.

[0095] 2) Local update area constraints Limit the calculation scope during dynamic replanning:

[0096] Parameter Description: Represents the set of changing obstacle indexes; Indicates the amount of safety margin expansion; Represents the local update area. Potential field update and gradient calculation are only Internal execution.

[0097] 2 Motion Smoothness Constraints In order to ensure that the path conforms to the robot's kinematic characteristics, a historical path constraint mechanism is introduced, which is specifically implemented as follows: 1) Construction of historical path smoothing term Define a smoothed energy function based on a historical pathpoint sequence:

[0098] Where: The first in the historical path Path point coordinates ; The first in the historical path The coordinates of the path points. is the smoothing weight coefficient (typical value range ); is the number of historical path points; is a smooth energy function.

[0099] 2) Reconstruction of composite potential energy field Add a smoothing term to the original potential field:

[0100] in is the original dynamic potential energy field; represents the composite potential field with a superimposed smoothing term. represents a sequence of waypoints, .

[0101] 3) Smooth gradient analytical calculation The path optimization direction is constrained by the smoothing term gradient:

[0102] Represents the gradient of the smoothing term of the historical path. This gradient term is introduced as a kinematic constraint during local replanning.

[0103] Next, the accompanying drawings of this embodiment will be described in detail: Figure 2 The construction logic of terrain adaptive potential energy field is revealed.

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

[0105] At the same time, a basic repulsive field (effective radius 3.0m) is generated based on the obstacle position 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 attraction field (gain coefficient k_att = 1.5) to ultimately output a composite potential energy field U that accurately reflects the terrain risk.

[0106] This process achieves a mathematical mapping from raw terrain data to a physically meaningful navigation field.

[0107] Figure 3 The principle of continuous path generation is explained: The sequence consists of three core modules: the control unit calculates the navigation force vector based on the composite potential field, performs iterative position updates, and outputs the final path sequence through the convergence judgment mechanism. The control unit initiates a gradient calculation request to the potential field module to obtain the current position At the moment The potential energy field gradient . Then calculate the negative gradient direction , and according to the spatial step Update the path point coordinates:

[0108] in By environmental sampling period Dynamically generated. After the position 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 safety upper limit When , the calculation is terminated and the ordered path point set is output .

[0109] This control mechanism establishes a mathematical mapping relationship between path generation and potential field gradient, and its technical effects are manifested in three aspects: 1. By fixing the spatial step length Ensure the uniformity of path point spacing; 2. Normalize the gradient vector , eliminating the impact of magnitude fluctuations on the path; 3. Double termination conditions ensure the real-time performance of the algorithm, where the distance threshold Control path accuracy, iteration limit Prevent the calculation from falling into an infinite loop. Output path sequence It can directly drive motion control systems to perform terrain-adaptive navigation.

[0110] Figure 4 The dynamic environment adaptation process is described: The detection module scans the environment every 50ms: a terrain update is triggered when the Frobenius norm of the terrain elevation change matrix is ​​greater than 0.2m; an obstacle update is triggered when the obstacle displacement is greater than 0.5m or when a new obstacle appears or disappears.

[0111] The update module uses a spatial constraint strategy: it only processes a circular area centered at the change point with a radius of 5.0m (obstacle radius + 2.0m safety margin). Terrain updates recalculate the local gain factor α, while obstacle updates reconstruct the repulsive field components.

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

[0113] The entire process takes ≤50ms, of which update calculation ≤10ms and re-planning ≤40ms, ensuring real-time responsiveness in dynamic environments.

[0114] Figure 5 The three-layer collaborative architecture of the hardware system of the present invention is demonstrated. The environmental perception layer includes a lidar, a depth camera, an IMU inertial measurement unit, and a dynamic obstacle sensor. These sensors are responsible for collecting terrain elevation data, vehicle posture information, and the location of dynamic obstacles, respectively. These data are then transmitted to the main control unit via a CAN bus (baud rate ≥ 1Mbps).

[0115] The computing and control layer uses an embedded GPU platform (such as NVIDIA Jetson AGX Xavier) to run terrain analysis algorithms and potential field calculation modules to generate path control instructions in real time.

[0116] The execution layer, comprising the steering actuator and drive controller, converts path data into vehicle steering angle and speed commands. Target point input is directly fed into the main controller as navigation endpoint parameters. Dynamic obstacle information is updated in real time via the environmental perception layer, forming a closed-loop control system. This architecture provides the complete sensor-computation-execution hardware support for algorithm implementation.

[0117] Experimental part 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: Enter the target point coordinates , set the safety slope threshold , upper limit of roughness , the static / dynamic obstacle repulsive field action radius is and Terrain data is input as a 0.1m×0.1m gridded elevation map. The slope value at each point is calculated in real time using the central difference method. The standard deviation of elevation within a 2m×2m neighborhood is calculated as a roughness indicator. The system automatically marks areas with a slope of ≤30° and a roughness of ≤0.3m as feasible, forming a capacity benchmark map.

[0118] During the potential field construction phase, the target gravitational field adopts a quadratic function , its gradient direction always points to the target point.

[0119] The core innovation module is the terrain adaptive repulsion field: when the slope exceeds the safety threshold, the repulsion gain is The roughness gain increases exponentially. Linear increase. Composite gain Dynamically amplify the basic repulsive field strength. The final composite potential energy field Accurately integrates navigation objectives and terrain risk constraints.

[0120] Path generation is achieved through negative gradient streamline integration. From the current position of the vehicle Initially, it iterate along the negative gradient direction of the potential energy field with a step length of 0.3m. The gradient vector is calculated by numerical differentiation (partial derivative calculation step length 0.01m), and the update formula for each step is 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 a continuous and smooth characteristic, avoiding the jagged path of traditional search algorithms.

[0121] A dynamic environmental adaptation mechanism ensures real-time performance. Every 50ms, terrain changes (triggered when the Frobenius norm of the elevation matrix changes by >0.2m) and obstacle displacement (movement >0.5m or the addition or disappearance of an obstacle) are detected. Upon detecting a change, only the affected area is updated: the terrain gain factor and repulsion field are reconstructed within a circular area with a radius of 5.0m centered on the change point, while a historical path smoothing term is introduced based on motion consistency constraints. The incremental update time is controlled within 10ms, and combined with local replanning, the total system response delay is ≤50ms.

[0122] Example 2 This embodiment discloses an elevation map path planning system based on terrain adaptive potential energy field.

[0123] The elevation map path planning system based on terrain adaptive potential field includes: The data acquisition module is configured to: acquire the elevation map and the position of the intelligent 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 them with the basic repulsion field to obtain the terrain adaptive repulsion field; The composite potential energy field calculation module is configured to: obtain a composite potential energy field based on the terrain adaptive repulsion field, the target gravity field, and the dynamic obstacle repulsion field; The overall planning module is configured to: determine the movement direction of the agent based on the composite potential energy field and determine the next position of the agent; The local replanning judgment module is configured to: determine whether it is necessary to perform local replanning on the next position of the intelligent agent based on a preset environment change judgment rule during the process of the intelligent agent moving to the next position; The composite potential energy field update module is configured to: when replanning is required, recalculate the terrain adaptive repulsion field and the dynamic obstacle repulsion field, and update the composite potential energy field; The local replanning execution module is configured to re-determine the next position of the agent based on the updated composite potential energy field. Those skilled in the art will appreciate that the modules or steps of the present invention described above can be implemented using a general-purpose computer device. Alternatively, they can be implemented using program code executable by a computing device, which can then be stored in a storage device and executed by the computing device. Alternatively, they can be fabricated into separate integrated circuit modules, or multiple modules or steps can be fabricated into a single integrated circuit module for implementation. The present invention is not limited to any specific combination of hardware and software.

[0124] Although the above describes the specific embodiments of the present invention in conjunction with the accompanying drawings, it 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 on the basis of the technical solution of the present invention without any creative work are still within the scope of protection of the present invention.

Claims

1. The elevation map path planning method based on terrain adaptive potential field is characterized by: The following steps are involved: Get the elevation map and the agent's position 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 them with the basic repulsion field to obtain the terrain adaptive repulsion field; Based on the terrain adaptive repulsion field, the target gravity field and the dynamic obstacle repulsion field are combined to obtain a composite potential energy field; Determine the direction of movement of the agent based on the composite potential energy field and determine the next position of the agent; When the agent moves to the next position, it determines whether it is necessary to perform local replanning of the next position of the agent based on the preset environment change judgment rules; When replanning is required, the terrain adaptive repulsion field and the dynamic obstacle repulsion field are recalculated and the composite potential energy field is updated; Based on the updated composite potential energy field, the next position of the agent is re-determined.

2. The elevation map path planning method based on terrain adaptive potential field according to claim 1, characterized in that: Calculate the slope and roughness of the agent's position, set the terrain adaptive gain, dynamically adjust the slope and roughness, and combine them with the basic repulsion field to obtain the terrain adaptive repulsion field, specifically including: By analyzing the elevation map, terrain characteristic parameters including the elevation matrix are obtained; Based on the spatial partial derivative of the elevation matrix, the slope value is solved; Calculate the roughness value based on the standard deviation of elevation within the local neighborhood; Based on the slope value, set the slope gain to cope with the overturning risk; Based on the roughness value, set the roughness gain to deal with the risk of bumps; Calculate the product of slope gain and roughness gain to get terrain gain; The terrain gain is coupled to the basic repulsion field to obtain the terrain adaptive repulsion field.

3. The elevation map path planning method based on terrain adaptive potential energy field according to claim 2, characterized in that: The basic repulsive field is defined as: Where: represents the basic repulsive field function; represents the basic repulsion function; Indicates the total number of static obstacles; represents the repulsive force strength coefficient; Indicates current location To Obstacle Center The Euclidean distance of represents the radius of the repulsive field; Indicates the The coordinates of the obstacle center; The slope gain is specifically: in, is the slope gain coefficient, is the nonlinear adjustment index, is the safety slope threshold; is the slope gain; Indicates the current position of the agent The slope value; The roughness gain is specifically: in, is the roughness gain coefficient; is the roughness gain; Indicates the roughness value of the agent's current position; The terrain gain is specifically: The terrain-adaptive repulsive field is specifically: in, is the terrain adaptive repulsion field; is the basic repulsive field; is the terrain adaptation gain.

4. The elevation map path planning method based on terrain adaptive potential field according to claim 1, characterized in that: Based on the terrain adaptive repulsion field, combined with the target gravity field and the dynamic obstacle repulsion field, a composite potential energy field is obtained, which specifically includes: Based on the quadratic potential energy function, the target gravitational field is obtained; Based on the distance between the dynamic obstacle and the agent and the dynamic obstacle influence radius, the dynamic obstacle repulsion field is obtained; The target gravitational field, dynamic obstacle repulsion field and terrain adaptive repulsion field are added together to obtain a composite potential energy field.

5. The elevation map path planning method based on terrain adaptive potential field according to claim 1, characterized in that: Determine the movement direction of the agent based on the composite potential energy field and determine the next position of the agent, specifically including: Calculate the negative gradient direction of the composite potential energy field and use the negative gradient direction of the composite potential energy field as the negative gradient vector field; Based on the negative gradient vector field, the movement direction of the intelligent agent is obtained, and a continuous path is generated using the numerical integration method to determine the next position of the intelligent agent; or, The method of using numerical integration to generate a continuous path specifically includes: Set the agent's starting position, define the integration step size and maximum number of iterations, and initialize the streamline integration; Based on the initial position of the agent and the defined integration step, the first-order Euler method is used to recursively calculate the path point sequence; Determine whether the termination condition is met based on the distance between the path point and the termination point, as well as the step limit. Output all path points after reaching the termination condition.

6. The elevation map path planning method based on terrain adaptive potential field according to claim 2, characterized in that: The environmental change discrimination rules include terrain change discrimination rules and obstacle movement discrimination rules, wherein: The terrain change judgment rule is specifically as follows: Set the terrain change threshold; Based on the elevation matrix, the elevation time series change matrix is ​​calculated within the set time interval; Calculate the Frobenius norm of the elevation time series change 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, the terrain is judged to have changed, triggering local replanning of the agent's next position; or, The obstacle motion discrimination rules are specifically as follows: Set the displacement threshold; Calculate the distance a dynamic obstacle moves within a set time interval; When the movement distance of a dynamic obstacle within a set time interval is greater than the displacement threshold, it is judged that the obstacle has moved, triggering local replanning of the next position of the agent; Determine whether there are new obstacles or disappeared obstacles; When there are new obstacles or obstacles disappear, local replanning of the next position of the agent is triggered.

7. The elevation map path planning method based on terrain adaptive potential field according to claim 6, characterized in that: When replanning is required, the terrain adaptive repulsion field and the dynamic obstacle repulsion field are recalculated and the composite potential energy field is updated, where: Recalculate the terrain adaptive repulsion field, including: Determining a local update sensitivity threshold; Determine the coordinate points in the elevation time series change matrix that are greater than the local update sensitivity threshold and determine the affected area for dynamic adjustment; Recalculate and dynamically adjust the slope gain and roughness gain in the affected area, and update the slope gain and roughness gain; Based on the updated slope gain and roughness gain, an updated terrain gain is obtained; Recalculate the terrain adaptive repulsion field based on the updated terrain gain; or, Recalculate the dynamic obstacle repulsion field, including: Count newly added obstacles and disappeared obstacles; Calculate the repulsive field of newly added obstacles and the repulsive field of disappeared obstacles; On the basis of the original dynamic obstacle repulsion field, the repulsion field of the disappeared obstacle is removed and the repulsion field of the newly added obstacle is added to obtain an updated dynamic obstacle repulsion field.

8. The elevation map path planning method based on terrain adaptive potential field according to claim 1, characterized in that: When re-determining the next position of the agent based on the updated composite potential energy field, motion consistency constraints are introduced: Define a smooth energy function based on the historical path point sequence as a smoothing term; The smoothing term is superimposed on the original composite potential energy field to reconstruct the composite potential energy field; When re-determining the next position of the agent, the path optimization direction is constrained by the smoothing term gradient; or, The smooth energy function is specifically: Where: The first in the historical path The coordinates of the path points, , is the number of historical path points; The first in the historical path The coordinates of the path points; is the smoothing weight coefficient; is a smooth energy function.

9. The elevation map path planning method based on terrain adaptive potential field according to claim 7, characterized in that: It also includes further adding local update area constraints when determining the affected area for dynamic adjustment: ； in, is the index set of changing obstacles; is the safety margin extension; represents the radius of the repulsive field; Indicates the obstacle centers; Represents a local update region.

10. The elevation map path planning system based on terrain adaptive potential field is characterized by: include: The data acquisition module is configured to: acquire the elevation map and the position of the intelligent 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 them with the basic repulsion field to obtain the terrain adaptive repulsion field; The composite potential energy field calculation module is configured to: obtain a composite potential energy field based on the terrain adaptive repulsion field, the target gravity field, and the dynamic obstacle repulsion field; The overall planning module is configured to: determine the movement direction of the agent based on the composite potential energy field and determine the next position of the agent; The local replanning judgment module is configured to: determine whether it is necessary to perform local replanning on the next position of the intelligent agent based on a preset environment change judgment rule during the process of the intelligent agent moving to the next position; The composite potential energy field update module is configured to: when replanning is required, recalculate the terrain adaptive repulsion field and the dynamic obstacle repulsion field, and update the composite potential energy field; The local replanning execution module is configured to re-determine the next position of the agent based on the updated composite potential energy field.

Images