A non-flat terrain heterogeneous collaborative navigation method based on monocular depth estimation
By combining monocular depth estimation with laser-optical flow sensors to generate dense environmental point clouds, the problem of high sensor weight and power consumption in complex terrain of UAV and unmanned vehicle systems is solved, enhancing terrain perception capabilities and improving navigation safety and efficiency.
Patent Information
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- TIANMUSHAN LABORATORY
- Filing Date
- 2026-02-02
- Publication Date
- 2026-06-05
AI Technical Summary
Existing drone and unmanned vehicle collaborative systems suffer from heavy sensor weight and power consumption in complex and uneven terrain environments, and their ability to perceive terrain features is insufficient, making it difficult to meet navigation and decision-making requirements.
A monocular depth estimation-based method is adopted, which combines a monocular camera and an airborne laser-optical flow sensor to generate dense environmental point clouds, assess traversability costs, and plan the autonomous navigation trajectory of unmanned vehicles.
Significantly reduces UAV payload weight and power consumption, enhances the ability to characterize complex and uneven terrain, improves navigation safety and efficiency, and achieves a balance between real-time high-precision environmental perception and low system power consumption.
Smart Images
Figure CN122149505A_ABST
Abstract
Description
Technical Field
[0001] This invention belongs to the field of heterogeneous cooperative autonomous navigation technology, and in particular relates to a heterogeneous cooperative navigation method for non-flat terrain based on monocular depth estimation. Background Technology
[0002] Currently, with the development of science and technology, air-ground heterogeneous systems formed by the collaboration of drones and unmanned vehicles have been widely used in many fields. In such collaborative systems, drones, with their high-altitude field of vision advantage, can provide effective assistance to unmanned vehicles operating in unknown environments. For example, Chinese invention patent application CN119860777A discloses a highly reliable positioning and navigation method for air-ground collaborative unmanned systems, Chinese invention patent application CN118730120A discloses an air-ground collaborative drone planning and navigation method and system, and Chinese invention patent application CN118408545A discloses an air-ground collaborative intelligent assisted driving and navigation method and collaborative system.
[0003] There are two main types of technical solutions in the current technology. One type uses airborne LiDAR and other sensors to acquire environmental point cloud information, providing data support for the environmental perception of unmanned vehicles (UAVs). However, LiDAR is heavy and consumes a lot of power, affecting the UAV's endurance in the air. The other type uses a UAV equipped with a monocular camera to provide environmental semantic information to the UAV through image semantic segmentation algorithms. However, this type of method often oversimplifies terrain features such as slope, ridges, and depressions in complex environments such as uneven terrain, making it difficult to meet the navigation and decision-making needs of UAVs in complex scenarios. Summary of the Invention
[0004] To address the problems existing in the prior art, this invention proposes a heterogeneous cooperative navigation method for non-flat terrain based on monocular depth estimation. This navigation method provides a dense environmental point cloud for unmanned vehicles through monocular depth estimation, assisting them in autonomous navigation in non-flat environments. This solves the problems of large sensor weight and power consumption, sparse data, and insufficient perception capability for complex terrain in existing technologies.
[0005] The technical solution of the present invention is as follows: A heterogeneous cooperative navigation method for non-flat terrain based on monocular depth estimation includes the following steps: Step S1: The UAV observes the unmanned vehicle in the target's field of view, acquires images using an airborne downward-looking monocular camera, and estimates the relative depth of the environment. relative depth gradient with respect to the environment , , These are the pixel coordinates on the image captured by the airborne downward-looking monocular camera; Step S2: Based on the relative depth and relative depth gradient of the environment, combined with the altitude information obtained by the airborne laser-optical flow sensor and the global positioning information of the UAV, and the relative distance between the UAV and the unmanned vehicle estimated by the airborne downward-looking monocular camera, the true depth of the environment is recovered. With the actual depth gradient of the environment , ; Step S3: Based on the actual environmental depth, the actual environmental depth gradient, the relative pose estimation results of the vehicle-mounted QR code by the airborne downward-looking monocular camera, and the global positioning information of the UAV, construct a terrain elevation raster map and a terrain elevation gradient raster map to characterize the terrain undulation. Step S4: Based on the terrain elevation grid map and terrain elevation gradient grid map, evaluate the traversal cost of the unmanned vehicle in non-flat terrain environments; Step S5: Based on the traversability cost, and combining the current pose of the unmanned vehicle with the target pose, plan and generate the autonomous navigation trajectory of the unmanned vehicle. Step S6: The unmanned vehicle travels according to the autonomous navigation trajectory to achieve autonomous navigation.
[0006] Preferably, in step S2, the true depth of the environment is restored. With the actual depth gradient of the environment , Use the following expression:
[0007]
[0008]
[0009] in, For the drone to estimate its own altitude, The relative distance between the drone and the unmanned vehicle. These are the pixel coordinates of the ranging point of the airborne laser-optical flow sensor on the image acquired by the airborne downward-looking monocular camera. The pixel-relative depth of the ranging point of the airborne laser-optical flow sensor. The pixel coordinates of the images captured by the onboard downward-looking monocular camera of the autonomous vehicle. This represents the relative depth of pixels for the autonomous vehicle.
[0010] Preferably, step S3 specifically includes: Step S3-1: Based on the actual depth of the environment, obtain the pixel coordinates on the image acquired by the airborne downward-looking monocular camera. Corresponding camera coordinate system coordinates ; Step S3-2: Set the coordinates in the camera coordinate system Convert to ground coordinate system coordinates ; Step S3-3: Rasterize the target field of view. Based on the ground coordinate system coordinates and the actual depth gradient of the environment, calculate the terrain elevation and terrain elevation gradient in each grid, and construct a terrain elevation raster map and a terrain elevation gradient raster map to represent the terrain undulation.
[0011] Preferably, in step S3-1 The expression is:
[0012] in, The focal length of the airborne downward-looking monocular camera. The vertical focal length of the airborne downward-looking monocular camera. These are the principal point coordinates of the airborne downward-looking monocular camera.
[0013] Preferably, the ground coordinate system coordinates in step S3-2 The expression is:
[0014] in, This is the transformation matrix from the body coordinate system to the ground coordinate system. This is the transformation matrix from the camera coordinate system to the body coordinate system. The coordinates of the origin of the camera coordinate system in the ground coordinate system.
[0015] Preferably, in step S3-3, the terrain elevation and terrain elevation gradient within each grid cell are calculated by averaging.
[0016] Preferably, the expression for the traversability cost in step S4 is:
[0017] in, raster coordinates The cost of traversability at that location Costs related to the flatness of the surrounding terrain. For slope cost, Cost based on height difference All are weighting coefficients.
[0018] Preferably, step S5 specifically includes: Step S5-1: Based on the current pose and target pose of the autonomous vehicle, by considering the cost of traversability, A... * The algorithm generates initial trajectory values; Step S5-2: Based on the initial trajectory value, establish the optimization objective function and constraints to optimize and obtain the autonomous navigation trajectory of the unmanned vehicle.
[0019] Preferably, the objective function in step S5-2 is:
[0020] in, To optimize the objective function, For the autonomous vehicle's state variable vector, For time, For trajectory duration, Let be the order of the differential equation. These are the weighting coefficients. To assess the traversal cost of autonomous vehicles crossing rugged terrain.
[0021] Preferably, step 6 specifically includes: the UAV transmitting the generated autonomous navigation trajectory to the unmanned vehicle through the vehicle-to-vehicle communication link, and the unmanned vehicle running a predictive controller on the onboard computer to track the autonomous navigation trajectory and achieve autonomous navigation.
[0022] Compared with the prior art, the beneficial effects of the present invention are as follows: 1. The heterogeneous cooperative navigation method for non-flat terrain based on monocular depth estimation proposed in this invention adopts a perception scheme that is mainly based on monocular vision and supplemented by single-point laser ranging, which significantly reduces the payload weight and power consumption of UAVs, and is conducive to improving endurance. It is applicable to small UAVs and unmanned vehicle systems.
[0023] 2. The heterogeneous cooperative navigation method for non-flat terrain based on monocular depth estimation proposed in this invention generates dense environmental point clouds through monocular depth estimation and combines them with depth gradient inference to enhance the representation ability of complex non-flat terrain. It overcomes the problem of oversimplification of environmental terrain features by traditional semantic segmentation methods and achieves a balance between real-time high-precision environmental perception and low system power consumption.
[0024] 3. The heterogeneous cooperative navigation method for non-flat terrain based on monocular depth estimation proposed in this invention evaluates the traversability cost based on the dense environmental point cloud acquired by the monocular camera, making the trajectory planning more in line with the actual terrain and improving the navigation safety, feasibility and efficiency of unmanned vehicles in non-flat terrain. Attached Figure Description
[0025] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the accompanying drawings used in the embodiments will be briefly introduced below. The features and advantages of the present invention can be more clearly understood by referring to the accompanying drawings. The accompanying drawings are schematic and should not be construed as limiting the present invention in any way. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.
[0026] Figure 1 This is a flowchart of the heterogeneous cooperative navigation method for non-flat terrain based on monocular depth estimation proposed in this invention.
[0027] Figure 2 This is a schematic diagram of the composition of the drone and unmanned vehicle collaborative system in Example 1.
[0028] Figure 3 This is a schematic diagram of the relative distance estimation for restoring the true depth in this invention.
[0029] Figure 4 This is a schematic diagram of the relative depth estimation results in Example 1.
[0030] Figure 5 This is a schematic diagram of the terrain elevation grid map and the autonomous navigation trajectory of the unmanned vehicle in Example 1.
[0031] The diagram is labeled as follows: 1-UAV, 1-1-Airborne downward-looking monocular camera, 1-2-Airborne computer, 1-3-Airborne laser-optical flow sensor, 2-Unmanned vehicle, 2-1-QR code, 2-2-Vehicle computer, 3-Data transmission, 4-Target node, 5-Autonomous navigation trajectory. Detailed Implementation
[0032] To better understand the above-mentioned objectives, features, and advantages of the present invention, the present invention will be further described in detail below with reference to the accompanying drawings and specific embodiments. It should be noted that, unless otherwise specified, the embodiments of the present invention and the features thereof can be combined with each other.
[0033] Many specific details are set forth in the following description in order to provide a full understanding of the invention. However, the invention may also be practiced in other ways different from those described herein, and therefore the scope of protection of the invention is not limited to the specific embodiments disclosed below.
[0034] The heterogeneous cooperative navigation method for non-flat terrain based on monocular depth estimation proposed in this invention includes the following steps, such as... Figure 1 As shown: Step S1: The drone observes the unmanned vehicle. When the unmanned vehicle enters the drone's field of view, the relative depth of the environment is estimated based on the images captured by the drone's onboard downward-looking monocular camera. relative depth gradient with respect to the environment , ,in These are the pixel coordinates on the image plane of a downward-looking monocular camera.
[0035] Step S2: Based on the relative depth and relative depth gradient of the environment, combined with the distance and positioning information obtained by the airborne laser-optical flow sensor, and the relative distance between the UAV and the unmanned vehicle estimated by the airborne downward-looking monocular camera, the true depth of the environment is recovered. With the actual depth gradient of the environment , .
[0036] in, The true depth of the environment for:
[0037]
[0038] in, For the drone to estimate its own altitude, The relative distance between the drone and the unmanned vehicle was obtained from the QR code 2-1 on the unmanned vehicle. These are the pixel coordinates of the ranging point of the airborne laser-optical flow sensor on the image acquired by the airborne downward-looking monocular camera. The pixel-relative depth of the ranging point of the airborne laser-optical flow sensor. The pixel coordinates of the images captured by the onboard downward-looking monocular camera of the autonomous vehicle. This represents the relative depth of pixels for the autonomous vehicle.
[0039]
[0040] in, The set of pixels in the central region of the image. This represents the number of pixels in the center region of the image.
[0041] The true depth of the environment for:
[0042] Step S3: Based on the true environmental depth and true environmental depth gradient obtained in step S2, and combined with the relative pose estimation results of the UAV on the unmanned vehicle, construct a terrain elevation-grid map and a terrain elevation gradient grid map to characterize the terrain undulation.
[0043] Given pixel coordinates on an image acquired by an airborne downward-looking monocular camera Corresponding camera coordinate system coordinates for:
[0044] in, The focal length of the airborne downward-looking monocular camera. The vertical focal length of the airborne downward-looking monocular camera. These are the principal point coordinates of the airborne downward-looking monocular camera.
[0045] Accordingly, convert to ground coordinate system coordinates for:
[0046] in, This is the transformation matrix from the body coordinate system to the ground coordinate system. This is the transformation matrix from the camera coordinate system to the body coordinate system. The coordinates of the origin of the camera coordinate system in the ground coordinate system.
[0047] The terrain elevation and terrain elevation gradient within each grid cell are calculated using a cumulative averaging method: The terrain elevation within each grid cell is calculated by averaging the sums:
[0048] in, raster coordinates The terrain elevation at the location, raster coordinates The number of pixels within, For the first The height of each pixel in the ground coordinate system.
[0049] The terrain elevation gradient within each grid cell is accumulated and averaged:
[0050]
[0051] in, raster coordinates The topographic elevation gradient at the location, These are the horizontal coordinates in the ground coordinate system. It is the Jacobian matrix from the image plane of the downward-looking monocular camera to the ground coordinate system plane.
[0052] Step S4: Based on the terrain elevation grid map and terrain elevation gradient grid map, evaluate the traversal cost of the unmanned vehicle in non-flat terrain environments.
[0053] Among them, grid coordinates The cost of traversability Build as:
[0054] in, raster coordinates The cost of traversability at that location Costs related to the flatness of the surrounding terrain. For slope cost, Cost based on height difference All are weighting coefficients.
[0055] The cost of ensuring the flatness of the surrounding terrain is:
[0056] in raster coordinates The set of neighborhood grids, for The number of grid elements, for The Middle The terrain elevation corresponding to each raster element.
[0057] The slope cost is:
[0058]
[0059] in, These are raster coordinates estimated based on terrain elevation gradients. The terrain normal vector, For the normalized terrain normal vector, Vertical vector .
[0060] The cost of the height difference is:
[0061] in, This represents the current elevation of the unmanned vehicle in the ground coordinate system.
[0062] Step S5: Based on the traversability cost, and combining the current pose of the unmanned vehicle with the target pose, plan and generate the autonomous navigation trajectory of the unmanned vehicle.
[0063] (1) Based on the current pose of the unmanned vehicle and the target pose, the initial trajectory value is generated by the A* algorithm that takes into account the cost of traversability.
[0064] using grid coordinates For the current node, its total cost function for:
[0065] in, The cumulative cost from the starting point to the current node. As a heuristic cost, we take the Euclidean distance from the current node to the target node 4.
[0066]
[0067] in, Accumulate the cost for the parent node. The horizontal diagonal distance from the current node to its parent node. The grid coordinates of the parent node.
[0068] (2) Based on the initial trajectory value, establish the optimization objective function and constraints, and optimize to obtain the autonomous navigation trajectory of the unmanned vehicle.
[0069] The objective function to be optimized is:
[0070] in, To optimize the objective function, For the autonomous vehicle's state variable vector, For time, For trajectory duration, Let be the order of the differential equation. These are the weighting coefficients. To assess the traversal cost of autonomous vehicles crossing rugged terrain, The larger the size, the more difficult it is for the driverless car to traverse.
[0071] Step S6: The UAV transmits the generated autonomous navigation trajectory to the unmanned vehicle through the vehicle-to-vehicle communication link. The unmanned vehicle runs a predictive controller on the onboard computer to track the autonomous navigation trajectory and achieve autonomous navigation.
[0072] Example 1 This embodiment uses, as follows Figure 2 The system shown is a collaborative system of UAV 1 and unmanned vehicle 2, using images output by the airborne downward-looking monocular camera 1-1 during actual flight in a non-flat terrain environment. The environment includes gentle slopes of 5°~15° and low vegetation with a height of <0.5m. A monocular depth estimation network is established using the Depth Anything V2 model to perform relative depth estimation on the images. The airborne downward-looking monocular camera has a resolution of 640×480, and the monocular depth estimation network runs in real time at 3Hz on the airborne computer 1-2.
[0073] (1) According to the above scheme, the relative depth of the environment and the relative depth gradient of the environment are estimated by using images acquired by the UAV's airborne downward-looking monocular camera, such as... Figure 3 As shown.
[0074] (2) Based on the relative depth of the environment and the relative depth gradient of the environment, combined with the distance information and positioning information obtained by the airborne laser-optical flow sensors 1-3, and the relative distance between the UAV and the unmanned vehicle estimated by the airborne downward-looking monocular camera, the true depth of the environment and the true depth gradient of the environment are recovered.
[0075] (3) Construct a topographic elevation raster map and a topographic elevation gradient raster map to represent the topographic relief, such as Figure 4 As shown in the figure. The grid resolution is 0.1m.
[0076] (4) Assess the traversal costs of autonomous vehicles in non-flat terrain environments, such as Figure 4 As shown. Among them, the weighting coefficients... Take values of 2.0, 1.0, and 0.1 respectively.
[0077] (5) Generate initial trajectory values using the A* algorithm that considers traversability costs. Based on the initial trajectory values, establish the optimization objective function and constraints, and optimize to obtain the autonomous navigation trajectory of the unmanned vehicle.
[0078] The objective function to be optimized is:
[0079] in, This is the state variable vector of the unmanned vehicle, namely its three-dimensional position and yaw angle in the ground coordinate system. The goal of trajectory optimization is to minimize the energy consumption of the autonomous vehicle's trajectory. The traversability cost is estimated by interpolation of the traversability cost of the surrounding grid.
[0080] The dynamic constraints of the trajectory are velocity constraints and yaw rate constraints:
[0081] in For the speed of driverless cars, The yaw rate of the autonomous vehicle. For maximum speed, This represents the maximum yaw rate.
[0082] The maximum traversability constraint of the trajectory is:
[0083] The maximum allowable traversability cost is used. By using the maximum traversability constraint, the trajectory can avoid obstacles such as potholes and pillars.
[0084] (6) The UAV transmits the generated autonomous navigation trajectory 5 to the unmanned vehicle via the vehicle-to-vehicle communication link. The unmanned vehicle executes the trajectory tracking task according to the instructions to achieve autonomous navigation. Among them, data transmission 3 is used as the communication link, and the unmanned vehicle tracks the autonomous navigation trajectory through the model prediction controller.
[0085] In this invention, unless otherwise explicitly specified and limited, the terms "installation," "connection," "linking," and "fixing," etc., should be interpreted broadly. For example, they can refer to a fixed connection, a detachable connection, or an integral part; they can refer to a mechanical connection or an electrical connection; they can refer to a direct connection or an indirect connection through an intermediate medium; they can refer to the internal communication of two components or the interaction between two components. Those skilled in the art can understand the specific meaning of the above terms in this invention according to the specific circumstances.
[0086] In this invention, unless otherwise explicitly specified and limited, "above" or "below" the second feature can include direct contact between the first and second features, or contact between the first and second features through another feature between them. Furthermore, "above," "over," and "on top" of the second feature includes the first feature directly above or diagonally above the second feature, or simply indicates that the first feature is at a higher horizontal level than the second feature. "Below," "below," and "under" the second feature includes the first feature directly below or diagonally below the second feature, or simply indicates that the first feature is at a lower horizontal level than the second feature.
[0087] In this invention, the terms "first," "second," "third," and "fourth" are used for descriptive purposes only and should not be construed as indicating or implying relative importance. The term "multiple" refers to two or more unless otherwise expressly defined.
[0088] The above description is merely a preferred embodiment of the present invention and is not intended to limit the invention. Various modifications and variations can be made to the present invention by those skilled in the art. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the scope of protection of the present invention.
Claims
1. A heterogeneous cooperative navigation method for non-flat terrain based on monocular depth estimation, characterized in that, Includes the following steps: Step S1: The UAV observes the unmanned vehicle in the target's field of view, acquires images using an airborne downward-looking monocular camera, and estimates the relative depth of the environment. relative depth gradient with respect to the environment , , These are the pixel coordinates on the image captured by the airborne downward-looking monocular camera; Step S2: Based on the relative depth and relative depth gradient of the environment, combined with the altitude information obtained by the airborne laser-optical flow sensor and the global positioning information of the UAV, and the relative distance between the UAV and the unmanned vehicle estimated by the airborne downward-looking monocular camera, the true depth of the environment is recovered. With the actual depth gradient of the environment , ; Step S3: Based on the actual environmental depth, the actual environmental depth gradient, the relative pose estimation results of the vehicle-mounted QR code by the airborne downward-looking monocular camera, and the global positioning information of the UAV, construct a terrain elevation raster map and a terrain elevation gradient raster map to characterize the terrain undulation. Step S4: Based on the terrain elevation grid map and terrain elevation gradient grid map, evaluate the traversal cost of the unmanned vehicle in non-flat terrain environments; Step S5: Based on the traversability cost, and combining the current pose of the unmanned vehicle with the target pose, plan and generate the autonomous navigation trajectory of the unmanned vehicle. Step S6: The unmanned vehicle travels according to the autonomous navigation trajectory to achieve autonomous navigation.
2. The heterogeneous cooperative navigation method for non-flat terrain according to claim 1, characterized in that, In step S2, the true depth of the environment is restored. With the actual depth gradient of the environment , Use the following expression: in, For the drone to estimate its own altitude, The relative distance between the drone and the unmanned vehicle. These are the pixel coordinates of the ranging point of the airborne laser-optical flow sensor on the image acquired by the airborne downward-looking monocular camera. The pixel-relative depth of the ranging point of the airborne laser-optical flow sensor. The pixel coordinates of the images captured by the onboard downward-looking monocular camera of the autonomous vehicle. This represents the relative depth of pixels for the autonomous vehicle.
3. The heterogeneous cooperative navigation method for non-flat terrain according to claim 2, characterized in that, Step S3 specifically includes: Step S3-1: Based on the actual depth of the environment, obtain the pixel coordinates on the image acquired by the airborne downward-looking monocular camera. Corresponding camera coordinate system coordinates ; Step S3-2: Set the coordinates in the camera coordinate system Convert to ground coordinate system coordinates ; Step S3-3: Rasterize the target field of view. Based on the ground coordinate system coordinates and the actual depth gradient of the environment, calculate the terrain elevation and terrain elevation gradient in each grid, and construct a terrain elevation raster map and a terrain elevation gradient raster map to represent the terrain undulation.
4. The heterogeneous cooperative navigation method for non-flat terrain according to claim 3, characterized in that, In step S3-1 The expression is: in, The focal length of the airborne downward-looking monocular camera. The vertical focal length of the airborne downward-looking monocular camera. The coordinates are the principal point coordinates of the airborne downward-looking monocular camera.
5. The heterogeneous cooperative navigation method for non-flat terrain according to claim 4, characterized in that, The ground coordinate system coordinates in step S3-2 The expression is: in, This is the transformation matrix from the body coordinate system to the ground coordinate system. This is the transformation matrix from the camera coordinate system to the body coordinate system. The coordinates of the origin of the camera coordinate system in the ground coordinate system.
6. The heterogeneous cooperative navigation method for non-flat terrain according to claim 1, characterized in that, In step S3-3, the terrain elevation and terrain elevation gradient within each grid cell are calculated by accumulating and averaging.
7. The heterogeneous cooperative navigation method for non-flat terrain according to claim 1, characterized in that, The expression for the traversability cost in step S4 is: in, raster coordinates The cost of traversability at that location Costs related to the flatness of the surrounding terrain. For slope cost, Cost based on height difference All are weighting coefficients.
8. The heterogeneous cooperative navigation method for non-flat terrain according to claim 1, characterized in that, Step S5 specifically includes: Step S5-1: Based on the current pose and target pose of the autonomous vehicle, by considering the cost of traversability, A... * The algorithm generates initial trajectory values; Step S5-2: Based on the initial trajectory value, establish the optimization objective function and constraints to optimize and obtain the autonomous navigation trajectory of the unmanned vehicle.
9. The heterogeneous cooperative navigation method for non-flat terrain according to claim 8, characterized in that, The objective function for optimization in step S5-2 is: in, To optimize the objective function, For the autonomous vehicle's state variable vector, For time, For the duration of the trajectory, Let be the order of the differential equation. These are the weighting coefficients. To assess the traversal cost of autonomous vehicles crossing rugged terrain.
10. The heterogeneous cooperative navigation method for non-flat terrain according to claim 1, characterized in that, Step 6 specifically includes: the UAV transmits the generated autonomous navigation trajectory to the unmanned vehicle through the vehicle-to-vehicle communication link, and the unmanned vehicle runs a predictive controller on the on-board computer to track the autonomous navigation trajectory and achieve autonomous navigation.