Indoor unmanned aerial vehicle autonomous flight positioning navigation method based on navigation system

Abstract

The invention discloses an indoor unmanned aerial vehicle autonomous flight positioning navigation method based on a navigation system, and the method comprises the following steps: 1, obtaining indoor environment three-dimensional point cloud data, building a navigation coordinate system, and endowing each node position with a coordinate value in the navigation coordinate system; 2, the unmanned aerial vehicle obtains the current node position of the unmanned aerial vehicle in the navigation coordinate system, the flight path of the unmanned aerial vehicle from the current node position to the next target node position is planned and obtained by combining an A * algorithm and an artificial potential field algorithm, and meanwhile the actual flight path of the unmanned aerial vehicle is adjusted in real time by adopting a data fusion algorithm based on IMU data. The method has the advantages of being small in data processing calculation amount, high in autonomous flight capacity, high in anti-interference capacity, autonomous and flexible, and suitable for a complex indoor environment.

Description

Technical Field

[0001] The invention relates to the field of unmanned aerial vehicle positioning and navigation methods, and in particular to an indoor unmanned aerial vehicle autonomous flight positioning and navigation method based on a navigation system. Background Art

[0002] In recent years, the application of drone technology in substation inspection has become a major innovative trend in the power industry. Compared with traditional manual inspection, the application of drone in substation inspection not only improves the inspection efficiency and accuracy, but also reduces costs, enhances safety, and is not restricted by terrain, showing its unique advantages in improving efficiency, reducing costs, enhancing safety, adapting to various terrains, and intelligent level. With the continuous development and improvement of technology, drones are playing an increasingly important role in substation inspection.

[0003] At present, substation drone inspections are widely used outdoors, especially in the inspection of substation equipment. However, the application of indoor drone inspections is relatively small. The main reasons and challenges are: the indoor space of the substation is usually narrow, the equipment layout is complex, and there are many obstacles, which limit the free flight of drones. In addition, limited space may cause the flight stability of drones to decrease. There is a strong electromagnetic field inside the substation, which interferes with the navigation, communication and control system of the drone, affecting the flight performance and data collection capabilities of the drone. At the same time, the current drone technology is mainly concentrated in outdoor applications, and indoor navigation, obstacle avoidance and positioning technologies are not yet fully mature, resulting in limited application inside substations.

[0004] In order to solve the above problems, the prior art State Intellectual Property Office published an invention patent with publication number CN117268404B on February 6, 2024, entitled "A method for indoor and outdoor autonomous navigation of an unmanned aerial vehicle using multi-sensor fusion". The invention involves a method for indoor and outdoor autonomous navigation of an unmanned aerial vehicle using multi-sensor fusion. The invention uses low-cost sensors for positioning and mapping; it can comprehensively utilize data from multiple sensors to improve the accuracy and robustness of positioning and mapping, and adapt to a variety of environments, and can perform navigation tasks well both indoors and outdoors.

[0005] The National Intellectual Property Administration issued an invention patent with the publication number CN112629529B on December 6, 2022, entitled "UAV Indoor Autonomous Navigation Method". This invention patent discloses an indoor autonomous navigation method for UAVs. This method is based on the idea of ​​data fusion, relying on a sensor system including a micro-electromechanical inertial sensor combination IMU, a magnetometer, an ultrasonic rangefinder, and a barometric altimeter. It uses a combination of the least squares estimation algorithm and the Kalman filter algorithm to solve the problem of autonomous navigation of small UAVs in the transition stage between outdoor and indoor environments, and at the same time solves the problem of autonomous navigation of small UAVs indoors, especially in environments with narrow passages and insufficient lighting conditions. .

[0006] The National Intellectual Property Office published an invention patent with publication number CN116222542A on June 6, 2023, entitled "A method for autonomously sensing the position of an indoor drone with multi-sensor collaboration". The invention patent discloses a method for autonomously sensing the position of an indoor drone with multi-sensor collaboration. The invention includes extracting static target features and motion features of images captured by the drone camera, calculating the real-time data of the drone's current position, and then obtaining the drone's current position information based on the calculated real-time data and the positioning information obtained using the drone's own positioning device. In this way, image processing technology can be referenced in the acquisition of drone position information, thereby improving the real-time and accuracy of the drone's position information and extracting the static target features of the image.

[0007] However, the above prior art patents have the following problems:

[0008] 1. Multi-sensor data fusion requires a lot of computation. In order to improve positioning accuracy, indoor drones usually rely on multiple sensors (such as lidar, inertial measurement unit, visual sensor, ultrasonic sensor, etc.) for data fusion. The output data of each sensor must undergo complex technical processing, which increases the complexity of the overall system. A large amount of computing resources is required in the data fusion process, especially when running data that needs to be updated at a high frequency. This places extremely high performance requirements on the drone's processor (such as CPU and GPU), which may result in computing delays and increased energy consumption. Due to the rapid changes in the flight environment, data needs to be analyzed and processed in real time. High computing volume may lead to insufficient real-time performance, affecting the flight stability and responsiveness of the drone, thereby affecting safety.

[0009] 2. The flight route is single and cannot be edited. Many drones rely on preset flight paths in indoor environments and lack the ability to dynamically adjust and replan the path in real time. Once an obstacle is encountered or the task needs to be temporarily adjusted, this inflexible navigation strategy will limit the adaptability of the drone. When performing complex tasks (such as inspections, data collection, etc.), the limitations of fixed paths may not meet the changing operational requirements, reducing work efficiency and failing to achieve efficient and intelligent operations.

[0010] 3. Poor anti-interference ability. In indoor environments, due to the presence of reflection, scattering, and weak light, the performance of the sensor may be seriously affected. For example, changes in illumination, wall reflections, or interference signals generated by other devices may cause unstable sensor output, thereby affecting the positioning accuracy of the drone. In a small indoor space, signal multipath propagation effects (such as visual positioning) may occur, resulting in ranging errors, which in turn affects the reliability of the overall positioning. Summary of the invention

[0011] The present invention provides an indoor unmanned aerial vehicle autonomous flight positioning and navigation method based on a navigation system, so as to solve the problems of large amount of data fusion calculation, single and uneditable flight route and poor anti-interference ability in the prior art.

[0012] In order to achieve the above object, the technical solution adopted by the present invention is:

[0013] An indoor unmanned aerial vehicle autonomous flight positioning and navigation method based on a navigation system comprises the following steps:

[0014] Step 1: Acquire the three-dimensional point cloud data of the indoor environment, establish a navigation coordinate system based on the three-dimensional point cloud data of the indoor environment, and assign coordinate values ​​to each node position in the navigation coordinate system;

[0015] Step 2: The UAV obtains its current node position in the navigation coordinate system, and combines the A* algorithm and the artificial potential field algorithm to calculate the total cost obtained by the A* algorithm and the total field calculated by the artificial potential field algorithm. The weighted sum of the comprehensive unit price function is used to construct the next moving node or direction according to the comprehensive cost function, thereby planning the flight path of the UAV from the current node position to the next target node position.

[0016] In a further step 1, the center point of the indoor drone airport take-off and landing platform is selected in the navigation coordinate system as the origin of the coordinate system.

[0017] In the further step 2, based on the navigation coordinate system, the total cost f(n) of the drone driving from the current node position to the next arbitrary node position is calculated by the A* algorithm, and the total field cost f(n) of the drone driving from the current node to the next arbitrary set node is calculated by the artificial potential field algorithm. And set the weight coefficient, and then calculate the comprehensive cost function C(n) as shown below:

[0018]

[0019] Where: m represents the mass of the drone, α∈[0,1] is the weight coefficient, which is used to balance the role of the A algorithm and the artificial potential field method in path planning. The drone selects the next moving node or direction based on this comprehensive cost function.

[0020] Furthermore, when the A* algorithm is used for calculation in step 2, the Euclidean distance between the current node position and the next arbitrary node position is used as the heuristic function, and a path cost function is constructed based on the heuristic function, the flight speed and acceleration of the drone. Finally, the total cost f(n) is obtained by the sum of the heuristic function and the path cost function.

[0021] Furthermore, when the artificial potential field algorithm is used in step 2, the gravitational field and repulsive field of the drone from the current node position to the next arbitrary node position are calculated respectively, and the total field is obtained by the sum of the gravitational field and the repulsive field.

[0022] In the further step 2, the UAV also combines the positioning data obtained by its own IMU and uses a data fusion algorithm to update the position of the UAV in the navigation coordinate system to adjust the flight path of the UAV in real time. The data fusion algorithm process is as follows:

[0023] The UAV obtains its initial state matrix, acceleration, and angular velocity in the body coordinate system based on its own IMU data, and converts the acceleration of the UAV in the body coordinate system into the acceleration in the navigation coordinate system according to the initial state matrix; then the initial state matrix is ​​updated based on the angular velocity, thereby obtaining the updated acceleration in the navigation coordinate system; then based on the UAV's position at the current time t in the navigation coordinate system, the speed at the current time t, and the updated acceleration, the position of the UAV in the navigation coordinate system at the time t+Δt is calculated, thereby completing the update of the UAV's position in the navigation coordinate system.

