An indoor drone positioning, navigation and path planning method and system

Abstract

The present invention discloses an indoor UAV positioning, navigation and path planning method and system. The method includes: installing a binocular camera device and an inertial measurement unit sensor on the UAV, and acquiring binocular camera images and inertial measurement unit data of the UAV; extracting features from the binocular camera images to obtain image feature points, and establishing a time-series correspondence database of the image feature points based on the image feature points; using the time-series correspondence database of the image feature points and the inertial measurement unit data to estimate the pose of the UAV; constructing a 3D space through the image feature points, and constructing a local environment map of the constructed 3D space using the image feature points; the UAV performs path planning according to the local environment map to generate a UAV flight path; and optimizing the pose of the UAV based on the UAV flight path and the visual semi-direct method to obtain the final pose of the UAV. By calculating an optimal path and optimizing the pose of the UAV, the estimation error of the UAV position is reduced.

Description

Technical Field

[0001] The present invention relates to the technical field of unmanned aerial vehicles, and particularly to an indoor unmanned aerial vehicle positioning, navigation and path planning method and system. Background Art

[0002] With the continuous development of unmanned aerial vehicle technology, especially its application in indoor environments, the positioning and navigation of indoor unmanned aerial vehicles have become an important research direction.

[0003] Traditional indoor positioning methods mainly rely on ground sensors, lidar or ultrasonic sensors, but these methods often face many challenges in practical applications, such as high deployment difficulty and high cost.

[0004] In recent years, the unmanned aerial vehicle positioning method based on visual information has gradually become one of the mainstream technologies for indoor unmanned aerial vehicle positioning and navigation due to its low cost, light weight, strong adaptability to unknown environments, and high flexibility.

[0005] The fusion method based on binocular cameras and inertial measurement units (IMUs) has been widely used in unmanned aerial vehicle positioning and navigation.

[0006] Binocular cameras can obtain three-dimensional depth information of the environment through the principle of stereo vision, and thus provide accurate spatial perception for unmanned aerial vehicles. Especially in indoor environments without GPS signals, binocular vision provides an important basis for unmanned aerial vehicle positioning.

[0007] The inertial measurement unit (IMU) provides dynamic information such as acceleration, angular velocity, and direction of the unmanned aerial vehicle through sensors such as accelerometers and gyroscopes, and can effectively supplement the deficiencies of visual information. Especially in high-dynamic or partially occluded environments, the IMU can provide real-time attitude estimation and motion state.

[0008] In terms of path planning, existing path planning technologies mainly rely on map information of the environment, and calculate an optimal path by identifying obstacles and key structures.

[0009] Traditional path planning algorithms include the Dijkstra algorithm, the RRT (Rapidly-Exploring Random Tree) algorithm, etc.

[0010] However, in complex indoor environments, the layout of obstacles is irregular, and the real-time performance and accuracy of path planning are still problems to be solved urgently. Summary of the Invention

[0011] The purpose of the present invention is to provide an indoor unmanned aerial vehicle positioning, navigation and path planning method and system, which solves the above-mentioned technical problems pointed out in the prior art.

[0012] The present invention provides an indoor drone positioning, navigation, and path planning method, including the following operating steps:

[0013] Install a binocular camera device and an inertial measurement unit sensor on the drone, and obtain binocular camera images and inertial measurement unit data of the drone;

[0014] Extract image feature points from the binocular camera images, and establish a time-series correspondence database of the image feature points based on the image feature points;

[0015] Estimate the pose of the drone by using the time-series correspondence database of the image feature points and the inertial measurement unit data;

[0016] Construct a 3D space through the image feature points, and construct a local environment map of the constructed 3D space by using the image feature points; The drone performs path planning according to the local environment map to generate a drone flight path;

[0017] Optimize the pose of the drone based on the drone flight path and the visual semi-direct method to obtain the final pose of the drone.

[0018] Compared with the prior art, the embodiments of the present invention have at least the following technical advantages:

[0019] Analyzing the above indoor drone positioning, navigation, and path planning method provided by the present invention, it can be seen that in specific applications, a binocular camera device and an inertial measurement unit sensor are installed on the drone, and binocular camera images and inertial measurement unit data of the drone are collected and acquired, so that the depth information of each point in the scene can be calculated, the three-dimensional structure of the environment can be obtained, and the inertial measurement unit data can provide the real-time attitude information of the drone, which is very important for indoor drone navigation;

[0020] Furthermore, for the binocular camera images, the FAST corner detection algorithm can be used to extract image feature points. The extraction not only helps to identify key regions in the environment, but also can find out which image feature points are the same or corresponding in the images taken at different time points, so as to associate the same image feature points in the front and back frames of images, generate a time series relationship, and thus record the trajectory of the feature points changing with time;

[0021] Furthermore, estimate the pose of the drone by using the time-series correspondence database of the image feature points and the inertial measurement unit data. By estimating the pose of the drone, the positioning accuracy of the drone can be optimized, and thus the change of the pose of the drone can be estimated;

[0022] Furthermore, a 3D space is constructed based on the image feature points between consecutive image frames in the correspondence database. The 2D coordinates of each image feature point are converted into 3D space coordinates, and a point cloud is generated using these 3D space coordinates, so as to generate features such as the environmental distribution indoors using the point cloud. The 3D space is divided into multiple regular cubic grids, and the point cloud density is calculated for the point cloud within each grid area. The grid occupancy probability is calculated for the grid area based on the point cloud density. The occupancy probability of each grid reflects the possibility of the existence of an object in that area, thereby determining whether there is an object in the grid area. By the occupancy probability of each grid area, it is determined which grid area may have a higher object probability, so that a local environmental map can be constructed based on these higher occupancy probabilities of the object probabilities. And further, the RRT algorithm is used to sample and plan a path for the local environmental map. Each time a sample is taken, the sampling point is selected according to the grid occupancy probability of the grid area in the 3D space and the target position coordinates. The sampling point must not be in the obstacle area, that is, the grid area with a lower point cloud density, so as to avoid obstacles and enable the UAV to fly safely;

[0023] Furthermore, the adaptive step size of the search space of the local environmental map is calculated according to the environmental characteristics of the local environmental map and the grid occupancy probability of each grid area in the local environmental map. The path nodes are determined for the sampling points of the UAV according to the adaptive step size, so as to find the node closest to the flight target and avoid collisions with obstacles on the way. And the acceleration factor and attitude adjustment value are optimized for the adaptive step size, so that the final adaptive step size can avoid collisions with obstacles in advance during the actual flight of the UAV, which is an optimized safe step size. And the expansion direction is calculated through the environmental feature gradient, the occupancy probability gradient of the grid area, and the target direction. The expansion direction enables the UAV to screen the path nodes for the directional sampling points of the target end point, so as to find a fast and safe flight path. The sampling points of the new grid area coordinates are generated from the sampling points of the current grid area coordinates through the adaptive step size and the expansion direction. The new sampling points can avoid collisions with some obstacles. And through the preset occupancy probability threshold v, the grid occupancy probability of the sampling points of the new grid area coordinates is verified for the occupancy probability, so as to use the valid sampling points as path nodes, indicating that there are no obstacles in the path of the new sampling node and it can reach the target position coordinates;

