Indoor unmanned aerial vehicle positioning navigation path planning method and system

Abstract

The invention discloses an indoor unmanned aerial vehicle positioning navigation path planning method and system, and the method comprises the steps: installing a binocular camera device and an inertial measurement unit sensor for an unmanned aerial vehicle, and obtaining a binocular camera image and inertial measurement unit data for the unmanned aerial vehicle; performing feature extraction on the binocular camera image to obtain image feature points, and establishing a time sequence corresponding relation database of the image feature points based on the image feature points; estimating the pose of the unmanned aerial vehicle by using the time sequence corresponding relation database of the image feature points and the data of the inertial measurement unit; constructing a 3D space through the image feature points, and constructing a local environment map for the constructed 3D space by using the image feature points; the unmanned aerial vehicle performs path planning according to the local environment map to generate an unmanned aerial vehicle flight path; and optimizing the pose of the unmanned aerial vehicle based on the flight path of the unmanned aerial vehicle and a visual semi-direct method to obtain the final pose of the unmanned aerial vehicle. The optimal path is calculated, and the pose of the unmanned aerial vehicle is optimized, so that the estimation error of the position of the unmanned aerial vehicle is reduced.

Description

Technical Field

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

[0002] With the continuous development of drone technology, especially its application in indoor environments, the positioning and navigation of indoor drones has 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 difficult deployment and high cost.

[0004] In recent years, the UAV positioning method based on visual information has gradually become one of the mainstream technologies for indoor UAV 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 camera and inertial measurement unit (IMU) has been widely used in UAV positioning and navigation.

[0006] Binocular cameras can obtain three-dimensional depth information of the environment through the principle of stereo vision, thereby providing accurate spatial perception for drones. Especially in indoor environments without GPS signals, binocular vision provides an important positioning basis for drones.

[0007] The inertial measurement unit (IMU) provides dynamic information about the drone, such as acceleration, angular velocity, and direction, through sensors such as accelerometers and gyroscopes. It can effectively supplement the lack of visual information, especially in highly dynamic or partially occluded environments. The IMU can provide real-time attitude estimation and motion status.

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

[0009] Traditional path planning algorithms include Dijkstra algorithm, RRT (rapid random tree) algorithm, etc.

[0010] However, in complex indoor environments, where obstacles are arranged irregularly, the real-time and accuracy of path planning remains a problem to be solved. Summary of the invention

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

[0012] The present invention provides a method for indoor UAV positioning and navigation path planning, comprising the following steps: Installing a binocular camera device and an inertial measurement unit sensor on the drone, and acquiring binocular camera images and inertial measurement unit data from the drone; Extracting features from the binocular camera image to obtain image feature points, and establishing a temporal correspondence database of the image feature points based on the image feature points; The position and posture of the UAV are estimated by using the temporal correspondence database of the image feature points and the inertial measurement unit data; Constructing a 3D space through the image feature points, and constructing a local environment map for the constructed 3D space using the image feature points; performing path planning on the UAV according to the local environment map to generate a flight path for the UAV; The position and posture of the UAV is optimized based on the flight path of the UAV and the visual semi-direct method to obtain the final position and posture of the UAV.

[0013] Compared with the prior art, the embodiments of the present invention have at least the following technical advantages: By analyzing the indoor UAV positioning and navigation path planning method provided by the present invention, it can be known that in specific applications, a binocular camera device and an inertial measurement unit sensor are installed on the UAV, and the binocular camera image and inertial measurement unit data of the UAV are collected and acquired, so that the depth information of each point in the scene can be calculated, and the three-dimensional structure of the environment can be acquired. In addition, the inertial measurement unit data can provide real-time attitude information of the UAV, which is very important for indoor UAV navigation. Furthermore, the FAST corner detection algorithm can be used to extract image feature points from binocular camera images. The extraction not only helps to identify key areas in the environment, but also finds out which image feature points are the same or corresponding in images taken at different time points, so as to associate the same image feature points in the previous and next frames of images, generate a time series relationship, and thus record the trajectory of feature points changing over time. Furthermore, the position and posture of the UAV can be estimated by using the temporal correspondence database of the image feature points and the inertial measurement unit data. By estimating the position and posture of the UAV, the positioning accuracy of the UAV can be optimized, thereby estimating the change of the position and posture of the UAV. Furthermore, a 3D space is constructed by image feature points between consecutive image frames in a corresponding relational database, the 2D coordinates of each image feature point are converted into 3D space coordinates, and point clouds are generated using these 3D space coordinates, thereby using point clouds to generate indoor environmental distribution and other features; 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 the area, thereby determining whether there is an object in the grid area; the occupancy probability of each grid area is used to determine which grid area may have a higher probability of an object, 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 planned using the RRT algorithm. Each sampling selects a sampling point based on 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 that obstacles can be avoided and the drone can fly safely; Furthermore, the adaptive step length of the search space of the local environment map is calculated according to the environmental characteristics of the local environment map and the grid occupancy probability of each grid area in the local environment map, and the sampling points of the drone are determined according to the adaptive step length to find the node closest to the flight target and avoid collision with obstacles on the way; and the adaptive step length is optimized by the acceleration factor and the attitude adjustment value, so that the final adaptive step length can avoid collision with obstacles in advance during the actual flight of the drone, and optimize the safety step length; and the expansion direction is calculated by the environmental characteristic gradient, the grid area occupancy probability gradient and the target direction, and the expansion direction enables the drone to screen the path nodes of the directional sampling points of the target terminal, so as to find a fast and safe flight path; the sampling points of the current grid area coordinates are generated by the adaptive step length and the expansion direction. The new sampling points can avoid collision with some obstacles; and the grid occupancy probability of the sampling points of the new grid area coordinates is verified by the preset occupancy probability threshold v, so that the valid sampling points are used as path nodes, indicating that there are no obstacles on the path of the new sampling node and the target position coordinates can be reached; Furthermore, the effective sampling points are iteratively generated through adaptive step size and expansion direction, and all the effective sampling points are used as path nodes to generate a path node sequence. The path nodes in the path node sequence are connected to determine whether there are obstacles at the sampling points on the connection path, so as to find a shortest and safe UAV flight path. Furthermore, the UAV's flight path and the visual semi-direct method are used to optimize the UAV's posture and obtain the UAV's final posture. The visual semi-direct method is combined with the UAV's flight path to reduce the error in the UAV's position estimation and ensure that the UAV can fly along the predetermined path as accurately as possible. BRIEF DESCRIPTION OF THE DRAWINGS

