Complex terrain navigation device and method in three-dimensional space

Abstract

The invention provides a complex terrain navigation device and method in a three-dimensional space. Collecting topographic data of an environment where the ground unmanned platform is located; generating a three-dimensional map of the current environment in real time based on the topographic data, and acquiring coordinate information of a corresponding node of the ground unmanned platform in the three-dimensional map of the current environment; according to the current node coordinate of the ground unmanned platform in the three-dimensional map of the current environment and the previous node coordinate of the ground unmanned platform in the three-dimensional map of the current environment, utilizing a heuristic estimation function to predict the target node coordinate of the ground unmanned platform in the three-dimensional map of the current environment; according to the coordinates of the target node in the current environment three-dimensional map, planning a motion path from the ground unmanned platform to the target node; and the ground unmanned platform moves to the target node according to the motion path. According to the invention, the three-dimensional map is constructed and intelligent path planning is utilized, so that the unmanned platform can realize autonomous navigation and dynamic obstacle avoidance in complex terrains. And efficient and safe navigation is realized.

Description

Technical Field

[0001] The present invention relates to the field of intelligent driving technology, and in particular to a complex terrain navigation device and method in three-dimensional space. Background Art

[0002] With the development of unmanned platform technologies such as autonomous driving, unmanned delivery, and ground robots, ground unmanned vehicles are increasingly used in warehousing, logistics, agriculture, and security. Especially in complex terrains, such as mountainous areas, building ruins, and urban environments, traditional two-dimensional navigation technology can no longer meet the needs of accurate and efficient navigation. Ground unmanned platforms need to plan paths and avoid obstacles in a three-dimensional environment, and respond to dynamic environmental changes in real time. To this end, research on ground unmanned platform navigation technology suitable for complex terrain has become an important direction for improving the autonomy and intelligence of unmanned platforms.

[0003] Existing navigation technology solutions are mainly aimed at relatively flat or regular ground environments, such as path planning algorithms on two-dimensional planes. Such algorithms perform well on roads and simple industrial scenes, but have some problems in complex terrains such as mountains, gravel roads, building ruins and other three-dimensional complex terrains. Summary of the invention

[0004] The current intelligent navigation technology solutions have the following problems: First, two-dimensional path planning cannot accurately consider the height changes and slopes of the terrain, resulting in inaccurate navigation paths and affecting the safe driving of unmanned platforms. Secondly, the application of existing three-dimensional path planning algorithms in ground platforms is often computationally complex and difficult to achieve real-time response, especially in highly dynamic or narrow spaces. Finally, these algorithms are insufficient in dynamic obstacle avoidance and cannot respond to sudden environmental changes in a timely manner, affecting the autonomous obstacle avoidance capabilities of ground unmanned platforms. In response to this, the present invention proposes a complex terrain navigation device and method in three-dimensional space.

[0005] The technical solution of the present invention is: a complex terrain navigation device in three-dimensional space, which includes: a ground unmanned platform, a sensor module, a three-dimensional map construction module, a path planning module, and a control decision module;

[0006] A sensor module, which is used to collect terrain data of the environment in which the ground unmanned platform is located;

[0007] A three-dimensional map construction module, whose functions are: generating a three-dimensional map of the current environment in real time based on the terrain data, and obtaining coordinate information of corresponding nodes of the ground unmanned platform in the three-dimensional map of the current environment;

[0008] The path planning module has the following functions: according to the current node coordinates of the ground unmanned platform in the three-dimensional map of the current environment and the previous node coordinates of the ground unmanned platform in the three-dimensional map of the current environment, the target node coordinates of the ground unmanned platform in the three-dimensional map of the current environment are predicted by using a heuristic estimation function;

[0009] The control decision module has the following functions: planning the motion path of the ground unmanned platform to reach the target node according to the target node coordinates in the current three-dimensional map of the environment;

[0010] The ground unmanned platform moves to the target node according to the movement path.

