Unmanned aerial vehicle autonomous navigation method under complex illumination condition

Abstract

The invention discloses an unmanned aerial vehicle autonomous navigation method under a complex illumination condition, and the method comprises the steps: obtaining an image sequence and an inertial measurement value through the synchronous data collection of a binocular camera and an IMU (Inertial Measurement Unit); key point detection and matching in a dynamic environment are realized based on SuperPoint and LightGlue models, and pose estimation is optimized in combination with IMU pre-integration; constructing a dynamic voxel map based on binocular depth information and pose estimation, and performing real-time path planning of obstacle perception through a D * Lite algorithm, and taking the real-time path planning as a global path constraint; a discrete path generated by adopting MINCO trajectory representation and dynamic constraint optimization D * Lite is adopted, a smooth and executable unmanned aerial vehicle trajectory is generated, and finally autonomous obstacle avoidance flight is achieved through a control instruction. According to the invention, the accuracy and robustness of unmanned aerial vehicle positioning are improved; and local mapping is carried out by using camera depth data through a planner, and a traditional global planning algorithm and trajectory optimization are combined to find an optimal route, so that rapid autonomous navigation of the unmanned aerial vehicle is realized.

Description

Technical Field

[0001] The present invention relates to the technical field of UAV autonomous navigation, and particularly to a UAV autonomous navigation method under complex lighting conditions. Background Art

[0002] With the continuous progress of UAV technology, its development momentum in the civilian field is rapid. Especially with the improvement of positioning and mapping technology, the application scope of UAVs is expanding day by day, and autonomous navigation has become an important research direction in the field of robotics. UAVs play an important role in fields such as cluster performances, agricultural plant protection, aerial photography, 3D reconstruction, regional search and rescue, power inspection, police security, and military.

[0003] When a UAV conducts reconnaissance indoors or outdoors where it is affected by light and signals, it often encounters problems such as overexposure, insufficient light, and GPS positioning loss. Generally speaking, UAV navigation needs to first sense its own position and the surrounding environmental representation, and then further perform path planning and obstacle avoidance, control the UAV to move specifically according to the control algorithm, and reach the specified target point according to the task requirements.

[0004] In an environment with many obstacles, the GPS positioning effect is poor or even the signal is missing. The combined navigation method of binocular vision and inertial measurement unit can well solve the positioning accuracy problem caused by signals. As the carrier of UAV vision, a camera can obtain a large amount of information in the environment, including light, object category color, distance, etc. Compared with a monocular camera, binocular vision mimics the imaging principle of the human eye and estimates the distance of an object from the camera through the binocular parallax. The disadvantage is that it is greatly affected by changes in illumination, sensitive to texture changes, which may lead to matching errors. In addition, the depth calculation effect for weak texture environments such as walls is poor, and there is also the problem of high computing power consumption for depth calculation. Complementary to this is the IMU (Inertial Measurement Unit) sensor, which obtains the motion information of the UAV through gyroscopes and accelerometers, calculates the current acceleration and attitude quaternion through integration, is not limited by illumination conditions, can provide continuous motion information during the interval of binocular camera image processing, reduce the delay of visual information, and assist in providing more reliable navigation information in cases of large illumination changes or lack of environmental texture.

[0005] Visual odometry (VIO) is a method of positioning and navigation without GPS by means of low-cost sensors such as cameras and IMUs. It estimates the position and attitude of an unmanned aerial vehicle (UAV) by collecting inter-frame image information. However, current VIO algorithms all encounter robustness problems, and positioning drift still occurs in the case of missing image textures. There are also few datasets for testing the performance of VIO algorithms, which cannot fully reflect the practicality of VIO algorithms in engineering. In addition, it is also a challenging problem to navigate in scenarios with rapid light changes and missing features under low light. At present, UAV navigation technology is mostly affected by light and planned speed, and cannot balance rapid positioning and rapid planning. Summary of the Invention

[0006] The purpose of the present invention is to overcome the deficiencies of the prior art. To achieve the above purpose, a method for autonomous navigation of an unmanned aerial vehicle under complex lighting conditions is adopted to solve the problems raised in the above background technology.

[0007] A method for autonomous navigation of an unmanned aerial vehicle under complex lighting conditions includes the following steps:

[0008] Step S1: Obtain an image sequence and inertial measurement values through synchronous data acquisition of a binocular camera and an IMU.

[0009] Step S2: Implement key point detection and matching in a dynamic environment based on the SuperPoint and LightGlue models, and optimize the pose estimation by combining IMU pre-integration.

[0010] Step S3: Construct a dynamic voxel map based on binocular depth information and pose estimation, and perform real-time path planning for obstacle perception through the D*Lite algorithm, which is used as a global path constraint.

[0011] Step S4: Optimize the discrete path generated by D*Lite using the MINCO trajectory representation and dynamic constraints to generate a smooth and executable UAV trajectory, and finally achieve autonomous obstacle avoidance flight through control instructions.

[0012] As a further solution of the present invention: The specific steps in step S1 include:

[0013] Step S11: Receive data through a binocular vision camera and an IMU inertial measurement unit at a preset frequency.

[0014] Step S12: The picture sequences read by the binocular vision camera are respectively:

[0015] F src_r , src_l,i , src_l,1 , src_l,M , src_l,2 , src_l ={F src_l,1 ,F src_l,2 ,...,F src_l,i ,...,F src_l,M} and F src_r={F src_r,1 ,F src_r,2 ,...,F src_r,i ,...,F src_r,M};

[0016] Wherein, src_l and src_r represent the left and right stereo image sequences, n = 1, 2,..., M, and M represents the total number of single-camera images;

[0017] Step S13: Obtain the input data I according to the IMU inertial measurement unit src ={I src_1 ,I src_2 ,...,I src_i ,...,I src_N};