[0014] Figure 1 This is an overall flow chart of an indoor UAV positioning and navigation path planning method; Figure 2 This is a flow chart of the UAV flight trajectory; Figure 3 A schematic diagram of the grid occupancy probability for the point cloud density of the grid area; Figure 4 This is a flow chart of the sampling points; Figure 5 Generate a detailed flow chart of the UAV flight trajectory for the path nodes; Figure 6 Iterate the flow chart for filtering path nodes; Figure 7 Iterate the schematic diagram for filtering path nodes; Figure 8 Flowchart of the path planning system for indoor UAV positioning and navigation.

[0015] Label: data acquisition module 10; database module 20; posture estimation module 30; flight path module 40; posture optimization module 50. DETAILED DESCRIPTION

[0016] The technical solution of the present invention will be clearly and completely described below in conjunction with the accompanying drawings. Obviously, the described embodiments are only part of the embodiments of the present invention, rather than all of the embodiments.

[0017] The present invention is further described in detail below through specific embodiments in conjunction with the accompanying drawings.

[0018] Embodiment 1 like Figure 1 As shown, the present invention proposes an indoor UAV positioning and navigation path planning method, which includes the following steps: S1: installing a binocular camera device and an inertial measurement unit sensor on the drone, and obtaining binocular camera images and inertial measurement unit data from the drone; It should be noted that the drone is equipped with binocular camera equipment and inertial measurement unit sensors; the binocular camera consists of two cameras, usually two cameras parallel to each other and a certain distance apart; by taking images of the same scene with two cameras at the same time, the depth information of objects in the scene, that is, the distance between the camera and the object, can be calculated using the principle of stereo vision; compared with monocular cameras, binocular cameras can obtain richer three-dimensional information, thereby providing accurate spatial perception for drones, which is very important for indoor drone obstacle avoidance, map construction and other tasks; and the inertial measurement unit (IMU) is a common sensor, which contains sensors such as accelerometers, gyroscopes (and possible magnetometers), etc., which are used to measure the acceleration, angular velocity and direction of the drone. IMU can provide dynamic information of the drone, such as attitude, speed, acceleration, etc. In the process of obtaining the binocular camera image 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 processed synchronously; because the visual data and the IMU data usually have different sampling frequencies and timestamps, therefore, the data of these two sensors (i.e., the binocular camera device and the inertial measurement unit sensor) need to be time synchronized to ensure that they can accurately correspond to the environmental information at the same time; The acquired 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 and obstacles). This is very important for indoor drone obstacle avoidance, map building and other tasks. At the same time, the inertial measurement unit data may include the attitude information and motion status of the drone, 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. The motion status in the IMU data can help calculate the speed and acceleration of the drone, providing instant dynamic feedback. S2: extracting features from the binocular camera image to obtain image feature points, and establishing a temporal correspondence database of the image feature points based on the image feature points; It should be noted that the FAST corner detection algorithm can be used to extract image feature points from the binocular camera image; further, the binocular camera image is Gaussian filtered to smooth and construct a pyramid image, and the pyramid image is divided into a left camera image and a right camera image of the binocular camera image; the extracted image feature points are mapped to the image pixels of each layer in the pyramid image, and the position coordinates and description information (i.e., feature descriptors) of the image feature points in each layer of the pyramid are recorded, and the position coordinates and description information of the image feature points in the left image pyramid and the right image pyramid are matched and compared from the top layer of the pyramid to obtain the identified image feature points, and a temporal correspondence database is established based on the coordinates, description information, etc. of the identified image feature points; In binocular camera image processing, image feature points are local areas with significant information in the image, and these points usually remain stable under different viewing angles and lighting conditions. By extracting feature points in the image, 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 areas in the environment, but can also be used for tasks such as image matching, image stitching (map construction), motion estimation (visual odometer), etc. The FAST corner detection algorithm is a very classic feature extraction method. It detects feature points by finding local extreme points in the image. These feature points are scale and rotation invariant and are suitable for describing objects from different perspectives. 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 (that is, whether the coordinates of the image feature points of the left camera image and the right camera image of the binocular camera correspond to the matching of the descriptors; for each pair of images, by calculating the similarity of the feature points between them (that is, usually measured by the Euclidean distance or Hamming distance between descriptors), it is determined which feature points are one-to-one corresponding between the images), and the image feature points are constructed into a time series correspondence database based on the coordinates, descriptors, time and other information; the construction of the time series correspondence database means that for each image feature point (for example, the image feature point extracted in the nth frame image), it can be associated with the same image feature point in the previous and next frames of images to generate a time series relationship, thereby recording the trajectory of the feature point changing over time; S3: Estimate the position and posture of the drone using the temporal correspondence database of image feature points and inertial measurement unit data; It should be noted that the time-series correspondence database of image feature points records the feature point matching between each frame of the image. The information of these feature point matching helps the UAV system understand the relative motion between adjacent frames, and then calculate the displacement, rotation and other parameters; combined with the inertial measurement unit data (that is, in a complex indoor environment, obstacles and occlusions will affect the matching of visual features, so through IMU data compensation and correction, the UAV system can still maintain high-precision positioning in an environment with unclear features), the UAV's instantaneous linear velocity and angular velocity can be calculated, and the positioning accuracy of the UAV can be further optimized, thereby estimating the change in the UAV's posture (that is, real-time position and status); S4: construct 3D space through image feature points, and use image feature point pairs to construct 3D space to construct a local environment map; the UAV performs path planning based on the local environment map to generate the UAV flight path; It should be noted that the visual data obtained by the binocular camera (i.e., the real-time image data taken) combined with the extracted image feature points can capture the key structures and obstacles in the current environment (i.e., the indoor room structure and the layout and distribution of obstacles such as tables, chairs, and benches); in this process, the visual odometer is used to estimate the current drone camera posture, and the position and shape of objects in the environment are determined based on the matching of image feature points between consecutive image frames, thereby constructing a local environment map of the room; the drone system plans the drone's path based on the key structures and obstacles in the local environment 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 drone flight path), thereby generating a drone flight path; S5: Optimize the UAV's position and posture based on the UAV's flight path and the visual semi-direct method to obtain the UAV's final position and posture.

