Autonomous positioning and obstacle avoidance method for fixed-wing unmanned aerial vehicle flying close to ground

Abstract

The invention discloses an autonomous positioning and obstacle avoidance method for ground-approaching flight of a fixed-wing unmanned aerial vehicle, and the method achieves the high-precision positioning through constructing a stereogram and carrying out the feature point matching with a pre-stored base map, and achieves the safe and efficient obstacle avoidance through employing a dynamic constraint hierarchical progressive trajectory planning method. The problems of navigation and obstacle avoidance under the conditions of limited GPS signals and dynamic change of obstacles in ground-approaching flight are solved, and the survivability and task execution efficiency of the unmanned aerial vehicle in a complex environment are improved.

Description

Technical Field

[0001] The present invention belongs to the technical field of unmanned aerial vehicles (UAVs), and in particular relates to an autonomous positioning and obstacle avoidance method for a fixed-wing UAV flying close to the ground. Background Art

[0002] Fixed-wing drones often employ near-ground flight technology to obtain high-precision data when performing scientific research tasks such as environmental monitoring, geological exploration, and disaster assessment. Near-ground flight involves flying the drone at a relatively low altitude (typically 10-20 meters), close to terrain and features. This allows for higher-resolution imagery and improved recognition of terrain features. This flight method also avoids high-altitude winds, reduces energy consumption, and extends flight time. It is particularly suitable for inspections of long-distance linear projects (such as transmission lines, pipelines, and railways) and for detailed mapping of areas with dense terrain features.

[0003] However, near-Earth flight also presents two key technical challenges:

[0004] 1. Reduced positioning accuracy: In low-altitude flight environments, terrain obstruction, tree canopy, and canyon effects cause severe attenuation or interruption of GPS signals, making it impossible to obtain continuous and reliable positioning information. Traditional GPS-based navigation and positioning methods are particularly vulnerable to significant degradation or even complete failure in these environments, with positioning errors potentially exceeding 10 meters.

[0005] 2. Complex obstacle avoidance: During near-ground flight, drones face a variety of natural and man-made obstacles, such as mountains, trees, power lines, and communication towers. These obstacles require real-time obstacle avoidance decisions at high speeds of 15-30 m / s, placing extremely high demands on the real-time and safety of obstacle avoidance planning.

[0006] Currently, there are mainly the following methods for positioning and obstacle avoidance in near-ground flight:

[0007] 1. Inertial navigation combined with terrain matching: Terrain matching positioning is performed using an inertial navigation system (INS) in conjunction with a digital elevation map, such as terrain-assisted navigation. While this method can provide positioning capabilities in the absence of GPS signals, its accuracy is limited by the accumulated errors of the inertial navigation system, and matching accuracy decreases in areas with unclear terrain features.

[0008] 2. Visual odometry: This method estimates camera motion and infers the drone's position by tracking feature points in a continuous image sequence. This method is significantly affected by lighting conditions and is prone to failure in environments with drastic lighting changes (such as shadowed areas and during sunrise and sunset) or lacking features (such as water or snow). The average tracking loss rate is as high as 30%.

[0009] 3. Sampling-based trajectory planning: Algorithms such as the rapidly expanding random tree (RRT) and the probabilistic roadmap method (PRM) construct collision-free paths through random sampling. These methods suffer from low planning efficiency. In complex environments, a single planning calculation often exceeds 200ms, making it difficult to meet the required 50ms response time at a flight speed of 25m / s. Furthermore, the resulting trajectory often lacks smoothness, making it difficult for the control system to track.

[0010] 4. Optimization-based trajectory planning: Methods such as model predictive control (MPC) and gradient-based optimization transform the obstacle avoidance problem into a constrained optimization problem. These methods can generate smooth trajectories, but they suffer from slow convergence and susceptibility to local optimality in complex dynamic environments. Furthermore, their performance is highly sensitive to obstacle density.

[0011] The main reasons why existing methods are difficult to solve the positioning and obstacle avoidance problems in near-ground flight include:

[0012] 1. Insufficient multi-sensor fusion and information coupling: Existing methods are often independent of each other and lack the effective fusion of multi-source heterogeneous sensor data such as lidar, millimeter-wave radar, and stereo cameras. They are unable to fully utilize the complementary advantages and redundant characteristics of different perception methods.

[0013] 2. Inadequate handling of dynamic and environmental constraints: Traditional trajectory planning methods fail to effectively consider the coupling relationship between the fixed-wing aircraft's dynamic characteristics, such as the minimum turning radius and climb rate, and environmental constraints, resulting in difficult execution of planned trajectories or insufficient safety margins.

[0014] 3. The conflict between computational complexity and real-time performance: To meet the real-time obstacle avoidance requirements in high-speed flight environments, existing algorithms either suffer from high computational complexity (e.g., the O(n²) computational complexity of dynamic programming algorithms), resulting in delayed responses, or are overly simplified (e.g., the artificial potential field method), leading to poor planning quality and local minima.

[0015] 4. Multi-dimensional optimization conflict: There are conflicts between multi-dimensional optimization goals such as safety, smoothness, and energy consumption optimization. Existing methods lack an effective multi-objective coordination mechanism, making it difficult to achieve dynamic trade-offs between different goals. Summary of the Invention

[0016] In order to overcome the shortcomings of the existing technology, the present invention provides a method for autonomous positioning and obstacle avoidance of fixed-wing UAVs flying close to the ground. High-precision positioning is achieved by constructing a stereo map and matching feature points with a pre-stored base map, and a hierarchical progressive trajectory planning method with dynamic constraints is adopted to achieve safe and efficient obstacle avoidance. This solves the problems of navigation and obstacle avoidance in environments with limited GPS signals and dynamically changing obstacles during close-to-the-ground flight, thereby improving the UAV's survivability and mission execution efficiency in complex environments.

[0017] The technical solutions adopted by the present invention to solve the technical problems are as follows:

[0018] Step 1: Multi-source sensor data acquisition and preprocessing;

[0019] Step 2: Autonomous positioning based on stereo image matching;

[0020] Step 3: Trajectory planning framework based on hierarchical progression;

[0021] Step 4: Sequential multi-objective trajectory optimization.

[0022] Preferably, the step 1 is specifically:

[0023] Step 1-1: Multi-source data collection, simultaneously acquiring the following sensor data:

[0024] (1) Visible light camera image sequence , acquisition frequency 30Hz; They represent the 1st, 2nd, …, and nth frames of visible light camera images, respectively, and n represents the total number of collected data frames;

[0025] (2) Infrared camera image sequence , acquisition frequency 20Hz; Represent the 1st frame, 2nd frame, ..., nth frame of infrared camera images respectively;

[0026] (3) LiDAR point cloud data , acquisition frequency 10Hz; Represent the point cloud data of the 1st frame, the 2nd frame, ..., the nth frame respectively;

[0027] (4) Inertial measurement unit data , acquisition frequency 200Hz; Respectively represent the first, second, ..., nth group of inertial measurement unit data;

[0028] (5) Barometric altimeter data , acquisition frequency 50Hz; Respectively represent the 1st, 2nd, ..., nth barometric altimeter data;

[0029] (6) GPS positioning data , acquisition frequency 5Hz; Respectively represent the 1st, 2nd, ..., nth GPS positioning data;

[0030] Step 1-2: Time synchronization;

[0031] The sensor data is synchronized based on the timestamp, and the linear interpolation method is used to unify the data with different sampling frequencies. After the time synchronization process, the originally discrete data sequence is converted into a continuous time function:

[0032]

[0033] in To unify the system timestamp, the synchronization frequency is set to 20Hz; represents the synchronous data combination of all sensors at time t;

[0034] Steps 1-3: Data preprocessing;

[0035] Preprocess the synchronized sensor data, including:

[0036] (1) Image distortion correction: Zhang calibration method is used to correct visible light camera images and infrared camera images;

[0037] (2) Image enhancement: Improve image contrast through adaptive histogram equalization (CLAHE);

[0038] (3) Point cloud filtering: using voxel filtering and statistical outlier filtering methods to reduce noise and downsample;

[0039] (4) Inertial data correction: Kalman filtering is used to perform zero drift correction on inertial data;

[0040] Step 1-4: Sensor external parameter calibration;

[0041] Establish the coordinate transformation matrix between different sensors:

[0042] (1) Camera and lidar calibration matrix: ;

[0043] (2) Inertial navigation system and camera calibration matrix: ;

[0044] (3) Inertial navigation system and lidar calibration matrix: .

[0045] Preferably, the step 2 is specifically as follows:

[0046] Step 2-1: Stereograph construction;

[0047] Build a stereographic representation of the current scene based on sensor data:

[0048] (1) Feature extraction: Extract ORB (Oriented FAST and Rotated BRIEF) feature points from visible light and infrared images respectively and ; represents the set of feature points extracted from the visible light image, represents a set of feature points extracted from infrared images;

[0049] (2) Point cloud projection: Project the lidar point cloud onto the image plane to generate a depth-enhanced image ;

[0050] (3) Stereo image fusion: Fusing feature points and depth information to generate a stereo image representation :

[0051] ;

[0052] Among them, the fusion function A weighted combination method is used, and the weights of different modes are dynamically adjusted according to environmental conditions;

[0053] Step 2-2: Feature point extraction and description;

[0054] (1) Extract scale-invariant feature transform (SIFT) feature points from the current stereo image and generate a feature point set , express feature points;

[0055] (2) For each feature point Calculate 128-dimensional SIFT descriptor ;

[0056] (3) Combine depth information to construct enhanced descriptors for each feature point ,in Feature points The corresponding depth value;

[0057] Step 2-3: Topographic base map matching;

[0058] (1) From the pre-stored topographic base map Search for matching regions in ,The initial search range is determined based on the estimated position of inertial navigation;

[0059] (2) Using the fast approximate nearest neighbor FLANN algorithm Search and feature point sets Reference feature point set for feature point matching ;

[0060] (3) Calculate matching pairs Similarity score: ; Represents the reference feature points in the topographic base map, Indicates the SIFT features corresponding to the reference feature points, Represents the standard deviation parameter of the Gaussian similarity function, which controls the sensitivity of the similarity calculation;

[0061] (4) Retain matching pairs that meet the conditions through ratio test: ; in is the ratio threshold; is the similarity score between the current feature point and the reference feature point;

[0062] Step 2-4: Pose estimation and refinement:

[0063] (1) Estimate camera pose transformation by combining RANSAC algorithm with P3P method ;

[0064] (2) Use the retained inlier matching pairs to perform Bundle Adjustment optimization to minimize the reprojection error:

[0065]

[0066] in are the coordinates of the three-dimensional feature points, is the image projection coordinate corresponding to the three-dimensional feature point, represents the camera projection function, which projects a 3D point onto the image plane; Represents the reprojection error, which measures the accuracy of pose estimation;

[0067] (3) Combine inertial measurement unit data for pose fusion to obtain a smooth pose estimate :

[0068]

[0069] in is the pose obtained by map matching, is the updated pose of inertial navigation, is the fusion coefficient;

[0070] Step 2-5: Dynamic map update;

[0071] (1) Update the local map online based on newly acquired sensor data and pose estimation;

[0072] (2) Using the sliding window strategy, maintain a Local map of frame data;

[0073] (3) For newly observed feature points, update their positions and descriptors in the map to improve the accuracy of subsequent matching.

[0074] Preferably, the step 3 is specifically:

[0075] Step 3-1: Obstacle perception and representation:

[0076] (1) Fuse multi-source sensor data to detect obstacles ahead and generate a unified obstacle representation: ; Represents the multi-sensor fusion detection function;

[0077] (2) For each obstacle Extract position, size, velocity vector, obstacle type: , Respectively represent the position coordinates, size, velocity vector, and obstacle type;

[0078] (3) According to the characteristics of the obstacle, the obstacle is classified into static obstacles and dynamic obstacles ;

[0079] Step 3-2: constrain topology graph construction;

[0080] (1) Identify the set of constraints in trajectory optimization based on the flight mission and environment characteristics ; All indicate flight restrictions;

[0081] (2) Analyze the dependencies between constraints and construct a constraint topology graph ; Represents the dependency edge set between constraints;

[0082] (3) Calculate the weight of each constraint node :

[0083]

[0084] in, Indicates urgency, Indicates feasibility impact, Indicates the dependency on other constraints, 、 、 All are weight coefficients;