[0011] In one embodiment, the sensor module includes: a laser radar, a visual camera, an ultrasonic sensor, and a GPS;

[0012] The laser radar obtains three-dimensional point cloud data of the environment where the ground unmanned platform is located;

[0013] The visual camera acquires and collects image information of the environment where the ground unmanned platform is located;

[0014] Ultrasonic sensors obtain information about terrain and obstacles in the environment where the ground unmanned platform is located;

[0015] GPS acquires and collects geographic coordinate information of the ground unmanned platform itself and its environment;

[0016] The three-dimensional point cloud data, the image information, the obstacle information, and the geographic coordinate information of the ground unmanned platform itself and its environment constitute the terrain data of the environment in which the ground unmanned platform is located.

[0017] In one embodiment, the three-dimensional map construction module generates a three-dimensional map of the current environment in real time based on the terrain data, and the specific method is:

[0018] The three-dimensional point cloud data, the image information, the obstacle information, and the geographic coordinate information of the ground unmanned platform itself and its environment are integrated to generate a three-dimensional map of the current environment.

[0019] In one embodiment, the path planning module predicts the target node coordinates of the ground unmanned platform in the three-dimensional map of the current environment using a heuristic estimation function based on the current node coordinates of the ground unmanned platform in the three-dimensional map of the current environment and the previous node coordinates of the ground unmanned platform in the three-dimensional map of the current environment; the specific method is:

[0020] The previous node of the ground unmanned platform in the three-dimensional map of the current environment is taken as the parent node, and the actual cost function g(n) is established according to the parent node and the current node of the ground unmanned platform in the three-dimensional map of the current environment:

[0021]

[0022] Among them, p represents the parent node, n represents the current node, g(p) represents the actual cost function of the parent node, and g(n) represents the actual cost function of the current node;

[0023] (x n ,y n ,z n ) and (x p ,y p ,z p ) are the coordinates of the current node and the parent node respectively;

[0024] α is a weight factor used to adjust the height difference, and α is greater than 1;

[0025] The heuristic estimation function h(n) is established based on the current node coordinates and the target node coordinates:

[0026]

[0027] Among them, (x goal ,y goal ,z goal ) are the coordinates of the target node;

[0028] The formula for evaluating the function f(n) is:

[0029] f(n)=g(n)+h(n)

[0030] The evaluation function f(n) is used to evaluate whether the path formed by the current node coordinates and the target node coordinates is feasible, and the method is as follows:

[0031] Where g(n) is the actual cost from the starting node to the current node n, h(n) is the heuristic estimated cost from the current node n to the target node, and the heuristic cost estimate should meet the following threshold conditions:

[0032] (1) Consistency: For the current node n and its adjacent node m, the heuristic cost estimate should satisfy:

[0033] h(n)≤c(n,m)+h(m)

[0034] Where c(n,m) is the actual cost from the current node n to node m;

[0035] (2) Acceptability: The heuristic function h(n) should satisfy that for the current node n, the estimated cost cannot exceed the actual cost, that is:

[0036] h(n)≤h * (n)

[0037] where h* (n) is the true optimal cost from the current node n to the target;

[0038] (3) Motion constraints: When the ground unmanned platform moves, there is a maximum height difference D(n) of the feasible motion trajectory. If this difference is exceeded, the ground unmanned platform cannot complete the actual motion trajectory; for the current node n and its adjacent node m, the following should be satisfied:

[0039] z n -z m <D(n)

[0040]

[0041] h(n)≤c(n,m)+h(m)

[0042] Among them, z n is the elevation of the current node n, z m is the elevation of node m;

[0043] When the path formed by the current node coordinates and the target node coordinates meets the constraint threshold condition, the path is feasible, otherwise the path is infeasible.

[0044] A complex terrain navigation method in three-dimensional space, comprising the following steps:

[0045] Collect terrain data of the environment where the ground unmanned platform is located;