[0019] It should be noted that path planning provides an ideal flight trajectory or target route (i.e., the UAV flight path), and the UAV posture optimization will combine this path to reduce the error of position estimation. In the optimization stage, the goal of posture optimization is to adjust the current estimated position and posture of the UAV to ensure that the UAV can fly along the predetermined path as accurately as possible; Pose optimization not only relies on visual information (i.e., real-time image data taken), but also on the heading information provided by path planning (i.e., information on the flight direction of the UAV's flight path). The goal of the optimization process is to correct errors through visual semi-direct methods and other means, reduce pose drift, and ensure a smoother and more accurate flight. Visual Semi-Direct Method is a commonly used technology in the field of computer vision and robotics. It is 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. Specifically, if Figure 2 As shown, in step S4, 3D space is constructed by image feature points, and local environment map is constructed by using image feature points to construct 3D space; the UAV performs path planning according to the local environment map to generate the flight path of the UAV. The specific operation steps are as follows: S41: constructing a 3D space (i.e., three-dimensional space) for image feature points between consecutive image frames in the temporal correspondence database, determining pixel coordinates (i.e., 3D space coordinates) for each image feature point in the 3D space, and generating a corresponding point cloud using the pixel coordinates; It should be noted that the stereo vision principle of the binocular camera is used to combine 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 the parallax (i.e. the difference in pixel positions of the same feature points in the two images) is used to calculate the 3D spatial 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, and convert the 2D coordinates of each feature point into 3D spatial coordinates); Use these 3D space coordinates to generate a point cloud (that is, output these 3D space coordinate points as point clouds); 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 can be used for subsequent map construction and path planning. S42: Divide the 3D space into a plurality of regular cubic grids, each grid being represented as a region (i.e., a grid region); Calculate the point cloud density of each grid area, calculate the grid occupancy probability of the grid area through the point cloud density, and judge whether there is an object in the grid area (including obstacles such as tables, chairs, benches, etc.) through the grid occupancy probability of each grid area; And build a local environment map based on the grid occupancy probability of the point cloud density in each grid area in the 3D space; 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, each grid representing a small area; and calculates the number of point clouds in each grid area to check the density of the point clouds in the grid; the occupancy probability of each grid reflects the possibility of the existence of an object in the area. A high grid occupancy probability means that there is a high probability that there are obstacles or objects in the area, and a low grid occupancy probability means that the area is relatively empty (i.e., the grid occupancy probability is calculated based on the range occupied by the point cloud density in each grid area, thereby judging whether the grid occupancy probability is high or low, such as Figure 3 shown); If a grid contains dense point clouds, it means that there are objects in the area and the probability of obstacles is high; if a grid has almost no point clouds, it means that the area is empty and the probability of objects is low, such as Figure 3 As shown; The occupancy probability of each grid area is used to determine which grid area may have a higher probability of an object. In this way, a local environment map can be constructed based on the probability of objects with higher occupancy probabilities, reflecting the distribution of obstacles in the environment. The occupancy probability can also be used to guide path planning. S43: extracting environmental features based on the local environment map; environmental features include edge features, plane features and corner features; it should be noted that the local environment map is used to extract an environmental feature set y, and the environmental feature set Y includes edge features E (i.e. edge features E: extracting linear structures in the environment, such as wall edges, door frames, etc.), plane features S (i.e. plane features S: extracting plane structures in the environment, such as walls, floors, etc.), and corner features C (i.e. corner features C: extracting corner structures in the environment, such as wall corners, corners, etc.); The extracted environmental features are described, which may include their location, shape, direction, size and other attributes; 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; the feature description information obtained in the above steps is crucial for subsequent path planning (for details, see step S45 "Using environmental features and grid occupancy probabilities to calculate the adaptive step size of the drone and determine the expansion direction, the drone can identify which areas have obstacles and which areas are suitable for movement).

[0020] 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) to the UAV; sample the local environment map and plan the path through the RRT algorithm, and select the sampling point for each sampling according to the grid occupancy probability of the grid area in the 3D space and the target position coordinates; The RRT (Rapidly Expanding Random Tree) algorithm is used to plan the path to ensure that the drone can reach the target safely and efficiently, thereby generating an optimal drone flight trajectory. Before sampling, the starting point, end point, step length and other parameters of the drone need to be determined, so that the RRT algorithm can be used to plan the path of the drone and avoid obstacles. Using grid occupancy probability as a guide, the RRT algorithm performs random sampling in the local environment map; each time the drone samples, it selects the sampling point based on the grid occupancy probability (i.e., the grid occupancy probability selects the grid area with lower point cloud density, because the grid occupancy probability with higher point cloud density has a higher probability of obstacles appearing) and the target position coordinates (i.e., the flight distance from the starting point to the end point is used to sample the grid area with lower point cloud density to avoid obstacles). The selection of the sampling point not only takes into account 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 lower point cloud density); S45: Calculate the adaptive step size of the drone and determine the expansion direction through environmental features and grid occupancy probability (that is, here, through the RRT (rapidly expanding random tree) algorithm, the tree structure is gradually expanded to gradually approach the target position coordinates (that is, the end point of the drone flight), and finally form a path); The optimal path of the sampling points is calculated through adaptive step size and expansion direction to generate path nodes. Then, a path node sequence is obtained through the entire iteration. The path nodes in the path node sequence are smoothed according to the RRT algorithm, and the path nodes are connected to obtain the flight trajectory of the UAV. It should be noted that the adaptive step size (i.e., moving distance) and expansion direction of the drone are calculated; the calculation of step size and direction usually combines environmental characteristics and grid occupancy probability to ensure that the path avoids obstacles as much as possible and is the shortest; The RRT algorithm selects the expansion direction of the tree and the new node position (i.e., the new sampling point position) through random sampling, and expands from the current sampling point (known node of the tree) to the subsequent sampling points. It ensures that the new sampling point is safe by checking whether the path intersects with obstacles. If the path is found to intersect with obstacles during the expansion process, the sampling point is discarded and is not used as a valid path node. The effective path node is not sampled one point at a time, but multiple path nodes that meet the conditions are obtained through multiple samplings (i.e., the path node is a sampling point where there may be multiple paths (i.e., the flight path), and the sampling points are screened to select the path node of the optimal path as the final flight path of the drone), and evaluate which sampling points are optimal (for example, closer to the target, or safer); 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 end point is found; the RRT algorithm usually smoothes the path, eliminates unnecessary corners or twists, and 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; Specifically, if Figure 4 As shown, in step S44, the local environment map is sampled and the path is planned by using the RRT algorithm. Each sampling selects the sampling point 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: S441: Each grid area includes grid position coordinates and grid occupancy probability (i.e., the probability of whether the grid area is occupied by an obstacle); The sampling probability density is calculated based on each grid area in the local environment map. The sampling probability density formula is: ; In the formula, It is expressed as environmental characteristics, with a value range of [0,1]; it reflects the richness of the environmental characteristics of the current region; Represented as a target guidance item, with a value range of [0,1]; it indicates the proximity between the grid area and the target position coordinates. The target guidance item encourages the selection of a grid area close to the target position coordinates (i.e., the target point), thereby improving the efficiency and accuracy of path planning; O(g) represents the grid occupancy probability, with a value range of [0,1]; it indicates the probability of whether 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 represent obstacles, so the sampling probability density of these areas will be lower; g represents the grid area, (x, y, z) represents the position coordinates; w1, w2, w3 are weighted coefficients, which are used to control the influence of each factor on the selection of sampling points; 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; 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. S442: traverse each grid area g in the local environment map, and calculate the sampling probability density of each grid area g; 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: ; 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. G represents the Euclidean distance from the current grid area coordinates to the target location coordinates; α and β are parameters that control feature response and distance effects; α is the feature gain coefficient, which is usually set to 1.5 to control the intensity of the impact of environmental features on the score; β is the distance attenuation coefficient, which is used to control the degree of impact of the target distance of the target position coordinate on the score; The calculation formula of the Euclidean distance G from the current grid area coordinates to the target position coordinates is: ; Where (xg, yg, zg) represents the coordinates of the current grid area; (xG, yG, zG) are the target position coordinates; If the Euclidean distance G is smaller, it means that the grid area is closer to the target location coordinates, and the sampling point priority should be higher; It should be noted that the sampling probability density determines the selection of sampling points, while the acceptance score q determines whether to accept the sampling point based on the relative probability of the current state and the new sampling point; S444: Preset a score threshold R; determine whether the acceptance score Q of each grid area is greater than or equal to the score threshold R; If not, the grid area is discarded; If yes, the grid area is used as a candidate sampling point; and each grid area is traversed, and the grid areas that meet the requirements of being a candidate sampling point are stored in a sequence table E; S445: using the RRT algorithm to screen the candidate sampling points of the grid area in the sequence table E again according to the target guide item G (x, y, z), and selecting the grid area closest to the target position coordinates as the final sampling point; 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 characteristics, 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; Specifically, in one of the implementations, the adaptive step length of the drone is calculated by using environmental features and grid occupancy probability, and the specific operation steps are as follows: the adaptive step length of the search space of the local environment map is calculated according to the environmental features of the local environment map and the grid occupancy probability of each grid area in the local environment map; Optimize the adaptive step size and set a preset safe distance between the drone and the sampling point target ; Calculate the step distance between the drone and the sampling point target by the current distance between the drone and the sampling point target and the preset safety distance; Calculate the acceleration factor of the drone by presetting the safety distance; The attitude adjustment value of the drone is calculated based on the distance between the current drone and the sampling point target and the preset safety distance; The step distance is adjusted by the acceleration factor and attitude adjustment value of the UAV to obtain an adaptive step distance; the adaptive step distance is used as the final adaptive step length.