[0085] Step 3-3: Hierarchical optimization sequence generation;

[0086] (1) Based on the constraint topology graph, use the topological sorting algorithm to generate the optimized sequence ; Respectively represent the constraint processing order sorted by dependency, is the first constraint to be addressed. is the last constraint to be processed;

[0087] (2) According to the sorting results, the constraints are divided into basic constraints , safety constraints and performance constraints ;

[0088] (3) Design a multi-stage optimization strategy to handle different levels of constraints in turn:

[0089] Phase 1: Dealing with basic constraints : in express The acceleration vector at the moment, Indicates the maximum allowable acceleration;

[0090] Phase 2: Introducing safety constraints : in Indicates the distance between the drone and the obstacle. Indicates the safety distance threshold; Indicates the flight trajectory of the drone;

[0091] Phase 3: Considering performance constraints : , in Indicates the target location, represents the desired terminal velocity, represents the position of the terminal at time T, represents the speed at the terminal time T;

[0092] Step 3-4: Construction of safety corridor;

[0093] (1) Based on the current environment perception results and basic constraints , build an initial safety corridor;

[0094] Use convex polyhedron to represent the safe area: , Represents the three-dimensional space position coordinates, represents the normal vector matrix of the i-th constraint surface, Represents the distance parameter of the i-th constraint surface;

[0095] Each inequality represents a half-plane constraint, which together define the safe flight area;

[0096] (2) Based on the obstacle distribution, decompose the safety corridor into continuous convex polyhedron segments , They represent the first, second, ..., and first safety corridor areas respectively;

[0097] (3) For dynamic obstacles, predict their future motion trajectories and construct time-varying safety corridors;

[0098] Steps 3-5: parameterized trajectory representation;

[0099] (1) Using B-spline curve to represent the flight trajectory: in, is the i-th control point, is the d-order B-spline basis function, is the number of control points;

[0100] (2) The trajectory is parameterized as a set of control points: ;

[0101] (3) Initial control point Obtained by uniform sampling in the safe corridor: , Represent the coordinates of the 0th, 1st, ..., numth initial control points respectively.

[0102] Preferably, the step 4 is specifically as follows:

[0103] Step 4-1: Dynamic relaxation strategy design:

[0104] (1) Design the relaxation function of the dynamic constraint and dynamically adjust the constraint strength according to the optimization stage:

[0105]

[0106] in, is the original constraint, is the relaxation coefficient, is the current iteration number, Indicates the maximum number of iterations; represents the control input variable; represents the relaxed constraint function;

[0107] (2) Design relaxation strategies for different constraints based on the constraint hierarchy:

[0108] Safety constraints are obstacle avoidance: , indicating no relaxation;

[0109] Dynamic constraints are speed / acceleration limits: → , indicating a gradual tightening from initial relaxation;

[0110] Smoothness constraints: → , indicating a transition from initial relaxation to partial tightening;

[0111] Step 4-2: Construction of multi-objective cost function;

[0112] Design synthesis cost function , which includes multiple optimization objectives:

[0113]

[0114] in:

[0115] Obstacle avoidance safety cost function: , Indicates the time range, Indicates the number of obstacles, represents the obstacle avoidance coefficient, represents the distance function, represents the position of the i-th obstacle at time t;

[0116] Trajectory smoothness cost function: , represents the acceleration vector at time t+1;

[0117] Energy consumption optimality cost function: , represents the control input vector, represents the time step;

[0118] Terminal constraint cost function: , represents the position of the terminal at time T, Indicates the target location, represents the position weight coefficient, represents the speed at the terminal time T, Indicates the target speed;

[0119] 、 、 、 is the weight of each cost function; Represents time parameters;

[0120] Step 4-3: Progressive optimization algorithm;

[0121] Based on the hierarchical optimization sequence, a three-stage progressive optimization strategy is designed:

[0122] (1) Global planning stage, accounting for 10% of the total number of iterations:

[0123] (a) Freeze All constraints outside

[0124] (b) Using a large step size search to determine the overall trajectory shape;

[0125] (c) Use the graph search-based A* algorithm for preliminary path planning:

[0126] ;

[0127] (2) Security optimization phase, accounting for 40% of the total iterations;

[0128] (a) Activate safety constraints ;

[0129] (b) Minimize the collision cost using the covariant Hamiltonian optimized motion planning CHOMP algorithm:

[0130] ;

[0131] (c) Security weight gradually increase;

[0132] (3) Performance optimization phase, accounting for 50% of the total iterations;

[0133] (a) Activate performance constraints ;

[0134] (b) Solve using the continuous-time optimal control method:

[0135] ;

[0136] (c) Gradient projection method is used to ensure constraint satisfaction:

[0137]

[0138] in is the adaptive step size, To project the trajectory into the safe corridor; represents the gradient of the cost function at the kth iteration;

[0139] Step 4-4: Intermediate trajectory transition and adaptation;

[0140] (1) Constructing an intermediate transition trajectory set , as a bridge between theoretical models and practical scenarios; All represent intermediate transition trajectories; represents the total number of intermediate trajectories;

[0141] (2) Each intermediate trajectory Corresponding to different flight conditions;

[0142] (3) Based on the current flight state, the most similar intermediate trajectory is selected as the optimization starting point;

[0143] (4) Motion similarity evaluation function:

[0144]

[0145] in 、 、 is the weight coefficient; Represent two trajectories to be compared, They represent the position vector corresponding to the trajectory, They represent the velocity vector corresponding to the trajectory, They represent the acceleration vectors corresponding to the trajectories respectively;

[0146] Step 4-5: trajectory mapping and closed-loop correction;

[0147] (1) Mapping the optimized trajectory to the actual control instructions: ; represents the ideal trajectory after optimization, Represents the control instruction mapping function, Represents the current flight state vector;

[0148] (2) Build a closed-loop correction mechanism to adjust the trajectory in real time: in and are position error and velocity error respectively, and are all feedback gain matrices, represents the trajectory after closed-loop correction;

[0149] (3) Using the sliding time window strategy, every Replan local trajectories in seconds.

[0150] Preferably, the ratio threshold ; Fusion coefficient ; ; , , ; , , ; .

[0151] Preferably, the initial weight value of the cost function is set to: , , , .

[0152] The beneficial effects of the present invention are as follows:

[0153] The simulation and actual flight verification results consistently show that the autonomous positioning and obstacle avoidance method for close-to-the-ground flight that integrates stereo image matching and hierarchical progressive trajectory planning proposed in the present invention has the characteristics of high precision, high efficiency and high safety. It can effectively solve the navigation and obstacle avoidance problems in close-to-the-ground flight under the conditions of limited GPS signals and dynamically changing obstacles, and provides reliable technical support for UAV low-altitude flight missions. DETAILED DESCRIPTION

[0154] The present invention is further described below with reference to the embodiments.

[0155] The present invention proposes a method for autonomous positioning and obstacle avoidance in near-ground flight that integrates stereo image matching with hierarchical progressive trajectory planning. It achieves high-precision positioning through multimodal feature matching and significantly improves the efficiency and robustness of obstacle avoidance planning using a hierarchical progressive strategy. The method is particularly suitable for application scenarios such as scientific investigations, ecological monitoring, and fine mapping in areas with complex terrain.

[0156] The present invention proposes an autonomous positioning and obstacle avoidance method for fixed-wing unmanned aerial vehicles (UAVs) flying close to the ground. High-precision positioning is achieved by constructing a stereo image and matching feature points with a pre-stored base map. A hierarchical progressive trajectory planning method with dynamic constraints is used to achieve safe and efficient obstacle avoidance. This method solves the difficulties of navigation and obstacle avoidance in environments with limited GPS signals and dynamically changing obstacles during close-to-the-ground flight, thereby improving the UAV's survivability and mission execution efficiency in complex environments.

[0157] Step 1: Multi-source sensor data acquisition and preprocessing;

[0158] Step 1-1: Multi-source data collection, simultaneously acquiring the following sensor data:

[0159] (1) Visible light camera image sequence , acquisition frequency 30Hz;

[0160] (2) Infrared camera image sequence , acquisition frequency 20Hz;

[0161] (3) LiDAR point cloud data , acquisition frequency 10Hz;

[0162] (4) Inertial measurement unit data , acquisition frequency 200Hz;

[0163] (5) Barometric altimeter data , acquisition frequency 50Hz;

[0164] (6) GPS positioning data , acquisition frequency 5Hz;

[0165] Step 1-2: Time synchronization;

[0166] The sensor data is synchronized based on the timestamp, and the linear interpolation method is used to unify the data with different sampling frequencies. After the time synchronization process, the originally discrete data sequence is converted into a continuous time function:

[0167]

[0168] Steps 1-3: Data preprocessing;

[0169] Preprocess the synchronized sensor data, including:

[0170] (1) Image distortion correction: Zhang calibration method is used to correct visible light camera images and infrared camera images;

[0171] (2) Image enhancement: Improve image contrast through adaptive histogram equalization (CLAHE);

[0172] (3) Point cloud filtering: using voxel filtering and statistical outlier filtering methods to reduce noise and downsample;

[0173] (4) Inertial data correction: Kalman filtering is used to perform zero drift correction on inertial data;

[0174] Step 1-4: Sensor external parameter calibration;

[0175] Establish the coordinate transformation matrix between different sensors :

[0176] (1) Camera and lidar calibration matrix: ;

[0177] (2) Inertial navigation system and camera calibration matrix: ;

[0178] (3) Inertial navigation system and lidar calibration matrix: .

[0179] These coordinate transformation matrices are used for subsequent spatial alignment and fusion of multi-sensor data.

[0180] Step 2: Autonomous positioning based on stereo image matching;

[0181] Step 2-1: Stereograph construction;

[0182] Build a stereographic representation of the current scene based on sensor data:

[0183] (1) Feature extraction: Extract ORB feature points from visible light and infrared images respectively and ORB (Oriented FAST and Rotated BRIEF) is a feature extraction algorithm that combines the FAST corner detector and the BRIEF descriptor, and is rotation-invariant and scale-invariant.

[0184] (2) Point cloud projection: Project the lidar point cloud onto the image plane to generate a depth-enhanced image ;

[0185] (3) Stereo image fusion: Fusing feature points and depth information to generate a stereo image representation :

[0186] ;

[0187] Among them, the fusion function A weighted combination approach is used, where the weights of different modalities are dynamically adjusted according to environmental conditions (such as lighting and visibility);

[0188] Step 2-2: Feature point extraction and description;

[0189] (1) Extract SIFT feature points from the current stereo image and generate a feature point set , express feature points; SIFT is the abbreviation of Scale-Invariant Feature Transform, which is a classic image feature extraction algorithm.

[0190] (2) For each feature point Calculate 128-dimensional SIFT descriptor ;

[0191] (3) Combine depth information to construct enhanced descriptors for each feature point ,in Feature points The corresponding depth value;

[0192] Step 2-3: Topographic base map matching;

[0193] (1) From the pre-stored topographic base map Search for matching regions in ,The initial search range is determined based on the estimated position of inertial navigation;

[0194] (2) Using the fast approximate nearest neighbor FLANN algorithm Search and feature point sets Reference feature point set for feature point matching ;

[0195] (3) Calculate matching pairs Similarity score: ;

[0196] (4) Filter out low-quality matches through ratio testing and retain matching pairs that meet the conditions: ; in is the ratio threshold;

[0197] Step 2-4: Pose estimation and refinement:

[0198] (1) Estimate camera pose transformation by combining RANSAC algorithm with P3P method ;

[0199] (2) Use the retained inlier matching pairs to perform Bundle Adjustment optimization to minimize the reprojection error:

[0200]

[0201] (3) Combine inertial measurement unit data for pose fusion to obtain a smooth pose estimate :

[0202]

[0203] in is the pose obtained by map matching, is the updated pose of inertial navigation, is the fusion coefficient;

[0204] Step 2-5: Dynamic map update;

[0205] (1) Update the local map online based on newly acquired sensor data and pose estimation;

[0206] (2) Using the sliding window strategy, maintain a Local map of frame data;

[0207] (3) For newly observed feature points, update their positions and descriptors in the map to improve subsequent matching accuracy;

[0208] Step 3: Trajectory planning framework based on hierarchical progression;

[0209] Step 3-1: Obstacle perception and representation:

[0210] (1) Fuse multi-source sensor data to detect obstacles ahead and generate a unified obstacle representation: ;