[0046] Generate a three-dimensional map of the current environment in real time based on the terrain data, and obtain coordinate information of corresponding nodes of the ground unmanned platform in the three-dimensional map of the current environment;

[0047] According to the current node coordinates of the ground unmanned platform in the three-dimensional map of the current environment and the previous node coordinates of the ground unmanned platform in the three-dimensional map of the current environment, a heuristic estimation function is used to predict the target node coordinates of the ground unmanned platform in the three-dimensional map of the current environment;

[0048] According to the coordinates of the target node in the current three-dimensional map of the environment, plan the movement path of the ground unmanned platform to reach the target node;

[0049] The ground unmanned platform moves to the target node according to the movement path.

[0050] In one embodiment, the specific method for collecting terrain data of the environment where the ground unmanned platform is located is:

[0051] Obtain three-dimensional point cloud data of the environment where the ground unmanned platform is located;

[0052] Collect image information of the environment where the ground unmanned platform is located;

[0053] Obtain terrain and obstacle information of the environment where the ground unmanned platform is located;

[0054] Obtain and collect geographic coordinate information of the ground unmanned platform itself and its environment;

[0055] The terrain data of the environment in which the ground unmanned platform is located is constructed based on the three-dimensional point cloud data, the image information, the obstacle information, and the geographic coordinate information of the ground unmanned platform itself and its environment.

[0056] In one embodiment, a three-dimensional map of the current environment is generated in real time based on the terrain data, and the specific method is as follows:

[0057] The three-dimensional point cloud data, the image information, the obstacle information, and the geographic coordinate information of the ground unmanned platform itself and its environment are integrated to generate a three-dimensional map of the current environment.

[0058] In one embodiment, based on the current node coordinates of the ground unmanned platform in the three-dimensional map of the current environment and the previous node coordinates of the ground unmanned platform in the three-dimensional map of the current environment, a heuristic estimation function is used to predict the target node coordinates of the ground unmanned platform in the three-dimensional map of the current environment; the specific method is:

[0059] The previous node of the ground unmanned platform in the three-dimensional map of the current environment is taken as the parent node, and the actual cost function g(n) is established according to the parent node and the current node of the ground unmanned platform in the three-dimensional map of the current environment:

[0060]

[0061] Among them, p represents the parent node, n represents the current node, g(p) represents the actual cost function of the parent node, and g(n) represents the actual cost function of the current node;

[0062] (x n ,y n ,z n ) and (x p ,y p ,z p ) are the coordinates of the current node and the parent node respectively;

[0063] α is a weight factor used to adjust the height difference, and α is greater than 1;

[0064] The heuristic estimation function h(n) is established based on the current node coordinates and the target node coordinates:

[0065]

[0066] Among them, (x goal ,ygoal ,z goal ) are the coordinates of the target node;

[0067] The formula for evaluating the function f(n) is:

[0068] f(n)=g(n)+h(n)

[0069] The evaluation function f(n) is used to evaluate whether the path formed by the current node coordinates and the target node coordinates is feasible, and the method is:

[0070] Where g(n) is the actual cost from the starting node to the current node n, h(n) is the heuristic estimated cost from the current node n to the target node, and the heuristic cost estimate should meet the following threshold conditions:

[0071] (1) Consistency: For the current node n and its adjacent node m, the heuristic cost estimate should satisfy:

[0072] h(n)≤c(n,m)+h(m)

[0073] Where c(n,m) is the actual cost from the current node n to node m;

[0074] (2) Acceptability: The heuristic function h(n) should satisfy that for the current node n, the estimated cost cannot exceed the actual cost, that is:

[0075] h(n)≤h * (n)

[0076] where h * (n) is the true optimal cost from the current node n to the target;

[0077] (3) Motion constraints: When the ground unmanned platform moves, there is a maximum height difference D(n) of the feasible motion trajectory. If this difference is exceeded, the ground unmanned platform cannot complete the actual motion trajectory; for the current node n and its adjacent node m, the following should be satisfied:

[0078] z n -z m <D(n)