[0021] Specifically, in one of the implementation modes, the expansion direction is determined, and the specific operation steps are as follows: calculating partial derivatives of the environmental features according to the central difference method to obtain the gradient of the environmental features; The partial derivative of the grid occupancy probability of the grid area is calculated according to the central difference method to obtain the occupancy probability gradient of the grid area; The target direction is calculated based on the sampling points of the current grid area coordinates and the target position coordinates; the expansion direction is calculated based on the environmental feature gradient, the occupancy probability gradient of the grid area and the target direction.

[0022] The above two specific implementation methods are described below. Figure 5As shown, in step S45, the adaptive step size of the drone is calculated and the expansion direction is determined by the environmental features and the grid occupancy probability; then the optimal path of the sampling point is calculated by the adaptive step size and the expansion direction, and the path node is generated. Then, a path node sequence is obtained through the entire iteration, and the path nodes in the path node sequence are smoothed according to the RRT algorithm, and the path nodes are connected to obtain the flight trajectory of the drone. The specific operation steps are as follows: S451: Calculate the adaptive step length of the search space of the local environment map according to the environmental characteristics of the local environment map and the grid occupancy probability of each grid area in the local environment map. The adaptive step length formula is: ; Where h is the base step size; it represents a preset, conventional step size, usually set according to the scale of the environment (i.e., the step size without considering the influence of environmental features or obstacles); α is the feature influence coefficient; it controls the gain effect of the distribution of environmental features (such as terrain, obstacle density, etc.) on the step length. The distribution of environmental features D(x, y, z) will affect the adjustment size of the step length. If the environmental features change greatly, the step length will be adjusted accordingly (that is, when the environmental features change greatly, it means that the surrounding environment is more complex and the obstacles are more dense, so the step length will be increased to cross the area and look for places without obstacles in a farther area, so the step length is adjusted); β is the influence coefficient of occupancy probability, which controls the inhibitory effect of obstacles on step length. The grid occupancy probability O(g) will affect the step length, avoiding crossing obstacles due to excessive step length. D(x,y,z) represents the environmental characteristics; O(g) represents the grid occupancy probability; it indicates the occupancy status of the grid area where the current position is located. If O(g) is large, it means that there are obstacles in the grid area, and the step size will also be reduced (that is, when an obstacle appears in a grid area, but there are no obstacles around, the step size will be reduced to avoid the obstacle in advance); S452: Optimize the adaptive step size; set a preset safe distance between the drone and the sampling point target , the preset safety distance is the minimum safety distance The preset maximum safety distance ; The step distance between the drone and the sampling point target is calculated by the preset safety distance. The calculation formula is: ; In the formula, is the minimum step distance, ensuring that the drone can still move even when very close to the sampling point target; c is the basic step distance. When the drone is far away from the target, a larger step distance is used; u is the decay factor (i.e., the decay factor often takes the form of exponential decay, for example, Where T is the current distance to the target, is the minimum distance, a is the constant of the attenuation factor; as the drone approaches the target, the attenuation factor decreases and the step distance also decreases); T is the distance between the current UAV and the sampling point target; e is represented as a constant; According to the above formula, when searching for the step length of the sampling point, it should be considered that the drone should avoid the sampling points where obstacles may exist in advance during the flight, so a preset safety distance is set between the drone and the sampling point; when the safety distance is too large, a larger step length is used; when the safety distance is small, a smaller step length can be used to prevent the drone from approaching obstacles too quickly, so the adaptive step length needs to be further optimized; S453: Calculate the acceleration factor of the drone at the preset safety distance. The calculation formula is: ; In the formula, a is the acceleration factor of the UAV; It is expressed as the minimum acceleration of the drone; Expressed as the maximum acceleration of the drone; The attitude adjustment value of the drone is calculated by the distance between the current drone and the sampling point target and the preset safety distance. The calculation formula is: ; In the formula, Indicates the current attitude adjustment value; It is expressed as the initial attitude adjustment value, which refers to the attitude adjustment amount of the drone at a longer distance; a is the attenuation factor; T is the distance between the current UAV and the sampling point target; It should be noted that acceleration determines the speed change capability of the drone, affecting its movement speed and responsiveness. A larger acceleration means that the drone can start, accelerate or adjust direction quickly, while a smaller acceleration means that its response is relatively slow. When moving at high speed or adjusting quickly, the drone may need to increase the step distance to avoid staying in the current position for a long time. When the acceleration is low, the step distance may be reduced to prevent sudden rapid movement from causing unstable control or collision. Attitude adjustment values ​​refer to adjustments made in the control system based on the current attitude of the drone (such as pitch, roll, and yaw); these values ​​reflect the stability and directional control of the drone; attitude adjustment values ​​usually affect the size of the step distance. When the attitude deviation of the drone is large (for example, a sharp yaw or pitch occurs), the step distance may be reduced to reduce the impact of attitude errors on the overall movement; if the attitude adjustment value indicates that the drone is in a stable flight state, the step distance can be increased to allow it to move faster, so that the adaptive step size can be better optimized; S454: adjusting the step distance by using the acceleration factor and attitude adjustment value of the drone to obtain an adaptive step distance; using the adaptive step distance as the final adaptive step length; The adaptive step distance calculation formula is: ; In the formula, Adjust the acceleration factor of the drone. Adjust the step distance. If the drone is moving faster than the obstacle, the step distance will be reduced accordingly, allowing the drone to have enough time to avoid the obstacle. k is a constant; It is expressed as the object target impact factor (i.e. obstacle impact factor), which is used to adjust the step distance of the drone when approaching an obstacle. If the obstacle is closer, the step distance will be reduced to avoid collision. It is the safety distance, which is used to define when to slow down or avoid contact with obstacles. It ensures that during the obstacle avoidance process, the step distance is not too large, which may lead to collision or misjudgment. It should be noted that in the adaptive step length control, the acceleration and attitude adjustment value act together on the calculation of the step distance, so that the UAV can keep a safe distance from obstacles in the dynamic adaptive environment of flight and avoid obstacles; 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 obstacles; the attitude adjustment value ensures that the UAV can remain stable in different flight attitudes, and adjusts the step distance to cope with the instability of the attitude, so that the adaptive step length can be optimized. In practice, the UAV can avoid obstacles, find suitable path nodes for sampling points, and find a safe flight path; S455: Calculate partial derivatives of environmental features according to the central difference method to obtain environmental feature gradients; The partial derivative of the grid occupancy probability of the grid area is calculated according to the central difference method to obtain the occupancy probability gradient of the grid area; The target direction is calculated based on the sampling point of the current grid area coordinates and the target position coordinates. The calculation formula is: ; Where (xg, yg, zg) represents the sampling point of the current grid area coordinates; (xG, yG, zG) are the target position coordinates (i.e., when calculating the Euclidean distance from the current grid region coordinates to the target position coordinates in step S443, two formula factors of the sampling point of the current grid region coordinates and the target position coordinates have been obtained, so the target direction from the sampling point of the current grid region coordinates to the target position coordinates is directly calculated); The expansion direction is calculated by the environmental feature gradient, the occupancy probability gradient of the grid area and the target direction. The calculation formula is: ; Where normalize() is a normalization function (i.e., it is used to ensure that the length of the resulting direction vector is within a reasonable range); w1, w2, w3 are weighted coefficients used to balance the impact of three directions on expansion (i.e., environmental feature gradient, target direction, and occupancy probability gradient of grid area). Expressed as environmental characteristic gradient; indicating the degree of change of environmental characteristics in all directions; It is expressed as the occupancy probability gradient of the grid area; it indicates the degree of change of the current grid area occupancy state in each direction; The target direction from the sampling point of the current grid region coordinates to the target position coordinates (i.e., the vector indicating the direction of the target position); S456: Generate new sampling points of the grid region coordinates from the sampling points of the current grid region coordinates by using an adaptive step size and an expansion direction; It should be noted that the expansion direction is determined according to the sampling points of the current grid area coordinates, and new sampling points are generated according to the adaptive step size. The new sampling points can avoid collisions with some obstacles. S457: Preset an occupation probability threshold v, perform occupation probability verification on the grid occupation probability of the sampling point of the new grid region coordinates, and determine whether the grid occupation probability of the sampling point of the new grid region coordinates is less than the occupation probability threshold v; If not, the sampling point of the new grid area coordinates is determined to be in the object area of ​​the local environment map, and the sampling point of the new grid area coordinates is reselected; If so, the sampling point of the new grid region coordinate is a valid sampling point and serves as a path node; It should be noted that the preset occupancy probability threshold v determines 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 a new sampling point of the 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 the target position coordinates can be reached; S458: Generate valid sampling points iteratively through adaptive step size and expansion direction, take all valid sampling points as path nodes, and generate a path node sequence, where the path node sequence = {g1, g2, g3, g4..., gn}; It should be noted that, by iterating all generated path nodes 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, and the 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 constitute a preliminary path (that is, the preliminary path is equivalent to an unoptimized UAV flight path, and there may be a detour between two nodes (that is, there are obstacles in the grid area of ​​the sampling point (or path node) between the two path nodes), and it is not possible to fly in a straight line between the 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 that reduces flight time and collisions; S459: smoothing the path node sequence by using the RRT algorithm, checking the connection between the path nodes, connecting the path nodes that can be connected between the path nodes in series, and obtaining the flight trajectory of the UAV; It should be noted that by smoothing the path node sequence through the RRT algorithm, unnecessary turns in the path can be reduced, so that the UAV can travel in a more stable and efficient way, avoid unnecessary collisions, and shorten the path as much as possible; check whether there are obstacles at other sampling points on the path of the 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 determine whether there are obstacles at all sampling points on the straight path. The smoothing effect is achieved through screening, and the path nodes that meet the best path can also be screened out. The flight trajectory of the UAV is formed by connecting the path nodes of all the best paths in series; Specifically, if Figure 6As shown, in step S459, the path node sequence is smoothed by the RRT algorithm, the connection between the path nodes is checked, and the path nodes that can be connected between the path nodes are connected in series to obtain the flight trajectory of the drone. The specific operation steps are as follows: S4591: Connect the path node g1 and the path node g3 in the path node sequence with a straight line to form a path segment; determine for each sampling point on the path segment whether the grid occupancy probability of the grid area is less than the occupancy probability threshold v (i.e., the determination of whether there is an obstacle at the sampling point on the path segment can refer to step S457, and the occupancy probability threshold v is the threshold preset in step S457); If not, there is an object (i.e., the object includes obstacles such as tables, chairs, and benches) at the sampling point 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 yes, 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}; It should be noted that in order to smooth the path nodes and screen out the most optimized path nodes, the path nodes are directly connected to determine whether there are obstacles in the grid occupancy probability of the grid area of ​​all sampling points on the connecting line segment; If the grid occupancy probability O(q) of a sampling point on the straight line segment is greater than or equal to the occupancy threshold v, the sampling point on the straight line segment is located in an 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 replanned; 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 (i.e. the point in the middle of the path) can be removed, saving the flight path of the drone, such as Figure 7 As shown), this becomes the new path g1, g3, g4..., gn; S4592: Connect the path node g1 and the path node g4 in a straight line for the new path node sequence = {g1, g3, g4, ..., gn} to form a path segment, repeat the above step S4561 until the path node closest to the target position coordinates is selected, and update the retained path segment of the path node g1 and the path node g3; It should be noted that when the path segment of 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 determine whether there are obstacles at the sampling points on the path segment of path node g1 and path node g4 (that is, determining whether there are obstacles at the sampling points on the path segment is the same as step S4591 and will not be repeated here). When there are no obstacles at the sampling points on the path segment of path node g1 and path node g4, the reserved path segment of path node g1 and path node g3 is updated, and the path segment of path node g1 and path node g4 is retained, and so on, the path segment of path node g1 and path node g5 is determined until an obstacle appears in the path segment; in this way, the distance from g1 to the path node closest to the target position coordinates can be found, which reduces the flight time of the drone and makes the path node reach the maximum smoothness; S4593: taking the updated path node of the retained path segment as a new starting point and continuing the recursive operation of steps S4591-S4592 to obtain all optimized path nodes; Connect all optimized path nodes to obtain the flight trajectory of the drone; It should be noted that after the reserved path segment of step S4592 is updated, the new path node is recursively operated. For example, if the path segment of the reserved path node g1 and the path node g3 is updated, the new path node sequence = {g1, g4..., gn}, and the path node g1 and the path node g5 are connected by a straight line. If there is an obstacle in the path segment of the path node g1 and the path node g5, the update is stopped, because it indicates that there is an obstacle in the path node g5 and the drone cannot fly, so it is necessary to stop selecting the route and continue to recursively operate the steps S4591-S4592 for the path node g4 and the path node g6. Figure 7 As shown, until all the optimal path nodes are selected, the flight trajectory of the UAV is formed to ensure the flight safety of the UAV; Embodiment 2 like Figure 2As shown, the second embodiment 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 posture module 30; a flight path module 40; and an optimized posture 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 from the UAV; the database module 20 is used to extract features from the binocular camera image to obtain image feature points, and establish a time series correspondence database of image feature points based on the image feature points; the estimated posture module 30 is used to estimate the posture of the UAV using the time series correspondence database of image feature points and the inertial measurement unit data; the flight path module 40 is used to construct a 3D space through image feature points, and use image feature points to construct a local environment map of the 3D space; the UAV performs path planning according to the local environment map to generate a UAV flight path; the optimized posture module 50 is used to optimize the posture of the UAV based on the UAV flight path and the visual semi-direct method to obtain the final posture of the UAV.