[0211] (2) For each obstacle Extract position, size, and speed attributes: ;

[0212] (3) According to the characteristics of the obstacle, the obstacle is classified into static obstacles and dynamic obstacles ;

[0213] Step 3-2: constrain topology graph construction;

[0214] (1) Identify the set of constraints in trajectory optimization based on the flight mission and environment characteristics ;

[0215] (2) Analyze the dependencies between constraints and construct a constraint topology graph ;

[0216] (3) Calculate the weight of each constraint node :

[0217]

[0218] in, Indicates urgency, Indicates feasibility impact, Indicates the dependency on other constraints, , , All are weight coefficients;

[0219] Step 3-3: Hierarchical optimization sequence generation;

[0220] (1) Based on the constraint topology graph, use the standard topological sorting algorithm to generate the optimized sequence ;

[0221] (2) According to the sorting results, the constraints are divided into basic constraints , safety constraints and performance constraints ;

[0222] (3) Design a multi-stage optimization strategy to handle different levels of constraints in turn:

[0223] Phase 1: Dealing with basic constraints , build an initial safety corridor;

[0224] Phase 2: Introducing safety constraints , ensure that the trajectory meets the obstacle avoidance requirements;

[0225] Phase 3: Considering performance constraints , optimize the smoothness and energy consumption of the trajectory;

[0226] Step 3-4: Construction of safety corridor;

[0227] (1) Based on the current environment perception results and basic constraints , build an initial safety corridor;

[0228] Use convex polyhedron to represent the safe area: ;

[0229] Each inequality represents a half-plane constraint, which together define the safe flight area;

[0230] (2) Based on the obstacle distribution, decompose the safety corridor into continuous convex polyhedron segments ;

[0231] (3) For dynamic obstacles, predict their future motion trajectories and construct time-varying safety corridors;

[0232] Steps 3-5: parameterized trajectory representation;

[0233] (1) Using B-spline curve to represent the flight trajectory: in, is the i-th control point, is the d-order B-spline basis function, is the number of control points;

[0234] (2) The trajectory is parameterized as a set of control points: ;

[0235] (3) Initial control point Obtained by uniform sampling in the safe corridor: , Represent the coordinates of the 0th, 1st, ..., numth initial control points respectively.

[0236] Through this step, a hierarchical and progressive trajectory planning framework was constructed, constraint topology relationships were established, an optimization sequence was generated, and an initial safe corridor and parameterized trajectory representation were constructed. This framework laid the foundation for subsequent sequential trajectory optimization and provided a structured solution for addressing convergence instabilities in dynamic environments. The core innovation of this step lies in constructing a topological graph by analyzing the dependencies between constraints and generating a hierarchical optimization sequence based on this graph. This allows complex multi-constraint optimization problems to be solved progressively from simple to complex and from global to local, significantly improving planning efficiency and robustness.

[0237] Step 4: Sequential multi-objective trajectory optimization;

[0238] Step 4-1: Dynamic relaxation strategy design:

[0239] (1) Design the relaxation function of the dynamic constraint and dynamically adjust the constraint strength according to the optimization stage:

[0240]

[0241] (2) Design relaxation strategies for different constraints based on the constraint hierarchy:

[0242] Safety constraints are obstacle avoidance: , indicating no relaxation;

[0243] Dynamic constraints are speed / acceleration limits: → , indicating a gradual tightening from initial relaxation;

[0244] Smoothness constraints: → , indicating a transition from initial relaxation to partial tightening;

[0245] Step 4-2: Construction of multi-objective cost function;

[0246] Design synthesis cost function , which includes multiple optimization objectives:

[0247]

[0248] Specifically, it includes the following types of cost functions:

[0249] Obstacle avoidance safety cost function: ;

[0250] Trajectory smoothness cost function: ;

[0251] Energy consumption optimality cost function: ;

[0252] Terminal constraint cost function: ;

[0253] The initial settings of the weights of each cost function are: , , , ;

[0254] Step 4-3: Progressive optimization algorithm;

[0255] Based on the hierarchical optimization sequence, a three-stage progressive optimization strategy is designed:

[0256] (1) Global planning stage, accounting for 10% of the total number of iterations:

[0257] (a) Freeze All constraints outside

[0258] (b) Use large step size search to quickly determine the overall trajectory shape;

[0259] (c) Use the graph search-based A* algorithm for preliminary path planning;

[0260] (2) Security optimization phase, accounting for 40% of the total iterations;

[0261] (a) Activate safety constraints , with a focus on obstacle avoidance;

[0262] (b) Minimize the collision cost using the covariant Hamiltonian optimized motion planning CHOMP algorithm:

[0263]

[0264] (c) Security weight Gradually increase: ;

[0265] (3) Performance optimization phase, accounting for 50% of the total iterations;

[0266] (a) Activate performance constraints , optimize smoothness and energy consumption;

[0267] (b) Solve using the continuous-time optimal control method:

[0268]

[0269]

[0270] (c) Gradient projection method is used to ensure constraint satisfaction:

[0271]

[0272] Step 4-4: Intermediate trajectory transition and adaptation;

[0273] (1) Constructing an intermediate transition trajectory set , as a bridge between theoretical models and practical scenarios;

[0274] (2) Each intermediate trajectory Corresponding to different flight conditions (such as speed, altitude);

[0275] (3) Based on the current flight state, the closest intermediate trajectory is selected as the optimization starting point by calculating the Euclidean distance;

[0276] (4) Motion similarity evaluation function:

[0277]

[0278] in, 、 、 is the weight coefficient;

[0279] Step 4-5: trajectory mapping and closed-loop correction;

[0280] (1) Mapping the optimized trajectory to the actual control instructions: ;

[0281] (2) Build a closed-loop correction mechanism to adjust the trajectory in real time: ;

[0282] (3) Using the sliding time window strategy, every Replan local trajectories in seconds.

[0283] This step achieves sequential multi-objective trajectory optimization, resolving the trajectory planning challenge during dynamic obstacle avoidance. The core innovation lies in the introduction of a dynamic relaxation strategy and a multi-stage progressive optimization method. By relaxing secondary constraints, the key constraints are first addressed, and the optimization objectives are gradually refined, effectively balancing the real-time and optimal nature of trajectory planning. Furthermore, an intermediate trajectory transition mechanism addresses the discrepancy between theoretical models and actual environments, improving the trajectory's adaptability and robustness in actual flight.