[0018] Wherein, N represents the total number of data collected by the IMU;

[0019] Step S14: Obtain the data I collected at each timestamp src_i Expressed as:

[0020] I src_i ={timestamp i ,w_RS_S_x i ,w_RS_S_y i ,w_RS_S_z i ,a_RS_S_x i ,a_RS_S_y i ,a_RS_S_z i};

[0021] Wherein, timestamp i represents the current timestamp, that is, the time when the message is recorded, in ns; w_RS_S_x i ,w_RS_S_y i ,w_RS_S_z i represent the xyz-axis angular velocity values collected by the inertial measurement unit at the current timestamp, in rad / s; a_RS_S_x i ,a_RS_S_y i ,a_RS_S_z i represent the three-axis acceleration values collected at the current timestamp, in m / s^2.

[0022] As a further solution of the present invention: The specific steps in step S2 include:

[0023] Step S21: Use the SuperPoint network to extract the key point heat map and 256-dimensional descriptor of the binocular image, and combine YOLOv8 to detect dynamic targets in real time;

[0024] Step S22: Through the self-attention and cross-attention mechanisms of the LightGlue model, combined with relative position encoding, output a soft assignment matrix across images, and screen high-confidence feature matching pairs;

[0025] Step S23: Fuse the 3D point cloud of binocular triangulation and the IMU pre-integration result, and jointly optimize the camera pose, map points, and IMU bias through the PnP algorithm and bundle adjustment.

[0026] As a further solution of the present invention: The specific steps in the step S23 include:

[0027] Currently, it is necessary to calculate the camera pose R and t, which are represented by the Lie group as T, and K is the camera internal parameter. Specifically:

[0028]

[0029] Among them, f x and f y are the focal lengths, and c x and c y are the optical center coordinates;