[0079]

[0080] h(n)≤c(n,m)+h(m)

[0081] Among them, z n is the elevation of the current node n, z m is the elevation of node m;

[0082] When the path formed by the current node coordinates and the target node coordinates meets the constraint threshold condition, the path is feasible, otherwise the path is infeasible.

[0083] The advantages of the present invention over the prior art are as follows: the present invention proposes a ground unmanned platform navigation device and method suitable for three-dimensional complex terrain, which uses multi-sensor fusion technology, three-dimensional map construction technology and adaptive path planning algorithm to enable the unmanned platform to achieve autonomous navigation and dynamic obstacle avoidance in complex terrain. The method achieves efficient and safe navigation by building a real-time three-dimensional environment model and combining path planning with obstacle avoidance mechanism. BRIEF DESCRIPTION OF THE DRAWINGS

[0084] Figure 1 The system structure and flow chart of the complex terrain navigation device in three-dimensional space of the present invention. DETAILED DESCRIPTION

[0085] The present invention provides a complex terrain navigation device and method in three-dimensional space. Figure 1 The shown components include: ground unmanned platform, sensor module (such as lidar, visual camera, ultrasonic sensor, GPS, etc.), three-dimensional map construction module, path planning module, and control decision module.

[0086] (1) Sensor module: The unmanned platform is equipped with multiple sensors, which are used to collect terrain data of the environment in which the ground unmanned platform is located. Multiple sensors collect depth, distance, and location information in the environment in real time. Through data fusion technology, a three-dimensional map of the current map can be obtained to improve the perception accuracy of the terrain.

[0087] (2) Three-dimensional map construction module: Based on the terrain data obtained by the sensor, a three-dimensional map of the current terrain is generated in real time, and the self-positioning coordinate information of the ground platform in a complex environment is realized through SLAM technology.

[0088] (3) Path planning module: Adopting an adaptive path planning algorithm, combined with multi-dimensional information such as terrain height difference and obstacles, the optimal path is planned to ensure safe and efficient driving of the unmanned platform. Preferably, based on the current node coordinates of the ground unmanned platform in the three-dimensional map of the current environment and the previous node coordinates of the ground unmanned platform in the three-dimensional map of the current environment, a heuristic estimation function is used to predict the target node coordinates of the ground unmanned platform in the three-dimensional map of the current environment.

[0089] (4) Control decision module: monitors the motion state of the unmanned platform in real time, and plans the motion path of the ground unmanned platform to reach the target node according to the coordinates of the target node in the current three-dimensional map of the environment. In combination with environmental changes, the path is adjusted through a dynamic obstacle avoidance algorithm to ensure that the ground platform can safely avoid obstacles.

[0090] The ground unmanned platform moves to the target node according to the movement path. The ground unmanned platform, sensor module, three-dimensional map construction module, path planning module, and control decision module can be set on the ground unmanned platform or on other devices that can interact with the ground unmanned platform for data.

[0091] In terms of the process, the sensor module first obtains environmental data, and the 3D map construction module updates the map accordingly. Then, the path planning module generates a driving route that adapts to the current environment, and the control decision module makes real-time adjustments to ensure that the unmanned platform can navigate efficiently and avoid obstacles autonomously in complex terrain.

[0092] Module Implementation

[0093] (1) 3D map construction module

[0094] This technical solution proposes a method that combines laser radar and visual camera sensor data to generate a three-dimensional map of the current terrain in real time, which is suitable for unmanned platform navigation in complex terrain. The laser radar obtains the three-dimensional point cloud data of the environment where the ground unmanned platform is located; the visual camera obtains the image information of the environment where the ground unmanned platform is located; the ultrasonic sensor obtains the terrain undulations and obstacle information of the environment where the ground unmanned platform is located; the GPS obtains the geographic coordinate information of the ground unmanned platform itself and its environment; the three-dimensional point cloud data, the image information, the obstacle information, the geographic coordinate information of the ground unmanned platform itself and its environment constitute the terrain data of the environment where the ground unmanned platform is located. The three-dimensional point cloud data, the image information, the obstacle information, the geographic coordinate information of the ground unmanned platform itself and its environment are integrated to generate a three-dimensional map of the current environment.