[0284] Step 5: System verification and performance evaluation;

[0285] Step 5-1: Simulation environment verification;

[0286] (1) Construct a high-fidelity simulation environment that includes a variety of terrain features (mountains, plains, and suburban areas) and obstacle scenarios;

[0287] (2) Set up multiple test scenarios under different flight conditions (speed, altitude, and weather);

[0288] (3) Completed 1,000 hours of simulated flight, covering 112 typical scenarios and 32 edge conditions;

[0289] (4) Verify the positioning accuracy, obstacle avoidance performance, and trajectory optimization effect of the method in various scenarios;

[0290] Step 5-2: Actual flight test verification;

[0291] (1) Select a typical fixed-wing UAV platform for verification, with a flight speed range of 20-80m / s and an altitude of 10-50m;

[0292] (2) Conduct field flight tests in different terrains, including the northwest Gobi Desert, the central mountainous areas, and the eastern hilly areas;

[0293] (3) Record flight data and evaluate system performance using professional test equipment;

[0294] (4) Completed 87 flight tests with a total flight time of 128 hours to verify the effectiveness of the method in a real environment;

[0295] Step 5-3: System performance evaluation;

[0296] (1) Positioning performance: Even under GPS failure conditions, it still maintains an average positioning accuracy of 0.38m, with a drift rate as low as 0.52m / min;

[0297] (2) Obstacle avoidance: Successfully coped with various static and dynamic obstacles, with a planning success rate of 97.6% and calculation time reduced to 42ms;

[0298] (3) Flight safety: The flight envelope is well maintained, the risk of exceeding limits is significantly reduced, and the stability of close-to-ground flight is improved by 68%;

[0299] (4) Comprehensive verification results show that this method significantly improves the autonomous positioning and obstacle avoidance capabilities of fixed-wing UAVs under ground-based flight conditions.

[0300] To verify the effectiveness of this invention, a series of flight tests were conducted in various terrain environments and compared with existing mainstream methods. The test environments included mountainous, plain, and suburban terrain, with flight speeds ranging from 20 to 80 m / s and altitudes ranging from 10 to 50 m. Key evaluation metrics included positioning accuracy, obstacle avoidance success rate, planning time, trajectory smoothness, and energy consumption. The test results are shown in Table 1 below:

[0301] Table 1

[0302] Performance indicators Method of the present invention Traditional INS / Terrain Matching Obstacle avoidance planning based on RRT MPC-based trajectory optimization Positioning accuracy (RMS error / m) 0.38 1.72 - - Positioning drift when GPS signal is interrupted (m / min) 0.52 2.34 - - Obstacle avoidance planning success rate (%) 97.6 - 68.4 82.1 Dynamic obstacle avoidance success rate (%) 93.2 - 58.7 73.5 Planning calculation time (ms) 42 - 135 78 Trajectory smoothness (acceleration variance) 0.87 - 3.24 1.45 Energy consumption indicators (standardized) 0.76 - 1.32 0.94 Height stability of flight close to the ground (m) ±1.2 ±3.8 ±2.7 ±1.9 System comprehensive score 92.3 65.7 71.2 78.9

[0303] Experimental results show that the proposed method outperforms existing methods in all key metrics. In particular, positioning accuracy is improved by 78% compared to traditional INS / terrain matching methods. In obstacle avoidance planning, the success rate is 29.2% higher than that of RRT-based methods, while computational time is reduced by 68.9%. Trajectory smoothness and energy consumption are also significantly superior to other methods.

[0304] Particularly noteworthy is that even in the event of GPS signal interruption, the proposed method maintains high positioning accuracy, with a drift rate of only 22.2% of that of traditional methods. This is crucial for continuous navigation in low-lying flight environments. Furthermore, the proposed method demonstrates a significant advantage in dynamic obstacle avoidance, achieving a success rate of 93.2%, 19.7 percentage points higher than the closest MPC-based method.

[0305] The comprehensive scoring results show that the method of the present invention has significant comprehensive advantages in the application of autonomous positioning and obstacle avoidance in close-to-ground flight, and provides reliable technical support for the safe and efficient flight of UAVs in complex environments.

[0306] The innovative features of the present invention are as follows:

[0307] 1. Multimodal Stereo Image Matching and Positioning Technology: This technology constructs a stereo image representation by fusing visible light, infrared imagery, and lidar data. Combined with SIFT feature point extraction and enhanced descriptor technology, it achieves high-precision matching with a pre-stored terrain base map. This addresses the issue of limited signals associated with traditional GPS during close-to-the-ground flight, improving positioning accuracy by 78%. This allows drones to maintain centimeter-level positioning accuracy in complex environments where GPS signals are weak or interrupted.

[0308] 2. Hierarchical progressive trajectory planning framework: This invention decomposes complex multi-constraint optimization problems into three levels: basic constraints, safety constraints, and performance constraints by constructing a constraint topology graph and a hierarchical optimization sequence. It implements a progressive optimization strategy from "simple to complex, from global to local", and solves the problems of unstable convergence and low computational efficiency of traditional methods in complex dynamic environments. The obstacle avoidance planning time is reduced by 65%, significantly improving real-time and robustness.

[0309] 3. Dynamic relaxation strategy and intermediate trajectory transition mechanism: This paper designs a dynamic relaxation strategy to dynamically adjust the constraint strength according to the optimization stage. At the same time, it introduces an intermediate trajectory transition mechanism as a bridge between the theoretical model and the actual environment. This solves the discrepancy problem between the theoretical trajectory and the actual execution in traditional methods, improves the executableness of the planned trajectory by 43%, and significantly reduces the trajectory tracking deviation caused by model errors.

[0310] 4. Sequential Multi-Objective Trajectory Optimization Algorithm: This invention proposes a three-stage progressive optimization strategy, from global planning to safety optimization to performance optimization. Different algorithms are used to target the optimization priorities at different stages. At the same time, a multi-objective cost function is designed that comprehensively considers obstacle avoidance safety, trajectory smoothness, and energy consumption optimization. This solves the problem that traditional methods are difficult to balance multi-dimensional optimization objectives. The obstacle avoidance success rate is improved by 32% and the trajectory smoothness is improved by 40%.