[0023] In summary, it can be seen from the indoor UAV positioning and navigation path planning method and system proposed in the present invention that a 3D space is constructed by image feature points between continuous image frames in a corresponding relationship database, the 2D coordinates of each image feature point are converted into 3D space coordinates, and point clouds are generated using these 3D space coordinates, so as to use point clouds to generate indoor environmental distribution and other features; 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 the area, so as to judge whether there is an object in the grid area; the occupancy probability of each grid area is used to judge which grid area may have a higher probability of an object, so that a local environment map can be constructed through these object probabilities with higher occupancy probabilities; and the local environment map is further sampled and planned for the path through 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; Furthermore, the adaptive step length of the search space of the local environment map is calculated according to the environmental characteristics of the local environment map and the grid occupancy probability of each grid area in the local environment map, and the path nodes are determined for the sampling points of the UAV according to the adaptive step length so as to find the node closest to the flight target and avoid collision with obstacles on the way; and the adaptive step length is optimized by the acceleration factor and the attitude adjustment value, so that the final adaptive step length can avoid collision with obstacles in advance during the actual flight of the UAV, and optimize the safety step length; and the expansion direction is calculated by the environmental characteristic 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, ensuring the safe and reliable planning of the flight path of the UAV.

[0024] 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 it. A person skilled in the art may modify the technical solutions described in the above embodiments, or replace part or all of the technical features therein with equivalents. However, these modifications or replacements do not cause the essence of the corresponding technical solutions to deviate from the scope of the technical solutions of the embodiments of the present invention.