[0095] In one embodiment, by fusing the high-precision three-dimensional point cloud data obtained by a laser radar (such as Livox Mid360) with the image information captured by a visual camera, a three-dimensional map is generated in real time using the simultaneous localization and mapping (SLAM) technology. During the fusion process, the two-dimensional image information of the visual camera is converted into terrain feature points in three-dimensional space using techniques such as image depth estimation, feature matching, and coordinate transformation to supplement the perception blind spots of the laser radar. The generated three-dimensional map is stored and updated in the form of an occupancy grid, and a dynamic three-dimensional environment model is constructed based on an efficient sparse occupancy map (such as UFOMap) to achieve accurate and efficient real-time terrain mapping. This method has high robustness and environmental adaptability, and can be applied to autonomous navigation and obstacle avoidance of ground unmanned platforms such as unmanned vehicles and robots in complex terrain.

[0096] (2) Path Planning Module

[0097] The path planning module predicts the target node coordinates of the ground unmanned platform in the three-dimensional map of the current environment using a heuristic estimation function based on the current node coordinates of the ground unmanned platform in the three-dimensional map of the current environment and the previous node coordinates of the ground unmanned platform in the three-dimensional map of the current environment. In one embodiment, the technical solution proposes a path planning algorithm that is suitable for a three-dimensional environment model (UFOMap, etc.) and improves and optimizes the A* algorithm.

[0098] The core idea of ​​the A* algorithm is to use heuristic estimation function

[0099] f(n)=g(n)+n(n)

[0100] Each node is evaluated for priority, where:

[0101] g(n) is the actual cost from the starting point to the current node n.

[0102] h(n) is the heuristic estimate of the distance from the current node n to the target node.

[0103] In a three-dimensional map, the height difference of the terrain will affect the actual cost of path planning. Therefore, the calculation method of g(n) and h(n) needs to be modified to take into account the movement cost in three-dimensional space, especially the impact of ground height difference on ground unmanned platforms.

[0104] Improved algorithm:

[0105] 1. Actual cost function g(n):

[0106] For three-dimensional space, the cost function must not only consider the horizontal movement distance, but also the change in height. Therefore, the actual cost can be defined as:

[0107]

[0108] in:

[0109] p is the parent node and n is the current node.

[0110] (x n ,y n ,z n ) and (x p ,y p ,z p ) are the coordinates of the current node and the parent node respectively.

[0111] α is a weight factor used to adjust the height difference to control the impact of the height difference on the path cost. For ground unmanned platforms, vertical movement is often more expensive than horizontal movement, so α is usually greater than 1.

[0112] 2. Heuristic function h(n):

[0113] The heuristic function needs to estimate the cost from the current node to the target node. Similarly, in three-dimensional space, both horizontal distance and height difference need to be considered. You can use the three-dimensional Euclidean distance as a heuristic function:

[0114]

[0115] in:

[0116] (x goal ,y goal ,z goal ) are the coordinates of the target node.

[0117] α is the same weighting factor as in g(n) and is used to control the influence of height differences in the estimate.

[0118] In summary, the improved A* algorithm formula is:

[0119] In the A* algorithm in the three-dimensional map, combined with the cost of the ground height difference and heuristic estimation, the improved formula of the entire evaluation function f(n) is:

[0120] f(n)=g(n)+h(n)

[0121] Right now:

[0122]

[0123] Choice of parameter α

[0124] The value of α needs to be adjusted according to the terrain conditions. If the height difference of the ground has a greater impact on the path (such as steep terrain), a larger α value (such as 2 or 3) can be used. If the terrain is relatively flat, the value of α can be close to 1.