[0311] 5. System Verification and Performance Evaluation: Through large-scale simulation and actual flight tests, the present invention verified the effectiveness of the method in a variety of complex terrain environments, established a complete performance evaluation system, and proved that the system can still maintain high-precision positioning and safe obstacle avoidance capabilities under GPS failure conditions, providing reliable guarantees for autonomous navigation and control during close-to-the-ground flight, and significantly improving the survivability and mission execution efficiency of drones under extreme conditions.

Claims

1. A method for autonomous positioning and obstacle avoidance of a fixed-wing UAV flying close to the ground, characterized in that: The steps include: Step 1: Multi-source sensor data acquisition and preprocessing; Step 2: Autonomous positioning based on stereo image matching; Step 3: Trajectory planning framework based on hierarchical progression; Step 4: Sequential multi-objective trajectory optimization.

2. The method for autonomous positioning and obstacle avoidance of a fixed-wing UAV flying close to the ground according to claim 1, characterized in that: The step 1 is specifically as follows: Step 1-1: Multi-source data collection, simultaneously acquiring the following sensor data: (1) Visible light camera image sequence , acquisition frequency 30Hz; They represent the 1st, 2nd, …, and nth frames of visible light camera images, respectively, and n represents the total number of collected data frames; (2) Infrared camera image sequence , acquisition frequency 20Hz; Represent the 1st frame, 2nd frame, ..., nth frame of infrared camera images respectively; (3) LiDAR point cloud data , acquisition frequency 10Hz; Represent the point cloud data of the 1st frame, the 2nd frame, ..., the nth frame respectively; (4) Inertial measurement unit data , acquisition frequency 200Hz; Respectively represent the first, second, ..., nth group of inertial measurement unit data; (5) Barometric altimeter data , acquisition frequency 50Hz; Respectively represent the 1st, 2nd, ..., nth barometric altimeter data; (6) GPS positioning data , acquisition frequency 5Hz; Respectively represent the 1st, 2nd, ..., nth GPS positioning data; Step 1-2: Time synchronization; The sensor data is synchronized based on the timestamp, and the linear interpolation method is used to unify the data with different sampling frequencies. After the time synchronization process, the originally discrete data sequence is converted into a continuous time function: ； in To unify the system timestamp, the synchronization frequency is set to 20Hz; represents the synchronous data combination of all sensors at time t; Steps 1-3: Data preprocessing; Preprocess the synchronized sensor data, including: (1) Image distortion correction: Zhang calibration method is used to correct visible light camera images and infrared camera images; (2) Image enhancement: Improve image contrast through adaptive histogram equalization (CLAHE); (3) Point cloud filtering: using voxel filtering and statistical outlier filtering methods to reduce noise and downsample; (4) Inertial data correction: Kalman filtering is used to perform zero drift correction on inertial data; Step 1-4: Sensor external parameter calibration; Establish the coordinate transformation matrix between different sensors: (1) Camera and lidar calibration matrix: ; (2) Inertial navigation system and camera calibration matrix: ; (3) Inertial navigation system and lidar calibration matrix: .

3. The method for autonomous positioning and obstacle avoidance of a fixed-wing UAV flying close to the ground according to claim 2, characterized in that: The step 2 is specifically as follows: Step 2-1: Stereograph construction; Build a stereographic representation of the current scene based on sensor data: (1) Feature extraction: Extract ORB (Oriented FAST and Rotated BRIEF) feature points from visible light and infrared images respectively and ; represents the set of feature points extracted from the visible light image, represents a set of feature points extracted from infrared images; (2) Point cloud projection: Project the lidar point cloud onto the image plane to generate a depth-enhanced image ; (3) Stereo image fusion: Fusing feature points and depth information to generate a stereo image representation : ； Among them, the fusion function Using a weighted combination approach, the weights of different modalities are dynamically adjusted according to environmental conditions; Step 2-2: Feature point extraction and description; (1) Extract scale-invariant feature transform (SIFT) feature points from the current stereo image and generate a feature point set , express feature points; (2) For each feature point Calculate 128-dimensional SIFT descriptor ; (3) Combine depth information to construct enhanced descriptors for each feature point ,in Feature points The corresponding depth value; Step 2-3: Topographic base map matching; (1) From the pre-stored topographic base map Search for matching regions in ,The initial search range is determined based on the estimated position of inertial navigation; (2) Using the fast approximate nearest neighbor FLANN algorithm Search and feature point sets Reference feature point set for feature point matching ; (3) Calculate matching pairs Similarity score: ; Represents the reference feature points in the topographic base map, Indicates the SIFT features corresponding to the reference feature points, Represents the standard deviation parameter of the Gaussian similarity function, which controls the sensitivity of the similarity calculation; (4) Retain matching pairs that meet the conditions through ratio test: ; in is the ratio threshold; is the similarity score between the current feature point and the reference feature point; Step 2-4: Pose estimation and refinement: (1) Estimate camera pose transformation by combining RANSAC algorithm with P3P method ; (2) Use the retained inlier matching pairs to perform Bundle Adjustment optimization to minimize the reprojection error: ； in are the coordinates of the three-dimensional feature points, is the image projection coordinate corresponding to the three-dimensional feature point, represents the camera projection function, which projects a 3D point onto the image plane; Represents the reprojection error, which measures the accuracy of pose estimation; (3) Combine inertial measurement unit data for pose fusion to obtain a smooth pose estimate : in is the pose obtained by map matching, is the updated pose of inertial navigation, is the fusion coefficient; Step 2-5: Dynamic map update; (1) Update the local map online based on newly acquired sensor data and pose estimation; (2) Using the sliding window strategy, maintain a Local map of frame data; (3) For newly observed feature points, update their positions and descriptors in the map to improve the accuracy of subsequent matching.