Claims

1. A method for indoor UAV positioning and navigation path planning, characterized in that: The steps are as follows: Installing a binocular camera device and an inertial measurement unit sensor on the drone, and acquiring binocular camera images and inertial measurement unit data from the drone; Extracting features from the binocular camera image to obtain image feature points, and establishing a temporal correspondence database of the image feature points based on the image feature points; The position and posture of the UAV are estimated by using the temporal correspondence database of the image feature points and the inertial measurement unit data; Constructing a 3D space through the image feature points, and constructing a local environment map for the constructed 3D space using the image feature points; performing path planning on the UAV according to the local environment map to generate a flight path for the UAV; The position and posture of the UAV is optimized based on the flight path of the UAV and the visual semi-direct method to obtain the final position and posture of the UAV.

2. According to claim 1, a method for indoor UAV positioning and navigation path planning is characterized in that: The 3D space is constructed by the image feature points, and the local environment map is constructed for the constructed 3D space 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 temporal correspondence database, determine the pixel coordinates of each image feature point in the 3D space, and generate the corresponding point cloud using the pixel coordinates; Divide the 3D space into multiple regular cubic grids, each grid is represented as a region; Calculating the point cloud density of the point cloud in each grid area, calculating the grid occupancy probability of the grid area according to the point cloud density, and judging whether there is an object in the grid area according to the grid occupancy probability of each grid area; And the local environment map is constructed according to the grid occupancy probability of the point cloud density in each grid area in the 3D space.