[0024] Furthermore, effective sampling points are iteratively generated through the adaptive step size and the expansion direction. All the effective sampling points are used as path nodes to generate a sequence of path nodes, and the path nodes in the sequence of path nodes are connected. It is judged whether there are obstacles in the sampling points on the connected path, so as to find a shortest and safe UAV flight path;

[0025] Further, the flight path of the drone and the visual semi-direct method are used to optimize the pose of the drone, and the final pose of the drone is obtained; the visual semi-direct method is combined with the flight path of the drone to reduce the error of the position estimation of the drone, ensuring that the drone can fly along the predetermined path as accurately as possible. Description of the Drawings

[0026] Figure 1 It is an overall flowchart of an indoor drone positioning and navigation path planning method;

[0027] Figure 2 It is a flowchart of the drone flight trajectory;

[0028] Figure 3 It is a schematic diagram of the grid occupancy probability of the point cloud density in the grid area;

[0029] Figure 4 It is a flowchart of the sampling points;

[0030] Figure 5 It is a specific flowchart of the path node generating the drone flight trajectory;

[0031] Figure 6 It is a flowchart of screening path node iteration;

[0032] Figure 7 It is a schematic diagram of screening path node iteration;

[0033] Figure 8 It is a flowchart of the indoor drone positioning and navigation path planning system.

[0034] Marking: Data acquisition module 10; Database module 20; Estimated pose module 30; Flight path module 40; Optimized pose module 50. Specific Embodiments

[0035] Next, the technical solution of the present invention will be clearly and completely described in conjunction with the drawings. Obviously, the described embodiments are part of the embodiments of the present invention, rather than all of the embodiments.

[0036] Next, the present invention will be further described in detail through specific embodiments in conjunction with the drawings.

[0037] Embodiment 1

[0038] As Figure 1 shown, the present invention proposes an indoor drone positioning and navigation path planning method, including the following operating steps:

[0039] S1: Install a binocular camera device and an inertial measurement unit sensor on the drone, and obtain binocular camera images and inertial measurement unit data for the drone;

[0040] It should be noted that a binocular camera device and an inertial measurement unit sensor are installed on the drone; the binocular camera consists of two cameras, usually two cameras that are parallel to each other and separated by a certain distance; by simultaneously capturing images of the same scene with the two cameras and using the principle of stereo vision, the depth information of the objects in the scene, that is, the distance between the camera and the objects, can be calculated; compared with a monocular camera, the binocular camera can obtain richer three-dimensional information, thus providing precise spatial perception for the drone, which is very important for tasks such as indoor drone obstacle avoidance and map construction; while the inertial measurement unit (IMU) is a common sensor that includes sensors such as an accelerometer, a gyroscope (and possibly a magnetometer), etc., and is used to measure information such as the acceleration, angular velocity, and direction of the drone. The IMU can provide dynamic information of the drone, such as attitude, speed, acceleration, etc.

[0041] During the process of obtaining the binocular camera images and inertial measurement unit data of the drone through the binocular camera device and the inertial measurement unit sensor, the data of the binocular camera and the IMU must be synchronously processed; because the visual data and the IMU data usually have different sampling frequencies and timestamps, therefore, it is necessary to perform time synchronization on the data of these two sensors (that is, the binocular camera device and the inertial measurement unit sensor) to ensure that they can accurately correspond to the environmental information at the same moment.

[0042] The obtained binocular camera images can be matched and triangulated to calculate the depth information of each point in the scene and obtain the three-dimensional structure of the environment (such as the distance and position of walls, obstacles, etc.); this is very important for tasks such as indoor drone obstacle avoidance and map construction; at the same time, the inertial measurement unit data may include the attitude information and motion state of the drone, etc., and can provide the real-time attitude information of the drone (pitch angle, roll angle, and yaw angle). For indoor drone navigation, attitude estimation is very important, especially in indoor environments where GPS signals cannot be used, and the motion state in the IMU data can help calculate the speed and acceleration of the drone and provide instant dynamic feedback.

[0043] S2: Extract image feature points from the binocular camera images, and establish a time-series correspondence database of the image feature points based on the image feature points.

[0044] It should be noted that for binocular camera images, the FAST corner detection algorithm can be used to extract image feature points; further, Gaussian filtering is performed on the binocular camera images to smooth and construct pyramid images, which are divided into the left camera image and the right camera image of the binocular camera images; the extracted image feature points are mapped to the image pixels of each layer in the pyramid images, and the position coordinates and description information (i.e., feature descriptors) of the image feature points in each layer of the pyramid are recorded. Starting from the top layer of the pyramid, the position coordinates and description information of the image feature points in the left image pyramid and the right image pyramid are compared and matched to obtain the recognized image feature points, and a time-series correspondence database is established based on the coordinates, description information, etc. of the recognized image feature points;

[0045] In the processing of binocular camera images, image feature points are local regions with significant information in the images. Usually, these points can remain stable under different perspectives and lighting conditions. By extracting the feature points in the images, a description of the environment can be obtained for subsequent positioning and navigation; the extraction of image feature points not only helps to identify key regions in the environment but also can be used for tasks such as image matching, image stitching (map construction), and motion estimation (visual odometry);

[0046] The FAST corner detection algorithm is a very classic feature extraction method. It detects feature points by finding local extreme points in the images, and these feature points have scale and rotation invariance, which are suitable for describing objects under different perspectives;

[0047] After extracting the image feature points, it is necessary to match the image feature points between subsequent image frames, that is, to find out which image feature points in the images taken at different time points are the same or corresponding (i.e., whether the coordinates and descriptors of the image feature points of the left camera image and the right camera image of the binocular camera images match; for each pair of images, by calculating the similarity of the feature points between them (usually measured by the Euclidean distance or Hamming distance between the descriptors), to determine which feature points are in one-to-one correspondence between the images), and the image feature points are used to construct a time-series correspondence database based on information such as coordinates, descriptors, and time; the construction of the time-series correspondence database means that for each image feature point (such as the image feature point extracted from the nth frame image), it can be associated with the same image feature points in the previous and subsequent frames of images to generate a time-series relationship, thereby recording the trajectory of the feature points changing over time;

[0048] S3: Estimate the pose of the UAV by using the time-series correspondence database of the image feature points and the data of the inertial measurement unit;

[0049] It should be noted that the database of the temporal correspondence relationship of image feature points records the feature point matching between each pair of frames of images. The information of these feature point matchings helps the UAV system understand the relative motion between adjacent frames, and then calculate parameters such as displacement and rotation. Combined with the data of the inertial measurement unit (i.e., in a complex indoor environment, obstacles and occlusions will affect the matching of visual features. Therefore, through the data compensation and correction of the IMU, the UAV system can still maintain high-precision positioning in an environment with unclear features), the instantaneous linear velocity and angular velocity of the UAV can be calculated, further optimizing the positioning accuracy of the UAV, so as to estimate the change in the pose of the UAV (i.e., the real-time position and state).

[0050] S4: Construct a 3D space through image feature points, and use the image feature point pairs to construct a local environmental map for the constructed 3D space. The UAV conducts path planning according to the local environmental map to generate the flight path of the UAV.