[0125] The evaluation function f(n) is used to evaluate whether the path formed by the current node coordinates and the target node coordinates is feasible, and the method is as follows:

[0126] Where g(n) is the actual cost from the starting node to the current node n, and h(n) is the heuristic estimated cost from the current node to the target node. In order to effectively evaluate the priority of the node, the following threshold conditions must be met:

[0127] (1) Consistency: For all nodes n and their adjacent nodes m, the following should be satisfied:

[0128] h(n)≤c(n,m)+h(m)

[0129] where c(n,m) is the actual cost from node n to node m. This condition ensures that the heuristic function does not underestimate the cost from the current node to the goal.

[0130] (2) Acceptability: The heuristic function h(n) should satisfy that for all nodes n, the estimated cost cannot exceed the actual cost, that is:

[0131] h(n)≤h * (n)

[0132] where h * (n) is the true optimal cost from node n to the target. This ensures that the path found is optimal.

[0133] (3) Motion constraints: When the ground unmanned platform moves, there is a maximum height difference D(n) of the feasible motion trajectory. If this difference is exceeded, the actual motion trajectory cannot be completed. Therefore, for all nodes n and their adjacent nodes m, the following should be satisfied:

[0134] z n -z m <D(n)

[0135]

[0136] h(n)≤c(n,m)+h(m)

[0137] Ensure that the algorithm path satisfies the motion constraints. n is the elevation of the current node n, z m is the elevation of node m;

[0138] When the path formed by the current node coordinates and the target node coordinates meets the constraint threshold condition, the path is feasible, otherwise the path is infeasible.

[0139] (3) Control decision module

[0140] This technical solution proposes an unmanned platform navigation method based on an adaptive path planning algorithm. This module can dynamically plan the optimal driving path by combining multi-dimensional environmental information such as terrain height difference and obstacle distribution to ensure the safety and efficiency of the unmanned platform. First, the three-dimensional map generated by the fusion of laser radar and visual camera is used to extract information such as the location of obstacles and terrain undulations in the environment. Combined with the numerical information of terrain height difference, the path planning algorithm proposed above is adopted to adaptively adjust the path planning parameters according to the weights such as the slope of different terrains and obstacle distances to ensure that the unmanned platform can choose the most stable and safest route in a complex environment. At the same time, the algorithm is real-time and can quickly replan the path to avoid collisions when dynamic obstacles are detected. By updating environmental information in real time through a three-dimensional grid map (such as UFOMap), the module can dynamically adjust the path to ensure autonomous and efficient navigation of the unmanned platform in complex terrain.

[0141] The present invention has the following advantages:

[0142] (1) The present invention uses multi-sensor fusion technology to obtain and fuse three-dimensional terrain information in real time. Since this technology can accurately obtain complex terrain features such as height and slope, it solves the problem that traditional two-dimensional navigation cannot cope with three-dimensional complex terrain.

[0143] (2) The present invention uses an adaptive path planning algorithm to dynamically plan the optimal path based on real-time changing terrain and obstacle information. This technical means ensures the flexibility and safety of navigation and solves the problem of inaccurate path planning in complex terrain in the prior art.

[0144] (3) The present invention uses an autonomous obstacle avoidance algorithm to ensure that the ground unmanned platform can quickly adjust and safely avoid obstacles when the environment changes suddenly, thereby achieving safe navigation and efficient operation in complex terrain.

Claims

1. A complex terrain navigation device in three-dimensional space, characterized in that: It includes: Ground unmanned platform, sensor module, 3D map building module, path planning module, control decision module; A sensor module, which is used to collect terrain data of the environment in which the ground unmanned platform is located; A three-dimensional map construction module, whose functions are: generating a three-dimensional map of the current environment in real time based on the terrain data, and obtaining coordinate information of corresponding nodes of the ground unmanned platform in the three-dimensional map of the current environment; The path planning module has the following functions: according to the current node coordinates of the ground unmanned platform in the three-dimensional map of the current environment and the previous node coordinates of the ground unmanned platform in the three-dimensional map of the current environment, the target node coordinates of the ground unmanned platform in the three-dimensional map of the current environment are predicted by using a heuristic estimation function; The control decision module has the following functions: planning the motion path of the ground unmanned platform to reach the target node according to the target node coordinates in the current three-dimensional map of the environment; The ground unmanned platform moves to the target node according to the movement path.