3. According to claim 2, a method for indoor UAV positioning and navigation path planning is characterized in that: The drone performs path planning based on the local environment map and generates the drone flight path. The specific steps are as follows: Extracting environmental features based on the local environment map; Input the starting point coordinates and the target position coordinates to the drone; sample the local environment map and plan the path through the RRT algorithm, and select the sampling point for each sampling according to the grid occupancy probability of the grid area in the 3D space and the target position coordinates; Calculating the adaptive step length of the drone and determining the expansion direction through the environmental features and the grid occupancy probability; The optimal path of the sampling points is calculated through adaptive step size and expansion direction to generate path nodes. Then a path node sequence is obtained through the entire iteration. The path nodes in the path node sequence are smoothed according to the RRT algorithm, and the path nodes are connected to obtain the flight trajectory of the UAV.

4. According to claim 3, a method for indoor UAV positioning and navigation path planning is characterized in that: The environmental features include edge features, plane features and corner features.

5. According to claim 4, a method for indoor UAV positioning and navigation path planning is characterized in that: The local environment map is sampled and planned by the RRT algorithm. Each 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: Each grid area includes grid position coordinates and grid occupancy probability; The sampling probability density is calculated according to each grid area in the local environment map, and the sampling probability density formula is: ; In the formula, It is expressed as environmental characteristics, with a value range of [0,1]; it reflects the richness of the environmental characteristics of the current region; Represented as a target guide item, 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, (x, y, z) represents the position coordinates; w1, w2, w3 are weighted coefficients; Traversing each grid area g in the local environment map, and calculating the sampling probability density of each grid area g; Based on the sampling probability density, the acceptance score Q of each grid area g in the local environment map is calculated. The calculation formula of the acceptance score Q is: ; In the formula, It is represented as the Laplacian of the environmental characteristics; G represents the Euclidean distance from the current grid area coordinates to the target location coordinates; α is the characteristic gain coefficient; β is the distance attenuation coefficient; The calculation formula of the Euclidean distance G from the current grid area coordinates to the target position coordinates is: ; Where (xg, yg, zg) represents the coordinates of the current grid area; (xG, yG, zG) are the target position coordinates; Preset the scoring threshold R; determine whether the acceptance score Q of each grid area is greater than or equal to the scoring threshold R; If not, the grid area is discarded; If yes, the grid area is used as a candidate sampling point; and each grid area is traversed, and the grid areas that meet the requirements of being a candidate sampling point are stored in a sequence table E; The RRT algorithm is used to screen the candidate sampling points of the grid area in the sequence table E again according to the target guide item G (x, y, z), and the grid area closest to the target position coordinates is selected as the final sampling point.