[0051] It should be noted that the visual data obtained through the binocular camera (i.e., the real-time image data captured) and the extracted image feature points can capture the key structures and obstacles in the current environment (i.e., the indoor house type structure and the layout distribution of obstacles such as desks, chairs, and benches). In this process, the visual odometer is used to estimate the current pose of the UAV camera, and based on the image feature point matching between consecutive image frames, the positions and shapes of the objects in the environment are determined, thereby constructing a local environmental map of the indoor area. The UAV system plans the path of the UAV according to the key structures and obstacles in the local environmental map (i.e., the goal of path planning is to start from the current position, find an optimal or feasible path, avoid obstacles and reach the target position coordinates, thereby generating a flight path of the UAV), thereby generating the flight path of the UAV.

[0052] S5: Optimize the pose of the UAV based on the UAV flight path and the visual semi-direct method to obtain the final pose of the UAV.

[0053] It should be noted that path planning provides an ideal flight trajectory or target route (i.e., the UAV flight path). The pose optimization of the UAV will combine this path to reduce the error of position estimation. In the optimization stage, the goal of pose optimization is to adjust the current estimated position and attitude of the UAV to ensure that the UAV can fly along the predetermined path as accurately as possible.

[0054] The pose optimization not only depends on visual information (i.e., the real-time image data captured), but also on the heading information provided by path planning (i.e., the information about the flight direction of the UAV flight path). The goal of the optimization process is to correct errors and reduce pose drift through means such as the Visual SemiDirect Method, ensuring smoother and more precise flight. The Visual SemiDirect Method is a commonly used technique in the fields of computer vision and robotics, mainly used to estimate the relationship between the camera pose and the three-dimensional scene. It combines the advantages of the direct method and the feature method to optimize the pose of the UAV and obtain the final pose;

[0055] Specifically, as Figure 2 shown, in step S4, a 3D space is constructed through image feature points, and a local environmental map is constructed using the image feature points for the constructed 3D space; the UAV conducts path planning based on the local environmental map to generate the UAV flight path. The specific operation steps are as follows:

[0056] S41: Construct a 3D space (i.e., three-dimensional space) for the image feature points between consecutive image frames in the temporal correspondence database, determine the pixel coordinates (i.e., 3D space coordinates) for each image feature point in the 3D space, and generate a corresponding point cloud using the pixel coordinates;

[0057] It should be noted that using the stereo vision principle of a binocular camera, combining the two-dimensional feature points in the image with the internal and external parameters of the camera (such as focal length, relative position, etc.), and using the parallax (i.e., the pixel position difference of the same feature points in two images) to calculate the 3D space coordinates of these image feature points (that is, extract feature points from the 2D image, calculate the parallax of these feature points, and use the camera internal parameters to calculate the depth, converting the 2D coordinates of each feature point into 3D space coordinates);

[0058] Generate a point cloud using these 3D space coordinates (i.e., output the set of these 3D space coordinate points as a point cloud); a point cloud is a set of discrete points in three-dimensional space that can describe the shape and structure of objects in the environment; the generated point cloud will represent the structure of objects or scenes in the environment, and these point clouds provide convenience for subsequent map construction and path planning, etc.;

[0059] S42: Divide the 3D space into multiple regular cubic grids, and each grid is represented as a region (i.e., grid region);

[0060] Calculate the point cloud density for the point cloud within each grid region, calculate the grid occupancy probability for the grid region based on the point cloud density, and determine whether there are objects (i.e., including obstacles such as tables, chairs, benches, etc.) in the grid region through the grid occupancy probability of each grid region;

[0061] And construct a local environment map according to the grid occupancy probability of the point cloud density in each grid area within the 3D space;

[0062] It should be noted that based on the generated point cloud, the system divides the 3D space (i.e., three-dimensional space) into multiple regular cubic grids, and each grid represents a small area; and calculates the number of point clouds in each grid area to check the point cloud density within the grid; the occupancy probability of each grid reflects the possibility of the existence of objects in this area. A high grid occupancy probability means that there is a high probability of obstacles or objects in this area, and a low grid occupancy probability means that this area is relatively empty (that is, the grid occupancy probability is calculated according to the range occupied by the point cloud density in each grid area to determine whether the grid occupancy probability is high or low, as Figure 3 shown);

[0063] If a grid contains a dense point cloud, it indicates that there are objects in this area and the probability of the existence of obstacles is relatively high; if a grid has almost no point cloud, it means that this area is empty and the probability of the existence of objects is relatively low, as Figure 3 shown;

[0064] By judging the occupancy probability of each grid area, it is possible to determine which grid area may have a higher object probability. In this way, a local environment map can be constructed through these object probabilities with higher occupancy probabilities to reflect the distribution of obstacles in the environment, and the path planning can also be guided by the occupancy probability;

[0065] S43: Extract environmental features based on the local environment map; the environmental features include edge features, plane features, and corner features; it should be noted that an environmental feature set y is extracted from the local environment map, and the environmental feature set Y includes edge features E (that is, edge feature E: extract the linear structures in the environment, such as the edge of a wall, door frame, etc.), plane features S (that is, plane feature S: extract the planar structures in the environment, such as walls, floors, etc.), and corner features C (that is, corner feature C: extract the corner structures in the environment, such as the corner of a wall, corner, etc.);