4. The method for autonomous positioning and obstacle avoidance of a fixed-wing UAV flying close to the ground according to claim 3, characterized in that: The step 3 is specifically as follows: Step 3-1: Obstacle perception and representation: (1) Fuse multi-source sensor data to detect obstacles ahead and generate a unified obstacle representation: ; Represents the multi-sensor fusion detection function; (2) For each obstacle Extract position, size, velocity vector, obstacle type: , Respectively represent the position coordinates, size, velocity vector, and obstacle type; (3) According to the characteristics of the obstacle, the obstacle is classified into static obstacles and dynamic obstacles ; Step 3-2: constrain topology graph construction; (1) Identify the set of constraints in trajectory optimization based on the flight mission and environment characteristics ; All indicate flight restrictions; (2) Analyze the dependencies between constraints and construct a constraint topology graph ; Represents the dependency edge set between constraints; (3) Calculate the weight of each constraint node : in, Indicates urgency, Indicates feasibility impact, Indicates the dependency on other constraints, 、 、 All are weight coefficients; Step 3-3: Hierarchical optimization sequence generation; (1) Based on the constraint topology graph, use the topological sorting algorithm to generate the optimized sequence ; Respectively represent the constraint processing order sorted by dependency, is the first constraint to be addressed. is the last constraint to be processed; (2) According to the sorting results, the constraints are divided into basic constraints , safety constraints and performance constraints ; (3) Design a multi-stage optimization strategy to handle different levels of constraints in turn: Phase 1: Dealing with basic constraints : in express The acceleration vector at the moment, Indicates the maximum allowable acceleration; Phase 2: Introducing safety constraints : in Indicates the distance between the drone and the obstacle. Indicates the safety distance threshold; Indicates the flight trajectory of the drone; Phase 3: Considering performance constraints : , in Indicates the target location, represents the desired terminal velocity, represents the position of the terminal at time T, represents the speed at the terminal time T; Step 3-4: Construction of safety corridor; (1) Based on the current environment perception results and basic constraints , build an initial safety corridor; Use convex polyhedron to represent the safe area: , Represents the three-dimensional space position coordinates, represents the normal vector matrix of the i-th constraint surface, Represents the distance parameter of the i-th constraint surface; Each inequality represents a half-plane constraint, which together define the safe flight area; (2) Based on the obstacle distribution, decompose the safety corridor into continuous convex polyhedron segments , They represent the first, second, ..., and first safety corridor areas respectively; (3) For dynamic obstacles, predict their future motion trajectories and construct time-varying safety corridors; Steps 3-5: parameterized trajectory representation; (1) Using B-spline curve to represent the flight trajectory: in, is the i-th control point, is the d-order B-spline basis function, is the number of control points; (2) The trajectory is parameterized as a set of control points: ; (3) Initial control point Obtained by uniform sampling in the safe corridor: , Represent the coordinates of the 0th, 1st, ..., numth initial control points respectively.

5. The method for autonomous positioning and obstacle avoidance of a fixed-wing UAV flying close to the ground according to claim 4, characterized in that: The step 4 is specifically as follows: Step 4-1: Dynamic relaxation strategy design: (1) Design the relaxation function of the dynamic constraint and dynamically adjust the constraint strength according to the optimization stage: ； in, is the original constraint, is the relaxation coefficient, is the current iteration number, Indicates the maximum number of iterations; represents the control input variable; represents the relaxed constraint function; (2) Design relaxation strategies for different constraints based on the constraint hierarchy: Safety constraints are obstacle avoidance: , indicating no relaxation; Dynamic constraints are speed / acceleration limits: → , indicating a gradual tightening from initial relaxation; Smoothness constraints: → , indicating a transition from initial relaxation to partial tightening; Step 4-2: Construction of multi-objective cost function; Design synthesis cost function , which includes multiple optimization objectives: ； in: Obstacle avoidance safety cost function: , Indicates the time range, Indicates the number of obstacles, represents the obstacle avoidance coefficient, represents the distance function, represents the position of the i-th obstacle at time t; Trajectory smoothness cost function: , represents the acceleration vector at time t+1; Energy consumption optimality cost function: , represents the control input vector, represents the time step; Terminal constraint cost function: , represents the position of the terminal at time T, Indicates the target location, represents the position weight coefficient, represents the speed at the terminal time T, Indicates the target speed; 、 、 、 is the weight of each cost function; Represents time parameters; Step 4-3: Progressive optimization algorithm; Based on the hierarchical optimization sequence, a three-stage progressive optimization strategy is designed: (1) Global planning stage, accounting for 10% of the total number of iterations: (a) Freeze All constraints outside (b) Using a large step size search to determine the overall trajectory shape; (c) Use the graph search-based A* algorithm for preliminary path planning: ； ； (2) Security optimization phase, accounting for 40% of the total iterations; (a) Activate safety constraints ; (b) Minimize the collision cost using the covariant Hamiltonian optimized motion planning CHOMP algorithm: ； ； (c) Security weight gradually increase; (3) Performance optimization phase, accounting for 50% of the total iterations; (a) Activate performance constraints ; (b) Solve using the continuous-time optimal control method: ； ； (c) Gradient projection method is used to ensure constraint satisfaction: ； in is the adaptive step size, To project the trajectory into the safe corridor; represents the gradient of the cost function at the kth iteration; Step 4-4: Intermediate trajectory transition and adaptation; (1) Constructing an intermediate transition trajectory set , as a bridge between theoretical models and practical scenarios; All represent intermediate transition trajectories; represents the total number of intermediate trajectories; (2) Each intermediate trajectory Corresponding to different flight conditions; (3) Based on the current flight state, the most similar intermediate trajectory is selected as the optimization starting point; (4) Motion similarity evaluation function: ； in 、 、 is the weight coefficient; Represent two trajectories to be compared, They represent the position vector corresponding to the trajectory, They represent the velocity vector corresponding to the trajectory, They represent the acceleration vectors corresponding to the trajectories respectively; Step 4-5: trajectory mapping and closed-loop correction; (1) Mapping the optimized trajectory to the actual control instructions: ; represents the ideal trajectory after optimization, Represents the control instruction mapping function, Represents the current flight state vector; (2) Build a closed-loop correction mechanism to adjust the trajectory in real time: in and are position error and velocity error respectively, and are all feedback gain matrices, represents the trajectory after closed-loop correction; (3) Using the sliding time window strategy, every Replan local trajectories in seconds.

6. The method for autonomous positioning and obstacle avoidance of a fixed-wing UAV flying close to the ground according to claim 5, characterized in that: The ratio threshold ; Fusion coefficient ; ; , , ; , , ; .

7. The method for autonomous positioning and obstacle avoidance of a fixed-wing UAV flying close to the ground according to claim 6, characterized in that: The initial weight value of the cost function is set to: , , , .

Images