2. The device according to claim 1, characterized in that The sensor module includes: laser radar, visual camera, ultrasonic sensor, GPS; The laser radar obtains three-dimensional point cloud data of the environment where the ground unmanned platform is located; The visual camera acquires and collects image information of the environment where the ground unmanned platform is located; Ultrasonic sensors obtain information about terrain undulations and obstacles in the environment where the ground unmanned platform is located; GPS acquires and collects geographic coordinate information of the ground unmanned platform itself and its environment; The three-dimensional point cloud data, the image information, the obstacle information, and the geographic coordinate information of the ground unmanned platform itself and its environment constitute the terrain data of the environment in which the ground unmanned platform is located.

3. The device according to claim 1, characterized in that The three-dimensional map construction module generates a three-dimensional map of the current environment in real time based on the terrain data. The specific method is as follows: The three-dimensional point cloud data, the image information, the obstacle information, and the geographic coordinate information of the ground unmanned platform itself and its environment are integrated to generate a three-dimensional map of the current environment.

4. The device according to claim 1, characterized in that The path planning module predicts the target node coordinates of the ground unmanned platform in the three-dimensional map of the current environment using a heuristic estimation function based on the current node coordinates of the ground unmanned platform in the three-dimensional map of the current environment and the previous node coordinates of the ground unmanned platform in the three-dimensional map of the current environment; the specific method is: The previous node of the ground unmanned platform in the three-dimensional map of the current environment is taken as the parent node, and the actual cost function g(n) is established according to the parent node and the current node of the ground unmanned platform in the three-dimensional map of the current environment: Among them, p represents the parent node, n represents the current node, g(p) represents the actual cost function of the parent node, and g(n) represents the actual cost function of the current node; (x n ,y n ,z n ) and (x p ,y p ,z p ) are the coordinates of the current node and the parent node respectively; α is a weight factor used to adjust the height difference, and α is greater than 1; The heuristic estimation function h(n) is established based on the current node coordinates and the target node coordinates: Among them, (x goal ,y goal ,z goal ) are the coordinates of the target node; The formula for evaluating the function f(n) is: f(n)=g(n)+h(n) The evaluation function f(n) is used to evaluate whether the path formed by the current node coordinates and the target node coordinates is feasible, and the method is as follows: Where g(n) is the actual cost from the starting node to the current node n, h(n) is the heuristic estimated cost from the current node n to the target node, and the heuristic cost estimate should meet the following threshold conditions: (1) Consistency: For the current node n and its adjacent node m, the heuristic cost estimate should satisfy: h(n)≤c(n,m)+h(m) Where c(n,m) is the actual cost from the current node n to node m; (2) Acceptability: The heuristic function h(n) should satisfy that for the current node n, the estimated cost cannot exceed the actual cost, that is: h(n)≤h * (n) where h * (n) is the true optimal cost from the current node n to the target; (3) Motion constraints: When the ground unmanned platform moves, there is a maximum height difference D(n) of the feasible motion trajectory. If this difference is exceeded, the ground unmanned platform cannot complete the actual motion trajectory; for the current node n and its adjacent node m, the following should be satisfied: With n -With m <D(n) h(n)≤c(n,m)+h(m) Among them, z n is the elevation of the current node n, z m is the elevation of node m; When the path formed by the current node coordinates and the target node coordinates meets the constraint threshold condition, the path is feasible, otherwise the path is infeasible.