[0024] The present invention implements and designs an indoor unmanned aerial vehicle autonomous flight positioning and navigation method based on a navigation system, and uses a laser radar or a laser scanner to obtain point cloud data of the indoor environment. By scanning the environment, a high-precision model of the three-dimensional space is generated. Based on the obtained point cloud data, a three-dimensional navigation coordinate system of the indoor scene is constructed, and three-dimensional coordinates are assigned to all objects in the room. The unmanned aerial vehicle combines the information obtained by the laser radar, IMU (inertial measurement unit), etc., and performs algorithm fusion through methods such as Kalman filtering and particle filtering to perform self-coordinate positioning, so that the unmanned aerial vehicle can perform relative positioning and autonomous flight in a complex indoor environment. After setting the target position, the unmanned aerial vehicle can autonomously calculate the flight route through an algorithm, reduce manual intervention, and improve adaptive capabilities.

[0025] The present invention can achieve high-precision autonomous positioning and path planning, and has significant practical application value in substation inspection. With the continuous advancement of technology, the system is expected to play an important role in more industries and fields in the future, and improve the intelligence and automation level of drones. Compared with the existing technology, the present invention has the following advantages:

[0026] (1) The data processing calculation is small, and the navigation coordinate system can update the environmental data in real time, improving the response ability to moving obstacles and environmental changes.

[0027] (2) Strong autonomous flight capability. The flight path of the drone can be edited independently. The drone can fly autonomously based on its own navigation system, reducing its reliance on external signals (such as vision). This improves work efficiency and safety.

[0028] (3) With strong anti-interference capability and autonomous flexibility, laser point cloud technology reduces the impact of visual signal interference on positioning accuracy and is suitable for complex indoor environments. BRIEF DESCRIPTION OF THE DRAWINGS

[0029] Figure 1 It is a flow chart of a method according to an embodiment of the present invention. DETAILED DESCRIPTION

[0030] The present invention is further described below in conjunction with the accompanying drawings and embodiments.

[0031] like Figure 1As shown, this embodiment discloses an indoor unmanned aerial vehicle autonomous flight positioning and navigation method based on a navigation system, which uses a laser radar to scan the indoor environment, obtain three-dimensional point cloud data, and pre-process the obtained three-dimensional point cloud data. A three-dimensional model is constructed based on the point cloud data to form a map of the indoor environment. A navigation coordinate system is defined and established based on the laser point cloud three-dimensional data, and is used to calculate the position of the unmanned aerial vehicle in the coordinate system based on the selection of an appropriate origin and coordinate system direction. The position and attitude of the unmanned aerial vehicle are updated according to the motion equation. The data fusion algorithm is designed to realize the indoor positioning of the unmanned aerial vehicle of the present invention, and a fusion algorithm is constructed according to the sensor characteristics to integrate the IMU and laser point cloud data. The weight coefficient is set to optimize the contribution of different sensor data in the positioning process to realize the indoor positioning of the unmanned aerial vehicle. Path planning and autonomous flight realize the autonomous flight navigation of the unmanned aerial vehicle of the present invention. According to the real-time positioning and environmental model, an autonomous path planning algorithm (such as an A* algorithm or an artificial potential field) is designed to calculate a suitable flight path. A control algorithm is designed to enable the unmanned aerial vehicle to fly autonomously according to the planned path. This embodiment includes the following steps:

[0032] Step 1: Use the UAV's laser radar to obtain the indoor environment 3D point cloud data, establish a navigation coordinate system based on the indoor environment 3D point cloud data as the navigation coordinate system, and assign coordinate values ​​to each node position in the navigation coordinate system. In the navigation coordinate system, select the center point of the indoor UAV airport landing platform as the origin of the coordinate system, and use the UAV yaw direction as the X-axis, the UAV roll direction as the Y-axis, and the UAV pitch direction as the Z-axis.

[0033] Step 2: The UAV obtains its current node position in the navigation coordinate system, and combines the A* algorithm and the artificial potential field algorithm to calculate the total cost obtained by the A* algorithm and the total field calculated by the artificial potential field algorithm. The weighted sum of the IMU is used to construct a comprehensive unit price function, and the next moving node or direction is selected according to the comprehensive cost function, thereby planning the flight path of the drone from the current node position to the next target node position. In addition, the drone also uses a data fusion algorithm based on the IMU data to adjust the actual flight path of the drone in real time.