6. According to claim 5, a method for indoor UAV positioning and navigation path planning is characterized in that: The specific operation steps of calculating the adaptive step length of the drone by using the environmental features and the grid occupancy probability are as follows: Calculating an adaptive step length of a search space of the local environment map according to the environmental features of the local environment map and the grid occupancy probability of each grid area in the local environment map; The adaptive step length is optimized to preset a safe distance between the drone and the sampling point target. ; Calculate the step distance between the drone and the sampling point target by the distance between the current drone and the sampling point target and the preset safety distance; Calculating the acceleration factor of the drone using the preset safety distance; Calculate the attitude adjustment value of the drone by the distance between the current drone and the sampling point target and the preset safety distance; The step distance is adjusted by the acceleration factor of the UAV and the attitude adjustment value to obtain an adaptive step distance; and the adaptive step distance is used as the final adaptive step length.

7. According to claim 6, a method for indoor UAV positioning and navigation path planning is characterized in that: The specific steps for determining the expansion direction are as follows: Calculating partial derivatives of the environmental features according to the central difference method to obtain the environmental feature gradient; Calculating 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; Calculating the target direction according to the sampling points of the current grid area coordinates and the target position coordinates; The expansion direction is calculated by using the environment feature gradient, the occupancy probability gradient of the grid area and the target direction.

8. According to claim 7, a method for indoor UAV positioning and navigation path planning is characterized in that: The optimal path of the sampling points is calculated by adaptive step size and expansion direction, and path nodes are generated. Then, a path node sequence is obtained through the entire iteration. The path nodes in the path node sequence are smoothed according to the RRT algorithm, and the path nodes are connected to obtain the flight trajectory of the drone. The specific operation steps are as follows: Generate new sampling points of grid region coordinates from the sampling points of the current grid region coordinates by using the adaptive step size and the expansion direction; Preset an occupation probability threshold v, perform occupation probability verification on the grid occupation probability of the sampling point of the new grid region coordinates, and determine whether the grid occupation probability of the sampling point of the new grid region coordinates is less than the occupation probability threshold v; If not, the sampling point of the new grid area coordinates is determined to be in the object area of ​​the local environment map, and the sampling point of the new grid area coordinates is reselected; If so, the sampling point of the new grid region coordinate is a valid sampling point and serves as a path node; Iterate through the adaptive step size and the expansion direction to generate valid sampling points, use all valid sampling points as path nodes, and generate a path node sequence, where the path node sequence = {g1, g2, g3, g4, ..., gn}; The path node sequence is smoothed by the RRT algorithm, the connection between the path nodes is checked, and the path nodes that can be connected between the path nodes are connected in series to obtain the flight trajectory of the UAV.

9. The indoor UAV positioning and navigation path planning method according to claim 8, characterized in that: The path node sequence is smoothed by the RRT algorithm, the connection between the path nodes is checked, and the path nodes that can be connected between the path nodes are connected in series to obtain the flight trajectory of the drone. The specific operation steps are as follows: Connecting the path node g1 and the path node g3 in the path node sequence with a straight line to form a path segment; judging for each sampling point on the path segment whether the grid occupancy probability of the grid area is less than the occupancy probability threshold v; If not, there is an object at the sampling point 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 yes, 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}; Connect the path node g1 and the path node g4 in a straight line to form a path segment for the new path node sequence = {g1, g3, g4, ..., gn}, and repeat the above steps until the path node closest to the target position coordinates is selected, and update the retained path segment of the path node g1 and the path node g3; The path nodes of the updated retained path segments are used as new starting points to continue the recursive operation of the above steps to obtain all the optimized path nodes; Connect all optimized path nodes to obtain the flight trajectory of the UAV.

10. An indoor UAV positioning and navigation path planning system, characterized in that: include: Data acquisition module; Database module; Estimation module; Flight path module; Optimization 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 image to obtain image feature points, and to establish a temporal correspondence 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 using the temporal correspondence 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 to construct a local environment map for the constructed 3D space using the image feature points; the UAV performs path planning according to the local environment map to generate a flight path for the UAV; The posture optimization module is used to optimize the posture of the UAV based on the flight path of the UAV and the visual semi-direct method to obtain the final posture of the UAV.

Images