[0030] The preset coordinates of the three-dimensional point are P i =[X i , Y i , Z i T , and the projected pixel coordinates are u i =[u i , v i T , then the relationship between the pixel position and the spatial point position is:

[0031]

[0032] Construct a least-squares problem to solve the error of the unknown camera pose and the noise in the observation points. The error minimization formula is:

[0033]

[0034] Among them, the measurement model of the angular velocity is:

[0035] ω measured =ω true +b g +η g

[0036] The acceleration measurement model is:

[0037] a measured =a true +b a +η​​a

[0038] where b a , b g represent the biases of acceleration and angular velocity respectively; η a , η g represents the measurement noise, usually Gaussian noise;

[0039] The IMU performs pre-integration using the basic equations of inertial navigation. For adjacent frames i and j, the relative rotation, velocity change, and position change from i to j are calculated through the IMU data.

[0040] As a further solution of the present invention: The specific steps in step S3 include:

[0041] Step S31: Convert the binocular depth map into a point cloud in the world coordinate system, update the voxel occupancy probability based on the dynamic log-likelihood ratio, and construct a probabilistic obstacle map;

[0042] Step S32: Based on the dynamic priority queue and the edge cost update mechanism, perform real-time search and replanning of the obstacle avoidance path in the probabilistic voxel map based on the D*lite algorithm;

[0043] Among them, when using the D*lite algorithm for path finding, a feasible path from the starting point to the target point is found using graph search;

[0044] The process of path finding and planning using the D*lite algorithm includes the key functions calculateKey(s), initialize(), updateVertex(u), computeShortestPath(), and main();;

[0045] The priority of node s is calculated using the Calculatekey(s) function part. The calculation of the priority is based on the g and rhs values of the node, combined with the heuristic estimate. The priority queue sorts the nodes according to the priority.

[0046] As a further solution of the present invention: The specific steps in step S4 include:

[0047] Step S41: Parameterize the trajectory through piecewise polynomials, construct a differentiable banded linear equation system in combination with the boundary conditions, and perform decoupled optimization of the trajectory time and space;

[0048] Among them, when using the MINCO trajectory representation class, a multi-stage optimal control problem is constructed, and the polynomial trajectory of order N = 2s - 1 is defined as follows:

[0049]

[0050] Among them, p(t) is an N - degree polynomial of M segments in m dimensions; is the coefficient matrix, s is the order of the derivative, and q=(q1,..., qM - 1) represents the intermediate points, where is the zero - order derivative specified at ti; T=(T1,..., TM) T represents the time vector;

[0051] Step S42: Design an objective function based on differential flatness, and use the L - BFGS algorithm to solve the trajectory that satisfies the thrust and state constraints;

[0052] Among them, the objective function is expressed as:

[0053] min(λ s J s +λ t J t +λ d J d +λ o J o );

[0054] In the formula, J s , J t , J d , and J o respectively represent the smooth - term cost, the total - time cost, the dynamic feasibility, and the safety - space cost, and the weight distributions are λ s , λ t , λ d , λ o ;

[0055] Step S43: Convert the acceleration and angular - velocity commands of the optimized trajectory into motor speeds through a PID controller, drive the UAV to track the trajectory, and perform autonomous navigation.

[0056] Compared with the prior art, the present invention has the following technical effects:

[0057] By adopting the above - mentioned technical solution, a complete process of multi - sensor fusion perception - dynamic mapping - real - time planning - trajectory optimization is constructed. Through the combination of multi - sensor data fusion, lightweight deep - learning models, and optimal control theory, a complete technical closed - loop from environmental perception to autonomous flight of the UAV is realized, which has high precision, strong robustness, and engineering practicability.

[0058] Meanwhile, a neural network is used to extract and match features. Local mapping is performed using depth information in the case of only binocular cameras and IMU sensors, and fast path planning is applied for UAV flight and obstacle avoidance, thereby improving the success rate of UAV navigation in harsh environments. Brief Description of the Drawings

[0059] The specific implementation manners of the present invention will be described in detail below with reference to the accompanying drawings:

[0060] Figure 1 It is a schematic diagram of the steps of the UAV autonomous navigation method for the disclosed embodiments of this application;

[0061] Figure 2 It is a flow chart of the UAV positioning and path planning for the disclosed embodiments of this application;

[0062] Figure 3 It is a flow block diagram of the improved D*lite algorithm for the disclosed embodiments of this application. Specific implementation manners

[0063] The technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings in the embodiments of the present invention. Obviously, the described embodiments are only a part of the embodiments of the present invention, rather than all the embodiments. All other embodiments obtained by those of ordinary skill in the art based on the embodiments of the present invention without creative efforts shall fall within the protection scope of the present invention.

[0064] Please refer to Figure 1 and Figure 2 , in the embodiments of the present invention, a UAV autonomous navigation method under complex illumination conditions includes the following steps:

[0065] Step S1: Obtain an image sequence and inertial measurement values through synchronous data acquisition of a binocular camera and an IMU. The specific steps include:

[0066] Step S11: Receive data through a binocular vision camera and an IMU inertial measurement unit at a preset frequency; specifically, receive data at a certain frequency through the binocular vision and inertial measurement unit (IMU) as sensors, and publish data topics through ros;

[0067] Step S12: The picture sequences read by the binocular vision camera are respectively:

[0068] F src_l ={F src_l,1 , F src_l,2 ,..., F src_l,i ,..., F src_l,M} and F src_r ={F src_r,1 , F src_r,2 ,..., F src_r,i ,..., F src_r,M};

[0069] In the formula, src_l and src_r represent the picture sequences of the left and right binoculars, n = 1, 2,..., M, and M represents the total number of pictures of a single camera;

[0070] Step S13. Obtain the input data I according to the IMU inertial measurement unit src = {I src_1 , I src_2 ,..., I src_i ,..., I src_N};

[0071] Wherein, N represents the total number of data collected by the IMU;

[0072] Step S14. Obtain the data I collected at each timestamp src_i Expressed as:

[0073] I src_i = {timestamp i , w_RS_S_x i , w_RS_S_y i , w_RS_S_z i , a_RS_S_x i , a_RS_S_y i , a_RS_S_z i};

[0074] Wherein, timestamp i represents the current timestamp, that is, the time when the message is recorded, in units of ns; w_RS_S_x i , w_RS_S_y i , w_RS_S_z i represent the xyz three-axis angular velocity values collected by the inertial measurement unit at the current timestamp, in units of rad / s; a_RS_S_x i , a_RS_S_y i , a_RS_S_z i represent the three-axis linear acceleration values collected at the current timestamp, in units of m / s^2.

[0075] Step S2. Implement key point detection and matching in a dynamic environment based on the SuperPoint and LightGlue models, and optimize pose estimation by combining IMU pre-integration.

[0076] Specifically, perform real-time detection and matching of key points and line features on the image data; combine the results with the inertial measurement unit data to estimate the UAV pose to obtain the current location;

[0077] The specific steps in Step S2 include:

[0078] Step S21: Use the SuperPoint network to extract the key-point heatmap and 256-dimensional descriptors of the binocular images, and combine with YOLOv8 to detect dynamic targets in real time;

[0079] Key-point feature detection and descriptor extraction, based on the superpoint fully convolutional neural network to process the nth binocular image F with H*W*1 resolution src_l,n , F src_r,n , Use a VGG-style encoder to reduce the size of the input image and increase the depth / dimension of the image F.

[0080] The encoder consists of convolutional layers, pooling layers and non-linear activation functions. Through two 3*3 convolutions and one max pooling operation, this step is repeated three times and then two 3*3*128 convolutions are performed. Finally, the spatial dimension of the image is reduced to 1 / 8 of the original, obtaining an image with dimensions H / 8*W / 8*128; then, the image is decoded by the interest-point decoder and descriptor decoder simultaneously.

[0081] The interest-point decoder upsamples the image H / 8*W / 8*128 to 256 channels using a 3*3*128 convolutional kernel, then downsamples it to an image with dimensions H / 8*W / 8*65 using a 1x1 convolutional kernel, and finally softmax and reshape to obtain the original resolution image of H*W*1.

[0082] The descriptor decoder first learns a semi-dense descriptor with dimensions H / 8*W / 8*D (for example, one pixel point is taken for descriptor extraction every 8 pixels), where D is the vector of the descriptor for each pixel point. It successively passes through 256-unit convolutional kernels of 3*3 and 1*1 to obtain 256-channel data, and finally obtains the complete descriptor through bicubic interpolation, and L2 regularization to obtain a unit-length descriptor, outputting an image with dimensions H*W*256. According to the input image F src_l,n , F src_r,n , the output obtains F des_l,n ={F src_l_point,n ,F src_l_descri,n}, F des_r,n ={F src_r_point,n ,F src_r_descri,n};

[0083] Among them, it respectively contains the heatmap F with the interest-point probability in the range of 0-1 src_l_point,n F src_r_point,n (each pixel in the figure represents the probability value of the interest point) and the high-dimensional image F containing the dense descriptor src_l_descri,n F src_r_descri,n , with the same size as the input. Among them, to understand how lightglue performs feature matching, it is necessary to figure out how the pictures output by superpoint are arranged, because the descriptors are for matching;

[0084] Then, using the object detection algorithm of YOLOv8, with pedestrians as dynamic labels and other objects as static labels, real-time object detection is performed, and the speed is synchronized with the acquisition of binocular images. The binocular image F is input src_l and Fsrc_r , and the output Pdetec_l_i = {(xi, yi), (wi, hi)} and Pdetec_r_j = {(xj, yj), (wj, hj)} are obtained;

[0085] Specifically, they are the coordinates (xi, yi), (xj, yj) and box sizes (wi, hi), (wj, hj) of the i-th and j-th dynamic objects detected in the left and right images;

[0086] where i = 1, 2,..., M', j = 1, 2,..., N', and M' and N' are the numbers of dynamic objects in the two images.

[0087] Combining the outputs of Pdetec_l_I and Pdetec_r_j for joint feature extraction, we get:

[0088] F des_l,n ={F src_l_point,n ,F src_l_descri,n}F des_r,n ={F src_r_point,n ,F src_r_descri,n};

[0089] Pass it to the feature matching stage. As a low-coupling combination, they do not affect each other, and multi-threaded acceleration synchronization operations can be performed;

[0090] Step S22: Through the self-attention and cross-attention mechanisms of the LightGlue model, combined with relative position encoding, output a soft assignment matrix across images to screen high-confidence feature matching pairs;

[0091] Specifically, for key point matching, the image pairs output by the superpoint network are:

[0092] A: = {1,..., M}, B: = {1,..., N};

[0093] After non-maximum suppression (NMS) processing, the filtered feature points / interest points M or N and descriptors M*d or N*d (d is the dimension) are obtained. The feature points combined with the descriptors are used by the lightglue model for feature matching;

[0094] where A and B are the two images used for feature matching. Given two sets of feature points (with numbers M and N respectively) extracted from images A and B, each feature point i is represented by a normalized 2D coordinate p i : = (x, y)i ∈ [0, 1] 2 and visual descriptors (with d dimension being 256).

[0095] For each feature point i of image I ∈ {A, B}, a state is associated The state is initialized as the descriptor and is subsequently updated by the network layer.

[0096] The network layer is stacked by L = 9 identical layers, jointly processing the data of two groups of images. In each layer, lightglue takes the respective feature points pi and descriptors di of images A and B as the input of the self - attention unit based on relative position encoding (using rotation encoding). In the self - attention unit, each feature point focuses on other feature points in the same image. By calculating the attention scores of each point to all other points (obtaining the attention scores by calculating the dot product of the query vector and the key vector and considering the relative position encoding), weights are obtained according to these scores, and weighted summation is performed to update the state of each point For each feature point i, the self - attention unit aggregates the information from the original image S ∈ {A, B} obtained by calculating the weighted average of all states j.

[0097] Then, the processing results of A and B are communicated through the cross - attention unit. For each feature point in one image, the attention scores between it and all feature points in the other image are calculated based on the similarity metric. Using the calculated attention scores, the states of the feature points in the other image are weighted and summed according to the attention scores of each state to form a message. The aggregated message is added to the state of the current feature point to update the representation of the feature point (usually involving MLP non - linear transformation). After each layer, it is decided whether to stop the inference according to the confidence classifier c. If the confidence is high, the inference can be stopped in advance to reduce unnecessary calculations; otherwise, continue the inference of the next layer and prune the non - matchable points.

[0098] After all layers are completed, a soft partial assignment matrix P ∈ [0, 1] MxN is predicted, which is the output of the network, where M and N are the numbers of feature points of images A and B respectively. Each element Pij of the matrix represents the probability or similarity score that the i - th feature point in image A matches the j - th feature point in image B. Finally, the matching feature point pairs can be extracted from this soft assignment matrix.

[0099] Step S23: Fuse the 3D point cloud of binocular triangulation and the IMU pre - integration result, and jointly optimize the camera pose, map points, and IMU bias through the PnP algorithm and bundle adjustment.

[0100] Regarding data fusion, it lies in combining the results of feature matching and the data of the inertial measurement unit for the pose estimation of the UAV. First, pre-integrate the IMU, and use the integration result and the previous frame for initial pose estimation. Then, perform triangulation of points and local bundle adjustment, and finally update the map.

[0101] First, through threshold screening using the image assignment matrix P of the previous step, a matrix of feature point matching pairs is obtained, and the pose estimation is solved using the PnP algorithm; according to the triangulation principle of the binocular camera, using information such as the camera internal parameters, baseline, and parallax, the three-dimensional coordinates of the feature points are directly calculated through triangulation. Then, using the three-dimensional coordinates of the known n feature points obtained by this method in this frame and the two-dimensional matching points of the current frame, the transformation matrix R, t from the previous frame to this frame is generated using the PnP algorithm. Then, in the way of non-linear optimization, that is, using local bundle adjustment to construct a least squares problem and iterating to simultaneously optimize the camera pose and the positions of the three-dimensional points Pi (i = 1, 2,..., N) of adjacent frames, as well as the state of the IMU.

[0102] The specific steps in step S23 include:

[0103] Currently, it is necessary to calculate the camera pose R and t, which are represented by the Lie group as T, and K is the camera internal parameter, specifically:

[0104]

[0105] Among them, f x and f y are the focal lengths, c x and c y are the optical center coordinates;

[0106] The preset coordinates of the three-dimensional point are P i = [X i , Y i , Z i T , and the projected pixel coordinates are u i = [u i , v i T , then the relationship between the pixel position and the spatial point position is:

[0107]

[0108] Construct a least squares problem to solve the error of the unknown camera pose and the noise existing in the observation points. The error minimization formula is:

[0109]

[0110] For an IMU, the sensor needs to provide two quantities: angular velocity and acceleration. Among them, the measurement model of angular velocity is:

[0111] ω measured = ω true + b g + η g

[0112] The acceleration measurement model is:

[0113] a measured = a true + b a + η a

[0114] Among them, b a , b g respectively represent the biases of acceleration and angular velocity; η a , η g represents the measurement noise, usually Gaussian noise;

[0115] The IMU uses the basic equation based on inertial navigation for pre-integration. For adjacent frames i and j, the relative rotation, velocity change, and position change from i to j are calculated through the IMU data.

[0116] In the optimization process of the IMU, the pre-integration results of rotation, velocity, and position are compared with the results of visual pose estimation to calculate the IMU residuals. The residuals are used to minimize the error within adjacent frames through local bundle adjustment optimization to adjust the states of the camera and the IMU.

[0117] Step S3: Construct a dynamic voxel map based on binocular depth information and pose estimation, and perform real-time path planning for obstacle perception through the D*Lite algorithm, which is used as a global path constraint.

[0118] As Figure 3 shown, the figure shows the flow chart of the improved D*lite algorithm;

[0119] The estimated pose pose and the depth information depth of the depth camera are obtained by the drone to build a map, and then path planning and obstacle avoidance are performed;

[0120] The specific steps in step S3 include:

[0121] Step S31: Convert the binocular depth map into a point cloud in the world coordinate system, update the voxel occupancy probability based on the dynamic log-likelihood ratio, and construct a probabilistic obstacle map;

[0122] Regarding the part of map building, it is to use the current pose information pose of the drone and the depth map F calculated by the binocular camera depth,i(where \(i = 1, 2, \ldots, M\), representing the sequence of depth maps), use the camera intrinsic matrix \(K\) to convert the depth pixel coordinates \((u, v)\) to the three - dimensional coordinates \((X c , Y c , Z c ) of the point cloud:

[0123]

[0124] where \(d = F depth,i (u, v)\), representing the depth of the point in the depth map \(F depth,i ;

[0125] Next, perform the camera coordinate system transformation. According to the pose information of the camera, convert the point cloud pose \(P_c\) to the format \(P_w\) in the world coordinate system. \(T_{wc}\) is the transformation matrix:

[0126] P w = T wc ·P c

[0127] Convert the point cloud to a voxel map. Set the voxel size and map each point in the point cloud to the corresponding voxel grid. First, define the voxel size \(voxel\_size\) and traverse all points in the point cloud to discretize its world coordinates \(P_w\):

[0128]

[0129] Insert the discretized points into the voxel grid and mark their occupancy status.

[0130] The UAV equipped with a binocular camera can only see the surface of the obstacles in front by default. Therefore, the local map is built according to the FOV, that is, the field of view of the UAV. According to whether each voxel observed in each frame is an obstacle, assign a probability \(Prob\) score to each voxel being an obstacle. If this frame determines that the voxel \(V(i, j, k)\) is an obstacle, the probability of this grid increases; otherwise, the probability decreases. Each grid has a probability value. 0 means free; 1 means occupied; 0.5 means the occupancy status is unknown. Use the log - likelihood ratio to calculate and update the occupancy probability to avoid numerical instability.

[0131] Definition of the log - likelihood ratio:

[0132]

[0133] where \(P(m x |z 1:t , x 1:t ) represents the probability that the grid \(m_x\) is occupied at time \(t\), given all observations \(z 1:t and the UAV position \(x 1:t . After obtaining new observation data, the likelihood ratio is updated:

[0134]

[0135] Finally, substituting lt(x) into the sigmoid function can obtain the corresponding occupancy probability.

[0136] Step S32: Based on the dynamic priority queue and the edge cost update mechanism, in the probability voxel map, use the D*lite algorithm to search and replan the obstacle avoidance path in real time;

[0137] Regarding the path planning part, it lies in using the D*lite algorithm to find a path. Use graph search to find a feasible path from the starting point to the target point. Assume that the obstacles observed by the camera in the local map are only the surface part within the field of view angle, and the area behind the occlusion is set as an open area. Then the feasible path lies in searching for a route that bypasses the current obstacle from the current position of the camera.

[0138] Define each voxel as s, which can also be understood as a node (subsequently represented by nodes). Record the state and value of variables through the nodes. There is a global priority queue that determines which node to detect and modify each time. After a series of detections, select which route according to the value of each node;

[0139] Among them, use the D*lite algorithm to find a path, and use graph search to find a feasible path from the starting point to the target point;

[0140] The process of path planning using the D*lite algorithm includes the key functions calculateKey(s), initialize(), updateVertex(u), computeShortestPath(), and main();

[0141] Use the Calculatekey(s) function to calculate the priority of node s. The calculation of the priority is based on the g and rhs values of the node, combined with heuristic estimation. The priority queue sorts the nodes according to the priority. It is expressed as:

[0142] return[min(g(s),rhs(s))+h(s start ,s)+k m ; min(g(s),rhs(s))];

[0143] Among them, key(s) is the value used to represent the priority of the node, g(s) is the known cost from the current position to the target position; rhs(s) is the sub-optimal path estimate value from the predecessor node of s to the target node. km is used for key value modification, which can change the key value and the order of the nodes in the queue to make the sorting more reasonable.

[0144] The Initialize() function is used to initialize all variables and states of the algorithm, including setting the g-value and rhs-value of the initial node, and inserting the target node into the priority queue. The execution process is as follows:

[0145]

[0146] km = 0;

[0147] For all s ∈ S, rhs(s) = g(s) = ∞;

[0148] rhs(s goal ) = 0;

[0149] U.Insert(s goal , CalculateKey(s goal ));

[0150] First, initialize g(s) and rhs(s) of all nodes s to infinity. Let rhs(s goal ) of the target node be 0, indicating that there is no successor path for the target node. Insert s goal into the priority queue with the priority of CalculateKey(s goal ).

[0151] The Updatevertex(u) function is used to update the state of node u, that is, adjust the g and rhs values according to the changes of adjacent nodes. Specifically as follows:

[0152] if (u ≠ s goal ) rhs(u) = min s'∈Succ(u) (c(u, s') + g(s'));

[0153] if (u ∈ U) U.Remove(u);

[0154] if (g(u) ≠ rhs(u)) U.Insert(u, CalculateKey(u));

[0155] Its function is to update the adjacent optimal parent node of the neighboring node u. First, calculate the minimum cost from all successor nodes of u to u, where succ(u) is the set of all successor nodes of u, and c(u, s’) is the movement cost from u to s’, which can also be considered as the edge cost. If u is in the U set, then remove u; if g(u) and rhs(u) are not equal, then u needs to be inserted into the priority queue and its priority is updated.

[0156] The ComputeShortestPath() function is the core path calculation function of the algorithm. By processing the nodes in the priority queue, it continuously updates the path until the optimal path is found or the termination condition is met.

[0157] Repeat the following operations until the priority queue is empty or g(start) == rhs(start):

[0158] while (U.TopKey() < CalculateKey(s start ) OR rhs(s start )!= g(s start ))

[0159] Next, obtain the key value kold of the most prioritized point, dequeue the node with the smallest key value in the priority queue U, and the return value is this node:

[0160] k old = U.TopKey();

[0161] u = U.Pop();

[0162] If the key value of u is greater than kold, then insert the node u and its key value into the priority list U:

[0163] if (k old <CalculateKey(u))

[0164] U.Insert(u, CalculateKey(u));

[0165] If g(u) > rhs(u), that is, locally over-consistent and the edge cost function value becomes lower, it means that the obstacle on the node is cleared or a shorter path is found. At this time, make it locally consistent, traverse the set of predecessor nodes of node u, and update the key value of node s in the set (using s to represent any node):

[0166] else if (g(u) > rhs(u))

[0167] g(u) = rhs(u);

[0168] for all s ∈ Pred(u) UpdateVertex(s);

[0169] Otherwise, if u encounters an obstacle, traverse the set of predecessor nodes of node u and update the key values of the nodes in the set:

[0170] else

[0171] g(u) = ∞;

[0172] For all s, Pred(u) ∪ {u} UpdateVertex(s);

[0173] Next, the main function procedure Main() is elaborated, which includes initialization, calculating the shortest path, pathfinding through loop determination, and updating the priority queue; First, D*lite regards the current position point slast (the position node at the next moment) as the new sstart node, and repeatedly calculates the shortest path between the target point and the new start point to meet the replanning requirements. Then, the operations of initialization and planning the shortest path are carried out:

[0174] s last = s start ;

[0175] Initialize();

[0176] ComputeShortestPath();

[0177] Enter the loop, and the loop stop condition is that the start point and the end point coincide. Calculate the rhs values of all successor nodes of u, and select the corresponding point with the smallest value as the new start point. If the cosine value of the direction vector from the current position to the target point and the direction vector from the starting point to the target point is greater than 0, the movement step size is set to 2, otherwise the step size rate is default set to 1, that is, the exploration space is 26 voxel blocks on the surrounding three axes:

[0178] while (s start ≠ s goal )

[0179]

[0180] set rate = 2

[0181] else

[0182] set rate = 1

[0183] s start = argmin s'∈Succ(start) (c(s start , s') + g(s'));

[0184] Move the drone, update the starting point start, and scan the map to change the edge costs:

[0185] Move to s start ;

[0186] Scan graph for changed edge costs;

[0187] If any edge costs change, define the distance cost km from the current position point slast to the new start node sstart of the drone:

[0188] if any edge costs changed

[0189] km = km + h(sslast, sstart);

[0190] s last = s start ;

[0191] For all directed edges (u, v) with changed edge costs, update the cost of the edge and vertex u, and finally calculate the shortest path using ComputeShortestPath:

[0192] for all directed edges(u,v)with changed edge costs

[0193] Update the edge cost c(u,v);

[0194] Update Vertex(u);

[0195] ComputeShortestPath();

[0196] In this way, according to the starting point and the target point, a feasible shortest path can be found in real time, and when an obstacle is added midway, replanning operations can be performed, saving the time of planning from scratch. When suddenly appearing obstacles are encountered during the planned movement of the drone, the search map can be updated according to the coordinate set of the obstacles in the voxel map, and D*lite replanning can be called.

[0197] Step S4. Use the MINCO trajectory representation and dynamic constraints to optimize the discrete path generated by D*Lite to generate a smooth and executable drone trajectory, and finally achieve autonomous obstacle avoidance flight through control instructions.

[0198] Optimize the path searched by D*lite to meet the reasonable allocation of the drone in time and space and conform to the dynamic principles.

[0199] The specific steps in step S4 include:

[0200] Step S41. Parameterize the trajectory through piecewise polynomials, construct a differentiable banded linear equation set in combination with boundary conditions, and perform decoupling optimization of trajectory time and space;

[0201] Among them, using the MINCO trajectory representation class, a multi-stage optimal control problem is constructed, and the polynomial trajectory of order N = 2s - 1 is defined as follows:

[0202]

[0203] Among them, p(t) is an N-degree polynomial of m dimensions and M segments; is the coefficient matrix, s is the derivative order, and the intermediate points are represented by q = (q1,..., qM-1), where is the zero-order derivative specified at ti; T = (T1,..., TM) T represents the time vector; ψ MINCO Each segment of is related to q and T. For c = M(q,T), there is M(T)c = b(q),

[0204] where E i and F i are matrices for constructing the linear equations, related to the boundary conditions and intermediate conditions of the trajectory.

[0205] M and b are the matrix and vector for constructing the linear equations, used to solve the optimal coefficient matrix c.

[0206] Because for any positive time vector T, M is non-singular and is a banded system. For a non-singular banded matrix, PLU decomposition can always be performed.

[0207] The spatial representation of the trajectory is as follows:

[0208]

[0209] Among them, the i-th segment of the trajectory

[0210]

[0211] β(x) = (1, x,..., x N ) T ,

[0212]

[0213] Among them, β(x) is the basis function, c i is the coefficient matrix, and Ti = ti - ti-1 represents the time allocated to the i-th segment of the trajectory. Then the total time is

[0214] Finally, by applying the optimality conditions, the optimal trajectory can be obtained without a cost function, which improves the operation efficiency. At the same time, the time length of each allocated curve is also obtained.

[0215] Next, the trajectory optimization is solved through the MINCO trajectory representation class. The simplified dynamic equation of the UAV can be expressed as follows:

[0216]

[0217] where m is the mass, τ is the skew-symmetric matrix format of the vector cross product, w is the angular rate, is the total thrust, and ei is the vector of the i-th column of the I3 identity matrix.

[0218] According to the differential flatness condition z = (p x , p y , p z , θ) T , the state of the UAV can be represented by the three-axis components of the position and the yaw angle:

[0219]

[0220] x and u are the state variables and control inputs respectively.

[0221] The ranges of the UAV speed, thrust, and angular rate satisfy the constraints:

[0222]

[0223] Then the optimization problem needs to minimize the following formula, where t M The previous parameters are weights:

[0224]

[0225] The initial and boundary conditions are The trajectory needs to pass through the hard constraint points at time ti, that is, the waypoint p(t i ) = q, M is an invertible band matrix function and is related to T: t i-1 <t i , v and a cannot exceed the maximum limit The thrust cannot exceed the limit τ min ≤||τ(t)||≤τ max , and the safety space constraint G ∈ space needs to be satisfied safe .

[0226] Step S42: Design the objective function based on differential flatness and use the L-BFGS algorithm to solve the trajectory that satisfies the thrust and state constraints;

[0227] Taken together, the optimization problem can be summarized as minimizing J s 、J t 、J d , and J o , where the objective function is expressed as:

[0228] min(λ s J s +λ t J t +λ d J d +λ o J o );

[0229] In the formula, J s 、J t 、J d , and J o represent the smooth term cost, total time cost, dynamic feasibility, and safety space cost respectively, and the weight distributions are λ s 、λ t 、λ d 、λ o ;

[0230] J s is defined as the integral of the square s-th derivative, specifically

[0231] J t is the total time J t = sum(T), and the dynamic feasibility penalty function is:

[0232]

[0233] where v m 、a m 、j m are the maximum speed, acceleration, and jerk (jerk) that cannot be exceeded, and (j / ki)Ti represents the j-th small segment time of the i-th trajectory segment.

[0234] Use the DP algorithm to optimize the path generated by D*lite local replanning: Since D*lite generates a series of discrete points, find the discrete point farthest from the line connecting the current position of the UAV to the target point to obtain the vertical distance. If it is greater than the threshold, keep the point and recursively perform this operation on the paths on both sides of the point. Otherwise, delete all points between the two points. This operation can simplify the number of path points in the replanning. p i,jThe acquisition of constraint points is obtained by sampling. By dividing each segment of the minco curve into ki parts, where j = {0, 1, 2…, ki}, combined with the time allocation Ti of the i-th segment of the curve, the points pi,j at specific moments after the time period is subdivided are obtained. Specifically:

[0235] p i,j = p((j / k i )T i ).

[0236] Since the trajectory generated by D*lite is close to the obstacle, and the UAV has its own volume, a force is required to push it away from the obstacle. Denote the points on the local trajectory of D*lite as s. Through the vector v pointing from pk determined by p i,j to s (1 ≤ i ≤ M and 0 ≤ j ≤ k i ), the points pk on the minco trajectory are pushed to the outside of the obstacle to approximate s or be farther away from the obstacle than s. The formula is as follows:

[0237] d = (p k - s) T v

[0238] Let the weight of the safety obstacle avoidance cost function J o at both ends of the i-th segment of the MINCO trajectory be 1 / 2, and the weight at the intermediate sampling points be 1, that is, (w1, w2,..., w ki ) = (1 / 2, 1,..., 1, 1 / 2). Then the formula for J o is:

[0239]

[0240] In this way, each optimization term of the optimization problem is obtained. Through the L - BFGS algorithm, the minimum objective function is optimized, and then the optimal curve for the UAV to optimize the trajectory with MINCO after path planning at the current moment is solved.

[0241] Step S43: Convert the acceleration and angular velocity commands of the optimized trajectory into motor speeds through a PID controller, and drive the UAV to track the trajectory for autonomous navigation.

[0242] Specifically, convert them into the speeds of four motors through a PID controller to control the UAV to fly autonomously.

[0243] Although the embodiments of the present invention have been shown and described, for those of ordinary skill in the art, it can be understood that various changes, modifications, substitutions, and variations can be made to these embodiments without departing from the principles and spirit of the present invention. The scope of the present invention is defined by the appended claims and their equivalents and should be included within the protection scope of the present invention.

Claims

1. An autonomous navigation method for an unmanned aerial vehicle under complex lighting conditions, characterized in that, It includes the following steps: Step S1: Obtain an image sequence and inertial measurement values through synchronous data acquisition of a binocular camera and an IMU; Step S2: Implement key-point detection and matching in a dynamic environment based on the SuperPoint and LightGlue models, and optimize pose estimation by combining IMU pre-integration; Step S3: Construct a dynamic voxel map based on binocular depth information and pose estimation, and perform real-time path planning for obstacle perception through the D*Lite algorithm, which is used as a global path constraint; Step S4: Optimize the discrete path generated by D*Lite using the MINCO trajectory representation and dynamic constraints to generate a smooth and executable UAV trajectory, and finally achieve autonomous obstacle avoidance flight through control commands.

2. The method for autonomous navigation of an unmanned aerial vehicle under complex lighting conditions according to claim 1, wherein The specific steps in step S1 include: Step S11: Receive data through a binocular vision camera and an IMU inertial measurement unit at a preset frequency; Step S12: The picture sequences read by the binocular vision camera are respectively: F src_l = {F src_l,1 , F src_l,2 ,..., F src_l,i ,..., F src_l,M} and F src_r = {F src_r,1 , F src_r,2 ,..., F src_r,i ,..., F src_r,M}; where src_l and src_r represent the picture sequences of the left and right binoculars, n = 1, 2,..., M, and M represents the total number of pictures of a single camera; Step S13: Obtain the input data I according to the IMU inertial measurement unit src ={I src_1 , I src_2 ,..., I src_i ,..., I src_N}; where N represents the total number of data collected by the IMU; Step S14: Obtain the data I collected at each timestamp src_i It is expressed as: I src_i = {timestamp i , w_RS_S_x i , w_RS_S_y i , w_RS_S_z i , a_RS_S_x i , a_RS_S_y i , a_RS_S_z i}; where timestamp i represents the current timestamp, i.e., the time when the message is recorded, in units of ns; w_RS_S_x i , w_RS_S_y i , w_RS_S_z i represent the xyz-axis angular velocity values collected by the inertial measurement unit at the current timestamp, in units of rad / s; a_RS_S_x i , a_RS_S_y i , a_RS_S_z i represent the three-axis linear acceleration values collected at the current timestamp, in units of m / s^2.

3. The method for autonomous navigation of an unmanned aerial vehicle under complex lighting conditions according to claim 1, characterized in that, The specific steps in step S2 include: Step S21: Use the SuperPoint network to extract the key-point heat map and 256-dimensional descriptors of the binocular images, and combine YOLOv8 to detect dynamic targets in real time; Step S22: Through the self-attention and cross-attention mechanisms of the LightGlue model, combined with relative position encoding, output a soft assignment matrix across images, and screen high-confidence feature matching pairs; Step S23: Fuse the 3D point cloud of binocular triangulation and the IMU pre-integration result, and jointly optimize the camera pose, map points, and IMU bias through the PnP algorithm and bundle adjustment.

4. The autonomous navigation method of an unmanned aerial vehicle under complex lighting conditions according to claim 3, characterized in that, The specific steps in step S23 include: Currently, it is necessary to calculate the camera pose R and t, which are represented by the Lie group as T, and K is the camera internal parameter. Specifically: where f x and f y are the focal lengths, and c x and c y are the optical center coordinates; The coordinates of the preset 3D point are P i =[[X i ,[[Y i ,[[Z i T , and the projected pixel coordinates are u i =[[u i ,[[v i T , then the relationship between the pixel position and the spatial point position is:​​ Construct a least-squares problem to solve the error of the unknown camera pose and the noise of the observation points. The error minimization formula is: where the measurement model of the angular velocity is: ω measured = ω true + b g + η g The acceleration measurement model is: a measured = a true + b a + η a where b a , b g represent the biases of acceleration and angular velocity respectively; η a , η g denotes the measurement noise, usually Gaussian noise; The IMU performs pre-integration using the basic equation of inertial navigation. For adjacent frames i and j, the relative rotation, velocity change, and position change from i to j are calculated through the IMU data.

5. The method for autonomous navigation of an unmanned aerial vehicle under complex lighting conditions according to claim 1, wherein The specific steps in step S3 include: Step S31: Convert the binocular depth map into a point cloud in the world coordinate system, update the voxel occupancy probability based on the dynamic log-likelihood ratio, and construct a probabilistic obstacle map; Step S32: Based on the dynamic priority queue and edge cost update mechanism, perform real-time search and replanning of the obstacle avoidance path in the probabilistic voxel map based on the D*lite algorithm; Among them, the D*lite algorithm is used for path finding, and a feasible path from the starting point to the target point is found using graph search; The process of path planning using the D*lite algorithm includes key functions such as calculateKey(s), initialize(), updateVertex(u), computeShortestPath(), and main();; The Calculatekey(s) function is used to calculate the priority of node s. The calculation of the priority is based on the g and rhs values of the node, combined with heuristic estimation. The priority queue sorts the nodes according to the priority.

6. The method for autonomous navigation of an unmanned aerial vehicle under complex illumination conditions according to claim 1, wherein The specific steps in step S4 include: Step S41: Parametrize the trajectory through piecewise polynomials, construct a differentiable banded linear equation system in combination with boundary conditions, and perform decoupled optimization of trajectory time and space; Among them, if the MINCO trajectory representation class is used, a multi-stage optimal control problem is constructed. The polynomial trajectory of order N = 2s - 1 is defined as follows: where \(p(t)\) is an \(N\)-th degree polynomial of \(m\) dimensions and \(M\) segments; is the coefficient matrix, \(s\) is the order of the derivative, using \(q=(q_1,\ldots,q_{M - 1})\) to represent the intermediate points, where is the zero-order derivative specified at \(t_i\); \(T=(T_1,\ldots,T_M)\) T denotes the time vector of Step S42: Design the objective function based on differential flatness, and use the L-BFGS algorithm to solve the trajectory that satisfies the thrust and state constraints; Among them, the objective function is expressed as: min(λ s J s +λ t J t +λ d J d +λ o J o ); where, J s , J t , J d , and J o represent the smooth term cost, total time cost, dynamic feasibility, and safety space cost respectively, and the weight assignments are λ s , λ t , λ d , λ o ; Step S43: Convert the acceleration and angular velocity commands of the optimized trajectory into motor speeds through a PID controller, drive the UAV to track the trajectory, and perform autonomous navigation.

Images