[0066] Describe the extracted environmental features, which may include attributes such as their position, shape, direction, size, etc.; for example, the direction of a corner or the inclination of a plane can be described; these feature descriptions are helpful for subsequent path planning, especially in complex environments to help the system understand how to bypass obstacles and pass through narrow areas, etc.; the feature description information obtained in the above steps is crucial for subsequent path planning (specific details can be seen in step S45 "Calculate the adaptive step size of the drone and determine the expansion direction through environmental features and grid occupancy probability, and the drone can identify which areas have obstacles and which areas are suitable for movement.

[0067] S44: Input the starting point coordinates and the target position coordinates (i.e., the target position coordinates are the end position of the UAV flight) into the UAV; sample and plan the path for the local environment map through the RRT algorithm. Each time of sampling, select the sampling point according to the grid occupancy probability in the grid area within the 3D space and the target position coordinates.

[0068] Plan the path through the RRT (Rapidly-Exploring Random Tree) algorithm to ensure that the UAV can reach the target safely and efficiently, thus generating an optimal UAV flight trajectory; before sampling, parameters such as the starting point, end point, and step size of the UAV need to be determined so that the RRT algorithm can be used to plan the path for the UAV and avoid the obstacles of the object.

[0069] Using the grid occupancy probability as a guide, the RRT algorithm performs random sampling in the local environment map; each time of sampling, the UAV will select the sampling point according to the grid occupancy probability (that is, the grid occupancy probability selects the grid area with a lower point cloud density, because the grid occupancy probability of the grid area with a higher point cloud density has a higher probability of having obstacles) and the target position coordinates (that is, sample the grid area with a lower point cloud density through the flight distance from the starting point to the end point to avoid obstacles). The selection of the sampling point not only considers the current position but also needs to meet certain conditions (for example, the sampling point must not be in the obstacle area, that is, the grid area with a lower point cloud density).

[0070] S45: Calculate the adaptive step size of the UAV and determine the expansion direction through the environmental features and the grid occupancy probability (that is, in the RRT (Rapidly-Exploring Random Tree) algorithm here, gradually approach the target position coordinates (i.e., the end point of the UAV flight) by gradually expanding the tree structure, and finally form a path).

[0071] Calculate the optimal path of the sampling point through the adaptive step size and the expansion direction, generate path nodes, and then obtain a sequence of path nodes through the entire iteration. Smoothly process the path nodes in the sequence of path nodes according to the RRT algorithm, connect the path nodes, and obtain the UAV flight trajectory.

[0072] It should be noted that calculate the adaptive step size (i.e., the moving distance) and the expansion direction of the UAV; the calculation of the step size and the direction usually combines the environmental features and the grid occupancy probability to ensure that the path can avoid obstacles as much as possible and is the shortest.

[0073] The RRT algorithm selects the expansion direction of the tree and the position of the new node (i.e., the position of the new sampling point) through random sampling, expands from the current sampling point (the known node of the tree) towards the subsequent sampling points, and ensures that the new sampling point is safe by checking whether the path intersects with the obstacles. If it is found that the path intersects with the obstacles during the expansion process, the sampling point is discarded and not used as a valid path node.

[0074] The valid path nodes are not sampled one point at a time, but rather multiple samples are taken to obtain multiple path nodes that meet the conditions (i.e., the path nodes are sampling points (i.e., flight paths) where multiple paths may exist. After screening the sampling points, the path nodes of the optimal path are selected as the final flight path of the UAV), and it is evaluated which sampling points are optimal (e.g., closer to the target or safer).

[0075] After calculating the path nodes, the RRT algorithm will connect the path nodes and gradually expand the path until a complete path from the starting point to the ending point is found; the RRT algorithm usually smooths the path to eliminate unnecessary corners or twists and turns, reduces the length of the path, while maintaining safety; the optimization process usually adjusts the path according to the grid occupancy probability to ensure that the path does not pass through areas with high occupancy probability, thereby avoiding collisions.

[0076] Specifically, as Figure 4 shown, in step S44, the RRT algorithm is used to sample and plan the path for the local environment map. Each time a sample is taken, the sampling point is selected according to the grid occupancy probability of the grid area in the 3D space and the target position coordinates. The specific operation steps are as follows:

[0077] S441: Each grid area contains grid position coordinates and grid occupancy probability (i.e., the probability that the grid area is occupied by an obstacle).

[0078] Calculate the sampling probability density according to each grid area in the local environment map. The sampling probability density formula is: ;

[0079] In the formula, represents the environmental feature, with a value range of [0,1]; it reflects the richness of the environmental features in the current area;

[0080] represents the target guiding term, with a value range of [0,1]; it represents the degree of proximity of the grid area to the target position coordinates. The target guiding term encourages the selection of grid areas close to the target position coordinates (i.e., the target point), thereby improving the efficiency and accuracy of path planning;

[0081] O(g) represents the grid occupancy probability, with a value range of [0,1]; it represents the probability that the grid is occupied by an obstacle, which is determined by calculating the ratio of the number of point clouds falling into the grid to the grid volume. Areas with high grid occupancy probability usually indicate obstacles, so the sampling probability density in these areas will be lower;

[0082] g represents the grid area, and (x,y,z) represents the position coordinates;

[0083] w1, w2, w3 are weighted coefficients, which are used to control the influence of each factor on the selection of sampling points;

[0084] It should be noted that the role of sampling probability density is to comprehensively consider factors such as environmental characteristics, target guidance and occupancy probability to evaluate whether a grid is suitable as a sampling point. It is related to the grid occupancy probability O(g), but it is not just the grid occupancy probability; sampling probability density also considers other factors such as environmental characteristics and target guidance; by guiding sampling to concentrate on the areas most likely to bring useful information, the sampling of unimportant or irrelevant areas is reduced; for example, if an area is already occupied by an obstacle (the occupancy probability O(g) is close to 1), there is no need to continue sampling in this area, which can significantly reduce invalid calculations and resource waste;

[0085] Sampling probability density is not only used for sampling, but also can be applied to tasks such as path planning and navigation decision-making. By calculating the sampling probability density, the UAV system can choose to prioritize areas with higher sampling probability for path search when planning the path, thereby speeding up the process of finding a suitable path. If the grid occupancy probability is high, indicating that the area is occupied by obstacles, the system can avoid unsafe paths by reducing the sampling of these areas.

[0086] S442: traverse each grid area g in the local environment map, and calculate the sampling probability density of each grid area g;

[0087] S443: Based on the sampling probability density, the acceptance score Q of each grid area g in the local environment map is calculated (when sampling and planning the path for the local environment map, multiple sampling points will be generated on the connecting segment of the path, and the distribution of these sampling points will depend on the occupancy probability of the grid; for example, if the area with a high grid occupancy probability is not suitable for selecting sampling points, the sampling probability will also be low to avoid the path crossing the obstacle area. Therefore, based on the sampling probability density, the acceptance score Q of each grid area g is calculated at the same time to specifically determine which grid areas can be sampled). The calculation formula of the acceptance score Q is: ;

[0088] In the formula, It is expressed as the Laplace operator of environmental characteristics. It is calculated by the second-order partial derivative of three-dimensional space, which reflects the rate of change of environmental characteristics in the local environmental map. The larger the value of the environmental characteristic, the more drastic the characteristic change of the grid area, and the more suitable it may be for sampling or planning.

[0089] G represents the Euclidean distance from the current grid area coordinates to the target location coordinates;

[0090] α and β are parameters that control the feature response and distance influence; α is the feature gain coefficient, usually set to 1.5 to control the influence intensity of environmental features on scoring; β is the distance attenuation coefficient, used to control the influence degree of the target distance of the target position coordinates on scoring;

[0091] The calculation formula for the Euclidean distance G from the current grid area coordinates to the target position coordinates is: ;

[0092] In the formula, (xg, yg, zg) represents the current grid area coordinates;

[0093] (xG, yG, zG) are the target position coordinates;

[0094] If the Euclidean distance G is smaller, it means that the grid area is closer to the target position coordinates, and the priority of the sampling point should be higher;

[0095] It should be noted that the sampling probability density determines the selection of the collected sampling points, and the acceptance score q determines whether to accept the sampling point according to the relative probability of the current state and the new sampling point;

[0096] S444: Preset a scoring threshold R; judge whether the acceptance score Q of each grid area is greater than or equal to the scoring threshold R;

[0097] If not, discard the grid area;

[0098] If so, use this grid area as a candidate sampling point; and traverse each grid area, and store the grid areas that meet the conditions as candidate sampling points in the sequence list E;

[0099] S445: Use the RRT algorithm to screen the candidate sampling points of the grid areas in the sequence list E again according to the target guidance item G(x, y, z), and select the grid area closest to the target position coordinates as the final sampling point;

[0100] It should be noted that the goal of the entire scheme is to use the sampling probability density to guide the selection of appropriate sampling points; this process comprehensively considers environmental features, target guidance, and grid occupancy probability, and finally selects a suitable point as the sampling point for path planning or map construction through a reasonable weighting and scoring mechanism;

[0101] Specifically, in one of the implementation manners, the adaptive step size of the drone is calculated through environmental features and grid occupancy probability. The specific operation steps are as follows: Calculate the adaptive step size of the search space of the local environmental map according to the environmental features of the local environmental map and the grid occupancy probability of each grid area in the local environmental map;

[0102] Optimize the adaptive step size and preset a safety distance between the UAV and the sampling point target Calculate the stepping distance between the UAV and the sampling point target based on the distance between the current UAV and the sampling point target and the preset safety distance;

[0103] Calculate the acceleration factor of the UAV based on the preset safety distance;

[0104] Calculate the attitude adjustment value of the UAV based on the distance between the current UAV and the sampling point target and the preset safety distance;

[0105] Adjust the stepping distance based on the acceleration factor and the attitude adjustment value of the UAV to obtain the adaptive stepping distance; take the adaptive stepping distance as the final adaptive step size.

[0106] Specifically, in one of the embodiments, determine the expansion direction, and the specific operation steps are as follows: Calculate the partial derivative of the environmental features according to the central difference method to obtain the environmental feature gradient;

[0107] Calculate the partial derivative of the grid occupancy probability of the grid area according to the central difference method to obtain the occupancy probability gradient of the grid area;

[0108] Calculate the target direction based on the sampling point of the current grid area coordinates and the target position coordinates; calculate the expansion direction through the environmental feature gradient, the occupancy probability gradient of the grid area, and the target direction.

[0109] The following is an explanation of the above two specific embodiments. As Figure 5 shown, in step S45, calculate the adaptive step size of the UAV and determine the expansion direction through the environmental features and the grid occupancy probability; then calculate the optimal path of the sampling point through the adaptive step size and the expansion direction, generate path nodes, and then obtain a sequence of path nodes through the entire iteration. Smooth the path nodes in the sequence of path nodes according to the RRT algorithm and connect the path nodes to obtain the UAV flight trajectory. The specific operation steps are as follows:

[0110] S451: Calculate the adaptive step size of the search space of the local environmental map according to the environmental features of the local environmental map and the grid occupancy probability of each grid area in the local environmental map. The adaptive step size formula is: ;

[0111] In the formula, h represents the basic step size; represents a preset and conventional step size, usually set according to the scale of the environment (i.e., the step size when the environmental features or obstacle effects are not considered);

[0112] α is expressed as the feature influence coefficient; it controls the gain effect of environmental feature distribution (such as terrain, obstacle density, etc.) on the step length. The environmental feature distribution D(x, y, z) will affect the adjustment size of the step length. If the environmental feature changes greatly, the step length will be adjusted correspondingly larger (that is, when the environmental feature changes greatly, it indicates that the surrounding environment is more complex and the obstacles are more dense, so the step length is increased to cross this area and search for a location without obstacles in a farther area, so the step length is adjusted).

[0113] β is expressed as the occupancy probability influence coefficient; it controls the inhibitory effect of obstacles on the step length. The grid occupancy probability O(g) will affect the step length to avoid the step length being too large and crossing obstacles.

[0114] D(x, y, z) is expressed as the environmental feature;

[0115] O(g) is expressed as the grid occupancy probability; it represents the occupancy state of the grid area where the current position is located. If O(g) is large, it means that there are obstacles in this grid area and the step length will also be reduced (that is, when there are obstacles in a grid area and no obstacles appear around, the step length is reduced to avoid obstacles in advance).

[0116] S452: Optimize the adaptive step length; preset a safety distance between the UAV and the sampling point target , and the preset safety distance is the minimum safety distance and the preset maximum safety distance ;

[0117] Calculate the stepping distance between the UAV and the sampling point target through the preset safety distance. The calculation formula is: ;

[0118] In the formula, is the minimum stepping distance to ensure that the UAV can still move even when it is very close to the sampling point target;

[0119] c is the basic stepping distance. When the UAV is far from the target, a larger stepping distance is used;

[0120] u is the attenuation factor (that is, the attenuation factor often adopts the form of exponential attenuation. For example, where T is the current distance from the target, is the minimum distance, and a is the constant of the attenuation factor; as the UAV approaches the target, the attenuation factor gradually decreases and the stepping distance also decreases);

[0121] T is the current distance between the UAV and the sampling point target;

[0122] e is expressed as a constant;

[0123] Through the above formula, when searching for the sampling point step size, it should be considered that the UAV avoids the sampling points where there may be obstacles during flight, so a preset safety distance is set between the UAV and the sampling point; when the safety distance is large, a larger step size is used; when the safety distance is small, a smaller step size can be adopted to avoid the UAV approaching the obstacle too quickly, so the adaptive step size needs to be further optimized;

[0124] S453: Calculate the acceleration factor of the UAV through the preset safety distance, and the calculation formula is: ;

[0125] In the formula, a is the acceleration factor of the UAV;

[0126] represents the minimum acceleration of the UAV;

[0127] represents the maximum acceleration of the UAV;

[0128] Calculate the attitude adjustment value of the UAV through the distance between the current UAV and the sampling point target and the preset safety distance, and the calculation formula is: ;

[0129] In the formula, represents the current attitude adjustment value; represents the initial attitude adjustment value, which refers to the attitude adjustment amount of the UAV at a relatively long distance; a is the attenuation factor;

[0130] T is the distance between the current UAV and the sampling point target;

[0131] It should be noted that acceleration determines the speed change ability of the UAV and affects its movement speed and responsiveness. A larger acceleration means that the UAV can start, accelerate or adjust the direction quickly, while a smaller acceleration means that its response is relatively gentle; when moving at high speed or making rapid adjustments, the UAV may need to increase the stepping distance to avoid staying at the current position for a long time; when the acceleration is low, the stepping distance may be reduced to prevent sudden rapid movement from causing control instability or collision;

[0132] The attitude adjustment value refers to the adjustment made in the control system according to the current attitude of the UAV (such as pitch angle, roll angle, yaw angle); these values reflect the stability and direction control of the UAV; the attitude adjustment value usually affects the size of the stepping distance. When the attitude deviation of the UAV is large (for example, there is a large yaw or pitch), the stepping distance may be reduced to reduce the impact of the attitude error on the overall movement; if the attitude adjustment value indicates that the UAV is in a stable flight state, the stepping distance can be increased to allow it to move forward faster, so that the adaptive step size can be better optimized;

[0133] S454: Adjust the step distance according to the acceleration factor and attitude adjustment value of the drone to obtain an adaptive step distance; use the adaptive step distance as the final adaptive step size;

[0134] The calculation formula for the adaptive step distance is: ;

[0135] In the formula, is the acceleration adjustment factor of the drone; adjust the step distance. If the speed of the drone relative to the obstacle is fast, the step distance will be correspondingly reduced, allowing the drone to have sufficient time to avoid the obstacle;

[0136] k is a constant; represents the object target influence factor (i.e., the obstacle influence factor), which is used to adjust the step distance of the drone when approaching the obstacle. If the obstacle is relatively close, the step distance will be reduced to avoid collision; is the safety distance, which is used to define when to decelerate or avoid contact with the obstacle; ensure that during the obstacle avoidance process, the step distance will not be too large, resulting in collision or misjudgment;

[0137] It should be noted that in the adaptive step size control, the acceleration and attitude adjustment value act together on the calculation of the step distance, enabling the drone to have a safety distance from the obstacle in the dynamic adaptation environment during flight and be able to avoid the obstacle; the acceleration can affect the increase or decrease of the step distance, especially providing the ability of dynamic adjustment when quick response is required to avoid the obstacle; the attitude adjustment value ensures that the drone can maintain stability in different flight postures and adjusts the step distance to cope with the instability of the posture, so that the adaptive step size can be optimized. In practice, the drone can avoid obstacles, find suitable path nodes for the sampling points, and find a safe flight path;

[0138] S455: Calculate the partial derivative of the environmental feature according to the central difference method to obtain the environmental feature gradient;

[0139] Calculate the partial derivative of the grid occupancy probability of the grid area according to the central difference method to obtain the occupancy probability gradient of the grid area;

[0140] Calculate the target direction according to the sampling point of the current grid area coordinates and the target position coordinates. The calculation formula is: ;

[0141] In the formula, (xg, yg, zg) represents the sampling point of the current grid area coordinates;

[0142] (xG, yG, zG) is the target position coordinate (i.e., when calculating the Euclidean distance from the current grid area coordinate to the target position coordinate in step S443, two formula factors of the sampling point of the current grid area coordinate and the target position coordinate have been obtained, so the target direction from the sampling point of the current grid area coordinate to the target position coordinate can be directly calculated);

[0143] Calculate the expansion direction through the environmental feature gradient, the occupancy probability gradient of the grid area, and the target direction. The calculation formula is: ;

[0144] In the formula, normalize() represents a normalization function (i.e., used to ensure that the length of the result direction vector is within a reasonable range);

[0145] w1, w2, w3 represent weighted coefficients; used to balance the influence of the three directions on the expansion (i.e., the environmental feature gradient, the target direction, and the occupancy probability gradient of the grid area);

[0146] represents the environmental feature gradient; indicating the degree of change of the environmental feature in each direction;

[0147] represents the occupancy probability gradient of the grid area; indicating the degree of change of the occupancy state of the current grid area in each direction;

[0148] represents the target direction from the sampling point of the current grid area coordinate to the target position coordinate (i.e., the vector indicating the direction of the target position);

[0149] S456: Generate a new sampling point of the grid area coordinate from the sampling point of the current grid area coordinate through the adaptive step size and the expansion direction;

[0150] It should be noted that the expansion direction is determined according to the sampling point of the current grid area coordinate, and a new sampling point is generated according to the adaptive step size. The new sampling point can avoid collisions with some obstacles;

[0151] S457: Preset an occupancy probability threshold v, and perform an occupancy probability verification on the grid occupancy probability of the new sampling point of the grid area coordinate to determine whether the grid occupancy probability of the new sampling point of the grid area coordinate is less than the occupancy probability threshold v;

[0152] If not, the sampling point of the new grid area coordinate is determined to be in the object area of the local environmental map, and a new sampling point of the grid area coordinate is reselected;

[0153] If so, the sampling point of the new grid area coordinate is a valid sampling point and is used as a path node;

[0154] It should be noted that the preset occupancy probability threshold v is used to judge whether there is an obstacle in the grid occupancy probability of the sampling point of the new grid area coordinates; if there is an obstacle, it means that there is a high probability of an obstacle at the new sampling point, and it is necessary to regenerate the sampling point of the new grid area coordinates according to the adaptive step size and the expansion direction; if there is no obstacle, the new sampling point is used as a path node, indicating that there is no obstacle in the path of the new sampling node and it can reach the target position coordinates.

[0155] S458: Iteratively generate valid sampling points through the adaptive step size and the expansion direction, and use all valid sampling points as path nodes to generate a path node sequence, where the path node sequence = {g1, g2, g3, g4..., gn}.

[0156] It should be noted that through iteration, all generated path nodes are used to generate a path node sequence. {g1, g2, g3, g4..., gn} is equivalent to the complete path from the starting point of the current grid area coordinates to the target position coordinates. This complete path is composed of a series of path nodes; for example, the path nodes may include nodes g1, g2, g3, g4..., gn; each path node on the flight path is stored in a set, and these path nodes form a preliminary path (i.e., the preliminary path is equivalent to the unoptimized UAV flight path, and there may be a phenomenon of detouring between two nodes (i.e., there is an obstacle in the grid area of the sampling point (or path node) between two path nodes), and a straight flight cannot be performed between two path nodes); in the expansion step of step S459, the path nodes of the preliminary path are smoothed, and the optimal path nodes are selected to form the final UAV flight path with reduced flight time and reduced collisions.

[0157] S459: Smooth the path nodes of the path node sequence through the RRT algorithm, check the connection lines between the path nodes, and connect the path nodes that can be connected between the path nodes to obtain the UAV flight trajectory.

[0158] It should be noted that by using the RRT algorithm to smooth the path nodes of the path node sequence, unnecessary turns in the path can be reduced, enabling the UAV to travel in a more stable and efficient manner, avoiding unnecessary collisions, and shortening the path as much as possible; check whether there are obstacles of other sampling points on the path of the connection line segment between the path nodes, such as g1, g2, g3, g4..., gn, connect the path nodes g1 and g2 in a straight line, and judge whether there are obstacles in all sampling points on this straight path. Through screening, the role of smoothing can be achieved, and the path nodes that meet the best path can also be screened out. By connecting all the path nodes of the best path in series, the UAV flight trajectory is formed.

[0159] Specifically, as Figure 6 shown, in step S459, the path nodes of the path node sequence are smoothed by the RRT algorithm, the connection lines between the path nodes are checked, and the path nodes that can be connected between the path nodes are concatenated to obtain the UAV flight trajectory. The specific operation steps are as follows:

[0160] S4591: Connect the path node g1 and the path node g3 in the path node sequence linearly to form a path segment; judge whether the grid occupancy probability of each sampling point on the path segment is less than the occupancy probability threshold v (that is, the judgment of whether there are obstacles at the sampling points on this path segment can refer to step S457, and the occupancy probability threshold v is a preset threshold in step S457);

[0161] If not, there are objects at the sampling points on the path segment between the path node g1 and the path node g3 (that is, the objects include obstacles such as tables, chairs, and benches), and the path segment between the path node g1 and the path node g3 is discarded;

[0162] If so, the path segment between the path node g1 and the path node g3 is retained to obtain a new path node sequence = {g1, g3, g4,..., gn};

[0163] It should be noted that in order to smooth the path nodes, screen out the optimal path nodes, directly connect between the path nodes, and judge whether there are obstacles in the grid occupancy probability of all sampling points on the connection line segment;

[0164] If the grid occupancy probability O(q) of a certain sampling point on the straight line segment is greater than or equal to the occupancy threshold v, then the sampling point on the straight line segment is located in the obstacle, indicating that the path cannot be directly connected between p1 and p3 and there is a collision, so the path segment is discarded and the path is re-planned;

[0165] If the grid occupancy probability O(q) of all sampling points on the straight line segment is lower than the occupancy probability threshold v, it means that the path is feasible and g1 and g3 can be directly connected, then the path node g2 (that is, the middle point of the path, saving the UAV flight path, as Figure 7 shown) can be removed, and thus it becomes the new path g1, g3, g4,..., gn;

[0166] S4592: Connect the path node g1 and the path node g4 in the new path node sequence = {g1, g3, g4,..., gn} linearly to form a path segment, and repeat the above step S4561 until the path node closest to the target position coordinates among the path nodes g1 is selected, and update the path segment of the retained path nodes g1 and g3;

[0167] It should be noted that when the path segment between path node g1 and path node g3 is an optimized path, it is used as a reserved path segment; at this time, it is necessary to judge whether there are obstacles at the sampling points on the path segment between path node g1 and path node g4 (that is, here it is also the same as step S4591 to judge whether there are obstacles at the sampling points on the path segment, so it will not be elaborated again). When there are no obstacles at the sampling points on the path segment between path node g1 and path node g4, the reserved path segment between path node g1 and path node g3 is updated, the path segment between path node g1 and path node g4 is reserved, and so on, until obstacles appear on the path segment; in this way, the distance to the path node closest to the target position coordinates of g1 can be found, reducing the flight time of the drone and making the path node reach the maximum smoothness;

[0168] S4593: Use the path nodes of the updated reserved path segment as a new starting point to continue the recursive operations of steps S4591 - S4592 to obtain all optimized path nodes;

[0169] Connect all the optimized path nodes to obtain the flight trajectory of the drone;

[0170] It should be noted that after the reserved path segment in step S4592 is updated, recursive operations are performed on the new path nodes. For example: if the path segment between the updated reserved path nodes g1 and g3 is updated, the new path node sequence = {g1, g4,..., gn}. At this time, a straight connection is made between path node g1 and path node g5. If there are obstacles on the path segment between path node g1 and path node g5, the update is stopped because it indicates that there are obstacles at path node g5 and the drone cannot fly. Therefore, it is necessary to stop continuing to select the route and continue to perform the recursive operations of steps S4591 - S4592 on path node g4 and path node g6, as Figure 7 shown, until all the optimal path nodes are selected to form the flight trajectory of the drone to ensure the flight safety of the drone;

[0171] Embodiment 2

[0172] As Figure 2As shown in the figure, Embodiment 2 of the present invention also proposes an indoor UAV positioning and navigation path planning system, including: a data acquisition module 10; a database module 20; an estimated pose module 30; a flight path module 40; an optimized pose module 50; the data acquisition module 10 is used to install a binocular camera device and an inertial measurement unit sensor on the UAV, and obtain binocular camera images and inertial measurement unit data of the UAV; the database module 20 is used to extract features from the binocular camera images to obtain image feature points, and establish a temporal correspondence relationship database of the image feature points based on the image feature points; the estimated pose module 30 is used to estimate the pose of the UAV by using the temporal correspondence relationship database of the image feature points and the inertial measurement unit data; the flight path module 40 is used to construct a 3D space through the image feature points, and construct a local environment map of the 3D space by using the image feature points; the UAV performs path planning according to the local environment map to generate a UAV flight path; the optimized pose module 50 is used to optimize the pose of the UAV based on the UAV flight path and the visual semi-direct method to obtain the final pose of the UAV.

[0173] In summary, according to the indoor UAV positioning and navigation path planning method and system proposed by the present invention, a 3D space is constructed through the image feature points between consecutive image frames in the correspondence relationship database, the 2D coordinates of each image feature point are converted into 3D space coordinates, and a point cloud is generated by using these 3D space coordinates, so as to generate features such as the environmental distribution in the indoor by using the point cloud; the 3D space is divided into multiple regular cubic grids, the point cloud density is calculated for the point cloud in each grid area, and the grid occupancy probability is calculated for the grid area through the point cloud density. The occupancy probability of each grid reflects the possibility of the existence of an object in this area, so as to judge whether there is an object in the grid area; by the occupancy probability of each grid area, it is judged which grid area may have a higher object probability, so that a local environment map can be constructed through these object probabilities with higher occupancy probabilities; and further, the local environment map is sampled and path-planned by the RRT algorithm, and each sampling selects a sampling point according to the grid occupancy probability of the grid area in the 3D space and the target position coordinates;

[0174] Furthermore, an adaptive step size of the search space of the local environment map is calculated based on the environmental characteristics of the local environment map and the grid occupancy probability of each grid area in the local environment map. The sampling points of the UAV are determined as path nodes according to the adaptive step size, so as to find the node closest to the flight target and avoid collisions with obstacles on the way; and the adaptive step size is optimized by the acceleration factor and the attitude adjustment value, so that the final adaptive step size can avoid collisions with obstacles in advance during the actual flight of the UAV, which is an optimized safety step size; and the expansion direction is calculated through the environmental feature gradient, the occupancy probability gradient of the grid area and the target direction. The expansion direction enables the UAV to screen path nodes for the directional sampling points of the target end point, so as to find a fast and safe flight path and ensure the safe and reliable planning of the flight path of the UAV.

[0175] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit them; those of ordinary skill in the art can modify the technical solutions recorded in the foregoing embodiments, or perform equivalent replacements on some or all of the technical features; and these modifications or replacements do not make the essence of the corresponding technical solutions deviate from the scope of the technical solutions of the embodiments of the present invention.

Claims

1. An indoor drone positioning, navigation and path planning method, characterized in that It includes the following operation steps: Install a binocular camera device and an inertial measurement unit sensor on the drone, and obtain binocular camera images and inertial measurement unit data of the drone; Extract features from the binocular camera images to obtain image feature points, and establish a time-series correspondence database of the image feature points based on the image feature points; Estimate the pose of the drone by using the time-series correspondence database of the image feature points and the inertial measurement unit data; Construct a 3D space through the image feature points, and construct a local environment map for the constructed 3D space by using the image feature points; The drone conducts path planning according to the local environment map to generate a drone flight path; Optimize the pose of the drone based on the drone flight path and the visual semi-direct method to obtain the final pose of the drone; Construct a 3D space through the image feature points, and construct a local environment map for the constructed 3D space by using the image feature points. The specific operation steps are as follows: Construct a 3D space for the image feature points between consecutive image frames in the time-series correspondence database, determine the pixel coordinates for each image feature point in the 3D space, and generate corresponding point clouds by using the pixel coordinates; Divide the 3D space into multiple regular cubic grids, and each grid is represented as a region; Calculate the point cloud density for the point clouds in each grid region, calculate the grid occupancy probability for the grid region through the point cloud density, and determine whether there is an object in the grid region by using the grid occupancy probability of each grid region; And construct a local environment map according to the grid occupancy probability of the point cloud density in each grid region in the 3D space; The drone conducts path planning according to the local environment map to generate a drone flight path. The specific operation steps are as follows: Extract environmental features based on the local environment map; Input the starting point coordinates and the target position coordinates into the drone; sample and plan a path for the local environment map through the RRT algorithm, and select a sampling point each time according to the grid occupancy probability of the grid region in the 3D space and the target position coordinates; Calculate the adaptive step size of the drone and determine the expansion direction by using the environmental features and the grid occupancy probability; Calculate the optimal path of the sampling point through the adaptive step size and the expansion direction to generate path nodes, and then obtain a sequence of path nodes through the entire iteration. Smooth the path nodes in the sequence of path nodes according to the RRT algorithm, connect the path nodes, and obtain the drone flight trajectory; The environmental features include edge features, plane features, and corner features; Sample and plan a path for the local environment map through the RRT algorithm, and select a sampling point each time according to the grid occupancy probability of the grid region in the 3D space and the target position coordinates. The specific operation steps are as follows: Each grid region includes grid position coordinates and grid occupancy probability; Calculate the sampling probability density according to each grid region in the local environment map. The sampling probability density formula is: P(x, y, z) = w1·D(x, y, z) + w2·G(x, y, z) + w3·O(g); In the formula, D(x, y, z) represents the environmental feature, with a value range of [0, 1]; it reflects the richness of the environmental features in the current area; G(x, y, z) represents the target guiding term, with a value range of [0, 1]; O(g) represents the grid occupancy probability, with a value range of [0, 1]; g represents the grid area, and (x, y, z) represents the position coordinates; w1, w2, and w3 represent the weighting coefficients; Traverse each grid area g in the local environmental map and calculate the sampling probability density of each grid area g; Based on the sampling probability density, calculate the acceptance score Q of each grid area g in the local environmental map. The calculation formula for the acceptance score Q is: In the formula, represents the Laplacian operator for environmental features; G represents the Euclidean distance from the current grid area coordinates to the target position coordinates; α is the feature gain coefficient; β is the distance attenuation coefficient; The calculation formula for the Euclidean distance G from the current grid area coordinates to the target position coordinates is: In the formula, (xg, yg, zg) represents the current grid area coordinates; (xG, yG, zG) are the target position coordinates; Preset a scoring threshold R; judge whether the acceptance score Q of each grid area is greater than or equal to the scoring threshold R; If not, discard the grid area; If so, the grid area is used as a candidate sampling point; and traverse each grid area, and store the grid areas that meet the criteria as candidate sampling points in the sequence list E; Use the RRT algorithm to screen the candidate sampling points of the grid areas in the sequence list E again according to the target guiding term G(x, y, z), and select the grid area closest to the target position coordinates as the final sampling point.

2. The indoor drone positioning, navigation, and path planning method according to claim 1, wherein The adaptive step size of the UAV is calculated through the environmental feature and the grid occupancy probability. The specific operation steps are as follows: Calculate the adaptive step size of the search space of the local environmental map according to the environmental features of the local environmental map and the grid occupancy probability of each grid area in the local environmental map; Optimize the adaptive step size and preset a safety distance T between the drone and the sampling point target safe ; Calculate the step distance between the drone and the sampling point target based on the distance between the current drone and the sampling point target and the preset safety distance Calculate the acceleration factor for the UAV through the preset safety distance; Calculate the attitude adjustment value of the UAV through the distance between the current UAV and the sampling point target and the preset safety distance; Adjust the step distance through the acceleration factor of the UAV and the attitude adjustment value to obtain the adaptive step distance; take the adaptive step distance as the final adaptive step size.

3. The method for indoor UAV positioning, navigation, and path planning according to claim 2, wherein The determination of the expansion direction is as follows: Calculate the partial derivative of the environmental feature according to the central difference method to obtain the environmental feature gradient; Calculate the partial derivative of the grid occupancy probability of the grid area according to the central difference method to obtain the occupancy probability gradient of the grid area; Calculate the target direction according to the sampling point of the current grid area coordinates and the target position coordinates; Calculate the expansion direction through the environmental feature gradient, the occupancy probability gradient of the grid area, and the target direction.

4. The indoor drone positioning, navigation, and path planning method according to claim 3, wherein Calculate the optimal path of the sampling points through the adaptive step size and the expansion direction, generate path nodes, and then obtain a sequence of path nodes through the entire iteration. Smooth the path nodes in the sequence of path nodes according to the RRT algorithm, and connect the path nodes to obtain the UAV flight trajectory. The specific operation steps are as follows: Generate sampling points of a new grid area coordinate from the sampling points of the current grid area coordinate through the adaptive step size and the expansion direction; Preset an occupancy probability threshold v, verify the occupancy probability of the sampling points of the new grid area coordinate, and determine whether the occupancy probability of the sampling points of the new grid area coordinate is less than the occupancy probability threshold v; If not, the sampling points of the new grid area coordinate are determined to be in the object area of the local environment map, and new sampling points of the grid area coordinate are reselected; If so, the sampling points of the new grid area coordinate are valid sampling points and are used as path nodes; Iteratively generate valid sampling points through the adaptive step size and the expansion direction, use all valid sampling points as path nodes, and generate a sequence of path nodes. The sequence of path nodes = {g1, g2, g3, g4..., gn}; Smooth the path nodes in the sequence of path nodes through the RRT algorithm, check the connection lines between the path nodes, and connect the path nodes that can be connected between the path nodes to obtain the UAV flight trajectory.

5. The indoor drone positioning, navigation and path planning method according to claim 4, characterized in that, Smooth the path nodes in the sequence of path nodes through the RRT algorithm, check the connection lines between the path nodes, and connect the path nodes that can be connected between the path nodes to obtain the UAV flight trajectory. The specific operation steps are as follows: Make a straight connection between the path node g1 and the path node g3 in the sequence of path nodes to form a path segment; judge whether the occupancy probability of the grid area of each sampling point on the path segment is less than the occupancy probability threshold v; If not, there is an object at the sampling points on the path segment between the path node g1 and the path node g3, and the path segment between the path node g1 and the path node g3 is discarded; If so, retain the path segment between the path node g1 and the path node g3 to obtain a new sequence of path nodes = {g1, g3, g4..., gn}; Make a straight connection between the path node g1 and the path node g4 in the new sequence of path nodes = {g1, g3, g4..., gn} to form a path segment, and repeat the above steps until the path node closest to the target position coordinate of the path node g1 is selected, and update the path segment between the retained path node g1 and the path node g3; Use the path nodes of the updated retained path segment as a new starting point to continue the recursive operation of the above steps to obtain all optimized path nodes; Connect all optimized path nodes to obtain the UAV flight trajectory.

6. An indoor drone positioning, navigation and path planning system for implementing the indoor drone positioning, navigation and path planning method according to any one of claims 1-5, characterized in that, The system includes: a data acquisition module; a database module; an estimated pose module; a flight path module; an optimized pose module; The data acquisition module is used to install a binocular camera device and an inertial measurement unit sensor on the drone, and obtain binocular camera images and inertial measurement unit data from the drone; The database module is used to extract features from the binocular camera images to obtain image feature points, and establish a temporal correspondence relationship database of the image feature points based on the image feature points; The pose estimation module is used to estimate the pose of the drone by using the temporal correspondence relationship database of the image feature points and the inertial measurement unit data; The flight path module is used to construct a 3D space through the image feature points, and use the image feature points to construct a local environment map for the constructed 3D space; The drone performs path planning according to the local environment map to generate a drone flight path; The optimized pose module is used to optimize the pose of the drone based on the drone flight path and the visual semi-direct method to obtain the final pose of the drone.

Images