[0034] Specifically, the calculation process of the A* algorithm in this embodiment is as follows:

[0035] Assume that the current node position coordinate of the drone in the navigation coordinate system is n 1 =(x 1 ,y 1 ,z 1 ), the coordinates of the next arbitrary node position in the navigation coordinate system are n 2 =(x 2 ,y 2 ,z 2 ), and assume that the drone is moving from the current node position n at a speed v1 Move to the next arbitrary node position n 2 , acceleration is a, and moving time is t.

[0036] First, take the current node position n 1 and the next arbitrary node position n 2 The Euclidean distance d(n 1 ,n 2 ) is used as the heuristic function. The calculation formula of the heuristic function is as follows:

[0037]

[0038] Next, based on the heuristic function d(n 1 ,n 2 ), the UAV flight speed v and acceleration a, construct the UAV from the current node position n 1 Drive to the next arbitrary node position n 2 The path cost function g(n 1 ,n 2 ), the calculation formula is as follows:

[0039]

[0040] Where: k 1 and k 2 is a weight coefficient used to balance the costs related to distance and speed-acceleration.

[0041] Finally, the total cost f(n) is obtained by the sum of the heuristic function and the path cost function. The calculation formula is as follows:

[0042] f(n) = g(n 1 ,n 2 )+d(n 1 ,n 2 ).

[0043] In this embodiment, the calculation process of the artificial potential field algorithm is as follows:

[0044] First, calculate the drone Drive from the current node position to the next arbitrary node position The gravitational field is calculated as follows:

[0045]

[0046] where k att is the gravitational constant.

[0047] The gravitational force is Next, calculate the current node position of the drone from the navigation coordinate system Drive to the next arbitrary node position The repulsive field, assuming that the center position of the i-th obstacle is The influence radius is R i .

[0048] The calculation formula is as follows:

[0049]

[0050] in: It is the current node position of the drone in the navigation coordinate system Drive to the next arbitrary node position The repulsive field; Indicates the drone's position from the current node Drive to the next arbitrary node position The center position of the ith obstacle in the path in the navigation coordinate system; R i Indicates the influence radius; k rep is the repulsion coefficient;

[0051] The repulsive force is

[0052] Finally, according to gravity and repulsion Calculate the total force The calculation formula is as follows:

[0053]

[0054] Finally, in the process of flight path planning in this embodiment, the total cost f(n) calculated by the A* algorithm and the total field calculated by the artificial potential field algorithm are summed by weight allocation. Combined, the comprehensive cost function C(n) is calculated as shown below:

[0055]

[0056] Where: m represents the mass of the drone, α∈[0,1] is the weight coefficient, which is used to balance the role of the A algorithm and the artificial potential field method in path planning. The drone selects the next moving node or direction based on this comprehensive cost function. Then the flight path of the drone from the current node position to the next target node position is obtained.

[0057] In this embodiment, when the UAV flies according to the obtained flight path, in order to adjust the actual flight path of the UAV in real time, the positioning data obtained by the UAV's own IMU (i.e., inertial navigation unit) is also combined to update the position of the UAV in the navigation coordinate system using a data fusion algorithm, so that the UAV can perform relative positioning in a complex indoor environment. The data fusion algorithm process is as follows:

[0058] Suppose the initial state matrix of the drone in the body coordinate system obtained based on its own IMU data is R0, which is determined by the initial roll angle φ0, pitch angle θ0, and yaw angle ψ0. And suppose the acceleration of the drone in the body coordinate system obtained based on its own IMU data is The angular velocity is

[0059] According to the initial state matrix R0, the acceleration of the drone in the body coordinate system can be converted is the acceleration in the navigation coordinate system The calculation formula is as follows:

[0060]

[0061] Next, based on the angular velocity ω, the quaternion q of the initial state matrix R0 is calculated as follows: 0 ,q 1 ,q 2 ,q 3 ) to update the state matrix. The updated quaternion The calculation formula is as follows:

[0062]

[0063] The updated state matrix R is shown as follows:

[0064]

[0065] Substitute the updated state matrix R into the formula The updated acceleration can be calculated by R0 in

[0066] Finally, based on the updated acceleration The speed of the drone at the current time t in the navigation coordinate system Calculate the speed of the drone at time t+Δt in the navigation coordinate system