5. A complex terrain navigation method in three-dimensional space, characterized in that: It includes the following steps: Collect terrain data of the environment where the ground unmanned platform is located; Generate a three-dimensional map of the current environment in real time based on the terrain data, and obtain coordinate information of corresponding nodes of the ground unmanned platform in the three-dimensional map of the current environment; According to the current node coordinates of the ground unmanned platform in the three-dimensional map of the current environment and the previous node coordinates of the ground unmanned platform in the three-dimensional map of the current environment, a heuristic estimation function is used to predict the target node coordinates of the ground unmanned platform in the three-dimensional map of the current environment; According to the coordinates of the target node in the current three-dimensional map of the environment, plan the movement path of the ground unmanned platform to reach the target node; The ground unmanned platform moves to the target node according to the movement path.

6. The method according to claim 5, characterized in that The specific method for collecting terrain data of the environment where the ground unmanned platform is located is: Obtain three-dimensional point cloud data of the environment where the ground unmanned platform is located; Collect image information of the environment where the ground unmanned platform is located; Obtain terrain and obstacle information of the environment where the ground unmanned platform is located; Obtain and collect geographic coordinate information of the ground unmanned platform itself and its environment; The terrain data of the environment in which the ground unmanned platform is located is constructed based on the three-dimensional point cloud data, the image information, the obstacle information, and the geographic coordinate information of the ground unmanned platform itself and its environment.

7. The method according to claim 5, characterized in that A three-dimensional map of the current environment is generated in real time based on the terrain data, and the specific method is as follows: The three-dimensional point cloud data, the image information, the obstacle information, and the geographic coordinate information of the ground unmanned platform itself and its environment are integrated to generate a three-dimensional map of the current environment.

8. The method according to claim 5, characterized in that According to the current node coordinates of the ground unmanned platform in the three-dimensional map of the current environment and the previous node coordinates of the ground unmanned platform in the three-dimensional map of the current environment, the heuristic estimation function is used to predict the target node coordinates of the ground unmanned platform in the three-dimensional map of the current environment; the specific method is: The previous node of the ground unmanned platform in the three-dimensional map of the current environment is taken as the parent node, and the actual cost function g(n) is established according to the parent node and the current node of the ground unmanned platform in the three-dimensional map of the current environment: Among them, p represents the parent node, n represents the current node, g(p) represents the actual cost function of the parent node, and g(n) represents the actual cost function of the current node; (x n ,y n ,z n ) and (x p ,y p ,z p ) are the coordinates of the current node and the parent node respectively; α is a weight factor used to adjust the height difference, and α is greater than 1; The heuristic estimation function h(n) is established based on the current node coordinates and the target node coordinates: Among them, (x goal ,y goal ,z goal ) are the coordinates of the target node; The formula for evaluating the function f(n) is: f(n)=g(n)+n(n) The evaluation function f(n) is used to evaluate whether the path formed by the current node coordinates and the target node coordinates is feasible, and the method is: Where g(n) is the actual cost from the starting node to the current node n, h(n) is the heuristic estimated cost from the current node n to the target node, and the heuristic cost estimate should meet the following threshold conditions: (1) Consistency: For the current node n and its adjacent node m, the heuristic cost estimate should satisfy: h(n)≤c(n,m)+h(m) Where c(n,m) is the actual cost from the current node n to node m; (2) Acceptability: The heuristic function h(n) should satisfy that for the current node n, the estimated cost cannot exceed the actual cost, that is: h(n)≤h * (n) where h * (n) is the true optimal cost from the current node n to the target; (3) Motion constraints: When the ground unmanned platform moves, there is a maximum height difference D(n) of the feasible motion trajectory. If this difference is exceeded, the ground unmanned platform cannot complete the actual motion trajectory; for the current node n and its adjacent node m, the following should be satisfied: With n -With m <D(n) h(n)≤c(n,m)+h(m) Among them, z n is the elevation of the current node n, z m is the elevation of node m; When the path formed by the current node coordinates and the target node coordinates meets the constraint threshold condition, the path is feasible, otherwise the path is infeasible.

Images