[0067] Then based on the current position of the UAV at time t in the navigation coordinate system The current speed at time t Updated acceleration Calculate the position of the UAV at time t+Δt in the navigation coordinate system This completes the update of the drone's position in the navigation coordinate system. The calculation formula is as follows:

[0068]

[0069] in is the position of the UAV in the navigation coordinate system.

[0070] The preferred embodiments of the present invention are described in detail above in conjunction with the accompanying drawings. The embodiments described in the present invention are merely descriptions of the preferred embodiments of the present invention, and do not limit the concept and scope of the present invention. The various specific technical features described in the above specific embodiments can be combined in any suitable manner without contradiction, and such combinations should also be regarded as the contents disclosed in the present disclosure as long as they do not violate the concept of the present invention. In order to avoid unnecessary repetition, the present invention will not further describe various possible combinations.

[0071] The present invention is not limited to the specific details of the above-mentioned embodiments. Within the technical concept of the present invention and without departing from the design concept of the present invention, various modifications and improvements made to the technical solution of the present invention by technical personnel in this field should fall within the protection scope of the present invention. The technical contents for which protection is sought in the present invention have been fully recorded in the claims.

Claims

1. An indoor UAV autonomous flight positioning and navigation method based on a navigation system, characterized in that: The following steps are involved: Step 1: Acquire the three-dimensional point cloud data of the indoor environment, establish a navigation coordinate system based on the three-dimensional point cloud data of the indoor environment, and assign coordinate values ​​to each node position in the navigation coordinate system; Step 2: The UAV obtains its current node position in the navigation coordinate system, and combines the A* algorithm and the artificial potential field algorithm to calculate the total cost obtained by the A* algorithm and the total field calculated by the artificial potential field algorithm. The weighted sum of the comprehensive unit price function is used to construct the next moving node or direction according to the comprehensive cost function, thereby planning the flight path of the UAV from the current node position to the next target node position.

2. According to the method of autonomous flight positioning and navigation of indoor unmanned aerial vehicle based on navigation system in claim 1, it is characterized in that: In step 1, the center point of the indoor drone airport take-off and landing platform is selected as the origin of the coordinate system in the navigation coordinate system.

3. The method for autonomous flight positioning and navigation of an indoor unmanned aerial vehicle based on a navigation system according to claim 1, characterized in that: In step 2, based on the navigation coordinate system, the A* algorithm is used to calculate the total cost f(n) of the drone driving from the current node position to the next arbitrary node position, and the artificial potential field algorithm is used to calculate the total field cost f(n) of the drone driving from the current node to the next arbitrary set of nodes. And set the weight coefficient, and then calculate the comprehensive cost function C(n) as shown below: Where: m represents the mass of the drone, α∈[0,1] is the weight coefficient, which is used to balance the role of the A algorithm and the artificial potential field method in path planning. The drone selects the next moving node or direction based on this comprehensive cost function.

4. The method for autonomous flight positioning and navigation of an indoor unmanned aerial vehicle based on a navigation system according to claim 2, characterized in that: When the A* algorithm is used for calculation in step 2, the Euclidean distance between the current node position and the next arbitrary node position is used as the heuristic function, and a path cost function is constructed based on the heuristic function, the flight speed and acceleration of the drone. Finally, the total cost f(n) is obtained by the sum of the heuristic function and the path cost function.

5. The method for autonomous flight positioning and navigation of an indoor unmanned aerial vehicle based on a navigation system according to claim 2, characterized in that: When the artificial potential field algorithm is used in step 2, the gravitational field and repulsive field of the drone from the current node position to the next arbitrary node position are calculated respectively, and the total field is obtained by the sum of the gravitational field and the repulsive field.

6. The method for autonomous flight positioning and navigation of an indoor UAV based on a navigation system according to claim 1, characterized in that: In step 2, the drone also combines the positioning data obtained by its own IMU and uses a data fusion algorithm to update the position of the drone in the navigation coordinate system to adjust the flight path of the drone in real time. The data fusion algorithm process is as follows: The drone obtains its initial state matrix, acceleration, and angular velocity in the body coordinate system based on its own IMU data, and converts the acceleration of the drone in the body coordinate system into the acceleration in the navigation coordinate system according to the initial state matrix; then the initial state matrix is ​​updated based on the angular velocity, thereby obtaining an updated acceleration in the navigation coordinate system; Then, based on the current position of the UAV at time t, the current speed at time t, and the updated acceleration in the navigation coordinate system, the position of the UAV at time t+Δt in the navigation coordinate system is calculated, thereby completing the update of the position of the UAV in the navigation coordinate system.

Images