UAV navigation control method based on navigation positioning

Abstract

The present invention relates to the field of unmanned aerial vehicle navigation control technology, and specifically to a method for unmanned aerial vehicle navigation control based on navigation positioning. The invention obtains the speed fluctuation scale of each point cloud segmented object in each direction by analyzing point cloud data; and then obtains the threat range of each point cloud segmented object at each moment; for any direction, according to the maximum offset distance of the unmanned aerial vehicle during each deviation process and the monitored wind force level, obtains the orbital offset scale of the unmanned aerial vehicle; combines the actual wind force and wind field disturbance coefficient at the initial moment to obtain the maximum deviation scale of the unmanned aerial vehicle at each moment; combines the ideal navigation position to obtain the error range of the unmanned aerial vehicle at each moment; obtains the overall collision probability of the unmanned aerial vehicle and the point cloud segmented object; and determines whether the preset route needs to be adjusted. The present invention improves the judgment accuracy of route safety by obtaining the accurate collision probability of the unmanned aerial vehicle on the preset route.

Description

Technical Field

[0001] The present invention relates to the technical field of unmanned aerial vehicle navigation control, and in particular to a unmanned aerial vehicle navigation control method based on navigation positioning. Background Art

[0002] The navigation and positioning-based UAV navigation control technology is widely used in agriculture, aerial survey, logistics, rescue and other fields. By continuously optimizing algorithms, improving navigation accuracy and flight stability, the autonomous flight capability of UAVs in complex environments has been significantly enhanced.

[0003] In the prior art, navigation is performed based on the relevant navigation algorithms and navigation and positioning technologies of the UAV. However, when the UAV is driving based on navigation and positioning, it is not considered that the obstacle avoidance and route planning capabilities in the dynamic environment in which it is located are easily disturbed by the dynamic changes of the external environment, and there are influences such as wind fields and obstacle movements, which causes errors in the recognition of the collision state between the UAV and objects in the surrounding environment, resulting in poor accuracy in the assessment and judgment of the safety of the UAV's expected route during navigation. Summary of the invention

[0004] In order to solve the technical problem of errors in the identification of the collision state between a UAV and objects in the surrounding environment, the present invention aims to provide a UAV navigation control method based on navigation positioning, and the technical solution adopted is as follows:

[0005] The present invention proposes a navigation control method for an unmanned aerial vehicle based on navigation positioning, the method comprising:

[0006] Based on the preset route, obtain the point cloud data of the drone's environment, the monitored wind speed at each moment in different preset directions, and the maximum offset distance during each deviation process;

[0007] A plurality of point cloud segmentation objects corresponding to the point cloud data are obtained, and speed data of each point cloud segmentation object in each direction at each time is determined; a speed fluctuation scale of each point cloud segmentation object in each direction is obtained according to the speed data distribution of each point cloud segmentation object in each direction; a threat range of each point cloud segmentation object at each time is obtained according to the speed fluctuation scale of each point cloud segmentation object in different directions and the speed data at each time;

[0008] For any direction, the actual wind force of the drone at each moment is obtained according to the monitored wind force and preset flight wind resistance of the drone at each moment, and the wind field disturbance coefficient of the drone is obtained; the orbital deviation scale of the drone is obtained according to the maximum deviation distance and monitored wind force level of the drone during each deviation process; the maximum deviation scale of the drone at each moment is obtained according to the orbital deviation scale of the drone, the actual wind force at the initial moment, and the wind field disturbance coefficient; the error range of the drone at each moment is obtained according to the maximum deviation scale of the drone in different directions at each moment and the ideal navigation position;

[0009] According to the distribution of the threat range of different point cloud segmentation objects and the error range of the drone at each moment within the preset time range, the overall collision probability between the drone and the point cloud segmentation object is obtained;

[0010] It is determined whether the preset route needs to be adjusted according to the overall collision probability.

[0011] Furthermore, the method for obtaining the speed fluctuation scale includes:

[0012] According to the fluctuation degree of all speed data of each point cloud segmentation object in each direction, the speed fluctuation scale of each point cloud segmentation object in each direction is obtained, and the fluctuation degree is positively correlated with the speed fluctuation scale.

[0013] Furthermore, the method for obtaining the threat range includes:

[0014] For any direction, the speed data at each moment is used as the simulated uniform speed data, and the position of each point cloud segmentation object at each moment in each direction is obtained based on the uniform motion formula to form the spatial position of each point cloud segmentation object at each moment;

[0015] Calculate the product of the velocity fluctuation scale and the corresponding time in each direction of each point cloud segmented object as the displacement deviation;

[0016] The spatial position of each point cloud segmentation object at each moment is taken as the center of the fitting ellipse, the displacement deviation in different directions is taken as the radius of the fitting ellipse, and the corresponding fitting ellipse area is taken as the threat range of each point cloud segmentation object at each moment.

[0017] Furthermore, the method for obtaining the actual wind force includes:

[0018] The difference between the monitored wind force of the UAV in each direction at each moment and the preset flight wind resistance is calculated as the actual wind force of the UAV in each direction at each moment.

[0019] Furthermore, the method for obtaining the wind field disturbance coefficient includes:

[0020] For any direction, the wind field disturbance coefficient of the UAV in the corresponding direction is obtained based on the difference in actual wind force between the UAV at each moment and the adjacent previous moment, and the difference is positively correlated with the wind field disturbance coefficient.

[0021] Furthermore, the method for obtaining the track deviation scale includes:

[0022] For any direction, the average monitored wind level of the UAV during each deviation process is obtained; based on the maximum deviation distance of the UAV during each deviation process and the average monitored wind level, the track deviation distance of the UAV in each direction is obtained. The maximum deviation distance is positively correlated with the track deviation distance, and the average monitored wind level is negatively correlated with the track deviation distance.

[0023] Furthermore, the method for obtaining the maximum deviation scale includes:

[0024] For any direction, calculate the product between the wind field disturbance coefficient of the UAV and the time, and obtain the wind field disturbance scale of the UAV at the corresponding time in each direction;

[0025] The sum of the wind field disturbance scale of the UAV at the corresponding moment and the initial actual wind force is obtained as the updated actual wind force;

[0026] The orbital deviation scale of the UAV is weighted according to the updated actual wind force to obtain the maximum deviation scale of the UAV in each direction at each moment.

[0027] Furthermore, the method for obtaining the error range includes:

[0028] The ideal navigation position of the UAV at each moment is taken as the center of the fitting ellipse, the maximum deviation scale of the UAV in different directions at each moment is taken as the radius of the fitting ellipse, and the corresponding fitting ellipse area is taken as the error range of the UAV at each moment.

[0029] Furthermore, the method for obtaining the overall collision probability includes:

[0030] At any moment, if the threat range of the point cloud segmented object and the error range of the drone do not have an intersection, the local collision probability is 0;

[0031] If there is an intersection between the threat range of the point cloud segmented object and the error range of the drone, obtain a first probability value corresponding to the probability density function of the intersection in the threat range, obtain a second probability value corresponding to the probability density function of the intersection in the error range, and calculate the product of the first probability value and the second probability value as the corresponding local collision probability;

[0032] If the threat range of the point cloud segmented object intersects with the error range of the drone and is greater than one intersection point, a first ratio of the intersection range to the threat range is obtained, a second ratio of the intersection range to the error range is obtained, and the product of the first ratio and the second ratio is calculated to obtain the corresponding local collision probability;

[0033] According to the local collision probability between the threat range of different point cloud segmentation objects and the error range of the drone at each moment within the preset time range, the overall collision probability between the drone and the point cloud segmentation object is obtained. The local collision probability is positively correlated with the overall collision probability.

[0034] Further, judging whether the preset route needs to be adjusted according to the overall collision probability includes:

[0035] If the overall collision probability is greater than the preset collision threshold, it is determined that the preset route needs to be adjusted.

[0036] The present invention has the following beneficial effects:

[0037] The present invention takes into account that the UAV may be affected by the movement of objects in the external environment during the navigation of the preset route, obtains multiple point cloud segmentation objects corresponding to the point cloud data, determines the speed data of each point cloud segmentation object in each direction at each moment, and analyzes the motion trajectory and dynamic characteristics of a single object in a more targeted manner by decomposing complex scenes into independent objects; according to the speed data distribution of each point cloud segmentation object in each direction, the speed fluctuation scale of each point cloud segmentation object in each direction is obtained, the possible motion range of the object is determined, and the motion safety and stability of the object are evaluated; according to the speed fluctuation scale of each point cloud segmentation object in different directions and the speed data at each moment, the threat range of each point cloud segmentation object at each moment is obtained, and the spatial area where the object may move at each moment is estimated; because the navigation trajectory is easily disturbed by the wind field during the actual operation of the UAV, for any direction, according to the monitored wind force of the UAV at each moment and the preset flight wind resistance, the speed of the UAV at each moment is obtained. Actual wind force, and obtain the wind field disturbance coefficient of the drone; in order to more accurately analyze the deviation of the drone caused by the external wind force, according to the maximum deviation distance of the drone during each deviation and the monitored wind force level, the orbital deviation scale of the drone is obtained; according to the orbital deviation scale of the drone, the actual wind force at the initial moment and the wind field disturbance coefficient, the maximum deviation scale of the drone at each moment is obtained, reflecting the orbital stability of the drone when it is affected by wind force during the actual flight process; according to the maximum deviation scale of the drone in different directions and the ideal navigation position at each moment, the error range of the drone at each moment is obtained, and the deviation range of the drone relative to the ideal navigation position is accurately analyzed; according to the distribution between the threat range of different point cloud segmentation objects at each moment within the preset time range and the error range of the drone, the overall collision probability of the drone and the point cloud segmentation object is obtained, and the collision risk of the drone with objects in the environment on the preset route is more accurately evaluated; it is judged whether the preset route needs to be adjusted. The present invention improves the judgment accuracy of route safety by obtaining the accurate collision probability of the drone on the preset route. BRIEF DESCRIPTION OF THE DRAWINGS

[0038] In order to more clearly illustrate the technical solutions and advantages in the embodiments of the present invention or the prior art, the drawings required for use in the embodiments or the prior art descriptions are briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative work.

[0039] Figure 1 A flowchart of a method for controlling a drone based on navigation and positioning according to an embodiment of the present invention;

[0040] Figure 2A flow chart of a method for acquiring a threat range provided by an embodiment of the present invention;

[0041] Figure 3 A flow chart of a method for obtaining a maximum deviation scale provided by an embodiment of the present invention;

[0042] Figure 4 A flow chart of a method for obtaining overall collision probability provided by an embodiment of the present invention. DETAILED DESCRIPTION

[0043] In order to further explain the technical means and effects adopted by the present invention to achieve the predetermined invention purpose, the specific implementation method, structure, features and effects of the unmanned aerial vehicle navigation control method based on navigation positioning proposed by the present invention are described in detail below in combination with the accompanying drawings and preferred embodiments. In the following description, different "one embodiment" or "another embodiment" does not necessarily refer to the same embodiment. In addition, specific features, structures or characteristics in one or more embodiments may be combined in any suitable form.

[0044] Unless defined otherwise, all technical and scientific terms used herein have the same meaning as commonly understood by one of ordinary skill in the art to which this invention belongs.

[0045] The specific scheme of the unmanned aerial vehicle navigation control method based on navigation and positioning provided by the present invention is described in detail below with reference to the accompanying drawings.

[0046] See also Figure 1 , which shows a flow chart of a method for controlling a drone based on navigation and positioning according to an embodiment of the present invention, wherein the specific method includes:

[0047] Step S1: Based on the preset route, obtain the point cloud data of the environment in which the drone is located, the monitored wind force at each moment in different preset directions, and the maximum offset distance during each deviation process.

[0048] In an embodiment of the present invention, during the navigation of the UAV, the navigation and positioning technology provides clear navigation instructions for the UAV by pre-calibrating the preset position. Once the relevant instructions are received, the UAV will start the autonomous navigation mode, monitor the dynamic environment in real time, and intelligently plan and select the best preset track to ensure accurate and efficient arrival at the preset location. In order to ensure the stability and safety of the UAV's autonomous navigation according to the navigation position, multiple types of sensors are configured on the UAV to collect environmental data in real time, analyze the motion state and environmental conditions of the UAV during navigation, and ensure the safe flight of the UAV.

[0049] Firstly, by configuring a laser radar scanning device on the drone to obtain point cloud data in the environment, and by configuring GPS and a barometer, the longitude, latitude and altitude information of the drone can be accurately determined. When the drone deviates from the preset route or position, the drone system can sense the deviation distance in real time. By configuring anemometers to monitor the wind speed in different directions during flight, various information such as the speed of the drone can also be obtained. Based on the preset route, the point cloud data of the drone's environment, the monitored wind speed at each moment in different preset directions, and the maximum offset distance during each deviation are obtained. Among them, the preset directions are the x-axis direction from due west to due east, the y-axis direction from due south to due north, and the height z-axis direction in three-dimensional space.

[0050] It should be noted that, in one embodiment of the present invention, point cloud data refers to a set of vectors in a three-dimensional coordinate system, including three-dimensional coordinates; after acquiring the point cloud data in the environment, in order to improve the efficiency and accuracy of subsequent processing, the point cloud data will be pre-processed, including denoising, filtering and simplification methods; the specific means are technical means well known to those skilled in the art and will not be elaborated here.

[0051] Step S2: obtain multiple point cloud segmentation objects corresponding to the point cloud data, and determine to obtain the speed data of each point cloud segmentation object in each direction at each moment; obtain the speed fluctuation scale of each point cloud segmentation object in each direction according to the speed data distribution of each point cloud segmentation object in each direction; obtain the threat range of each point cloud segmentation object at each moment according to the speed fluctuation scale of each point cloud segmentation object in different directions and the speed data at each moment.

[0052] Since the flight trajectory of the drone may be disturbed by the movement of external obstacles while it is navigating on the preset route, it is necessary to analyze the environment in which the drone is located, obtain multiple independent objects based on the characteristics of the point cloud data, analyze the movement of each point cloud segmented object, determine the threat range of the point cloud segmented object at each moment, and better understand and evaluate the movement state of objects in dynamic environments and the potential risks to drones.

[0053] The original point cloud data often contains a large number of points. Directly processing the data will consume a lot of computing resources. Through point cloud segmentation, the point cloud data can be segmented into different objects or areas, thereby improving the speed and efficiency of data processing and accurately identifying and understanding objects in the environment. Therefore, multiple point cloud segmentation objects corresponding to the point cloud data are obtained.

[0054] It should be noted that point cloud segmentation can classify point cloud data according to the characteristics of different objects to which it belongs, thereby realizing the recognition and classification of multiple objects. In one embodiment of the present invention, the method for obtaining point cloud segmentation objects includes: using existing regional growth, density-based clustering or segmentation and merging methods to segment different objects in point cloud data, obtain corresponding multiple point cloud segmentation objects, detect and identify obstacles in the environment in real time, and provide important information for the route planning and obstacle avoidance of the drone. The specific means are technical means well known to those skilled in the art and will not be repeated here.

[0055] Point cloud data provides the position information of objects at different times. The position of each point cloud segmented object in three-dimensional space is clear, and its position change can reflect the motion state of the object. Then, the speed data of the point cloud segmented object in different directions can be obtained to more accurately analyze the motion trajectory and dynamic characteristics of the object. Therefore, the speed data of each point cloud segmented object in each direction at each time is determined.

[0056] It should be noted that the speed of the point cloud segmentation object can be estimated by analyzing the coordinate change of the point in the point cloud data or the motion estimation algorithm. In one embodiment of the present invention, the method for obtaining the speed data includes: in any direction, calculating the ratio of the position change of the point cloud data of each point cloud segmentation object at each moment and the adjacent previous moment to the difference at the corresponding moment, as the speed data of each point cloud segmentation object at each moment in each direction. The specific means are well known to those skilled in the art and will not be elaborated here.

[0057] Among them, the greater the position change, the greater the rate of movement of the point cloud segmentation object between adjacent moments, and the larger the speed data; the greater the time difference, the longer the time required for the point cloud segmentation object to run, and the smaller the speed data.

[0058] Due to the different physical properties and motion states of different objects, the speed distribution in different directions will be different. By analyzing the speed data distribution of the point cloud segmented object in each direction, the motion state of the object can be identified and the object's motion safety and stability can be evaluated. The more uneven the speed data distribution, the more unstable the object's motion and the larger the speed fluctuation scale. Therefore, according to the speed data distribution of each point cloud segmented object in each direction, the speed fluctuation scale of each point cloud segmented object in each direction is obtained.

[0059] Preferably, in one embodiment of the present invention, the method for obtaining the speed fluctuation scale includes:

[0060] According to the fluctuation degree of all speed data of each point cloud segmentation object in each direction, the speed fluctuation scale of each point cloud segmentation object in each direction is obtained, and the fluctuation degree is positively correlated with the speed fluctuation scale.

[0061] Among them, the positive correlation means that the dependent variable will increase as the independent variable increases, and the dependent variable will decrease as the independent variable decreases, that is, the greater the degree of fluctuation, the more inconsistent the change in speed data, and the larger the scale of speed fluctuation.

[0062] In one embodiment of the present invention, the formula for the speed fluctuation scale is expressed as:

[0063]

[0064] Among them, σ i,x M represents the velocity fluctuation scale of the i-th point cloud segmentation object in the x-axis direction; i Indicates the number of velocity data of the i-th point cloud segmentation object; v x,i,p Indicates the pth velocity data of the i-th point cloud segmentation object in the x-axis direction; It represents the mean of all velocity data of the i-th point cloud segmentation object in the x-axis direction; norm() represents the normalization function.

[0065] In the formula of the speed fluctuation scale, the degree of fluctuation is expressed by calculating the variance of all speed data. The larger the variance, the greater the degree of fluctuation. The greater the fluctuation of the speed data of the i-th point cloud segmentation object in the x-axis direction, the more uneven the distribution, and the larger the speed fluctuation scale.

[0066] The speed fluctuation scale reflects the stability of the object's motion. Larger fluctuations may mean increased uncertainty in the object's motion, thereby increasing its threat range. Combined with the speed data at each moment, the object's real-time motion state can be understood. Objects moving at high speeds have greater kinetic energy and potential destructive power, so their threat range will also increase accordingly. Therefore, based on the speed fluctuation scale of each point cloud segmentation object in different directions and the speed data at each moment, the threat range of each point cloud segmentation object at each moment is obtained.

[0067] Preferably, in one embodiment of the present invention, the method for obtaining the threat range is as follows: Figure 2 , which shows a flow chart of a method for acquiring a threat range provided by an embodiment of the present invention, the method comprising:

[0068] Step S201: For any direction, the speed data at each moment is used as simulated uniform speed data, and the position of each point cloud segmentation object at each moment in each direction is obtained based on the uniform motion formula to form the spatial position of each point cloud segmentation object at each moment.

[0069] In one embodiment of the present invention, for the x-axis direction, the uniform motion formula is expressed as: x(t)=x0+v xt; x(t) represents the position of the point cloud segmentation object at the tth moment in the x-axis direction; x0 represents the initial position of the point cloud segmentation object in the x-axis direction; v x It represents the velocity data of the point cloud segmented object at the tth moment in the x-axis direction, that is, the simulated uniform speed data; t represents the time.

[0070] Based on this, the position of the point cloud segmentation object at each moment in all directions is obtained, that is, the spatial position (x(t), y(t), z(t)) of the point cloud segmentation object at each moment.

[0071] Step S202: Calculate the product of the velocity fluctuation scale and the corresponding time in each direction of each point cloud segmented object as the displacement deviation.

[0072] The velocity fluctuation scale reflects the range and uncertainty of the velocity change of an object in a certain direction; the product of the time and the velocity fluctuation scale can evaluate the additional displacement that may be caused by the velocity fluctuation within the time range; the displacement deviation is used to quantify the impact of velocity fluctuation on the prediction of the object's position. The larger the velocity fluctuation scale, the larger the corresponding time and the larger the displacement deviation.

[0073] Step S203: The spatial position of each point cloud segmentation object at each moment is used as the center of the fitting ellipse, the displacement deviation in different directions is used as the radius of the fitting ellipse, and the corresponding fitting ellipse area is used as the threat range of each point cloud segmentation object at each moment.

[0074] Since the uncertainty of the motion trajectory and position prediction of an object in three-dimensional space is usually closer to an ellipse, fitting an ellipse provides an intuitive and effective method to reflect the position uncertainty differences caused by the speed fluctuations of the object in different directions, as well as the spatial range that the object may reach, and can fully describe the threat range of the object at the corresponding moment. The specific ellipse fitting method is a technical means well known to those skilled in the art and will not be described in detail here.

[0075] Step S3: For any direction, the actual wind force of the UAV at each moment is obtained according to the monitored wind force and preset flight wind resistance of the UAV at each moment, and the wind field disturbance coefficient of the UAV is obtained; the orbital deviation scale of the UAV is obtained according to the maximum deviation distance and the monitored wind force level of the UAV during each deviation process; the maximum deviation scale of the UAV at each moment is obtained according to the orbital deviation scale of the UAV, the actual wind force at the initial moment and the wind field disturbance coefficient; the error range of the UAV at each moment is obtained according to the maximum deviation scale of the UAV in different directions at each moment and the ideal navigation position.

[0076] During the actual operation of the UAV, due to the diversity of the flight environment, its navigation trajectory is easily disturbed by the wind field, resulting in trajectory deviation. When the UAV is operating on the preset route, if it encounters external wind direction disturbances, its navigation trajectory is likely to change, thereby increasing the risk of collision. Therefore, it is necessary to analyze the changes in wind force during the movement of the UAV, obtain the deviation of the UAV when affected by wind force, and accurately quantify the potential impact of the external environment on the UAV's navigation trajectory.

[0077] During the flight process, the drone needs to know the wind conditions of the surrounding environment in real time in order to carry out precise flight control and mission execution. Wind is one of the important factors affecting the flight stability and safety of the drone. By monitoring the actual wind force, the drone can evaluate the impact of subsequent flight conditions on the deviation of the drone. Therefore, for any direction, the actual wind force of the drone at each moment is obtained based on the monitored wind force and the preset flight wind resistance of the drone at each moment.

[0078] Preferably, in one embodiment of the present invention, the method for obtaining the actual wind force includes:

[0079] Calculate the difference between the monitored wind force and the preset flight wind resistance of the drone in each direction at each moment as the actual wind force of the drone in each direction at each moment. The greater the preset flight wind resistance, the smaller the monitored wind force and the smaller the actual wind force.

[0080] It should be noted that, in one embodiment of the present invention, the preset formula for obtaining the flight wind resistance is: Among them, f x ’ ,l represents the flight wind resistance; C represents the air resistance coefficient, which is 0.5; p represents the air density at the flight altitude of the UAV at the first moment; S represents the windward area of ​​the UAV at the first moment; V x,l It represents the speed of the drone at the lth moment in the x-axis direction. The corresponding data are all obtained in advance by the implementers based on the known reference data and the navigation conditions of the drone, and will not be described in detail here.

[0081] The magnitude and direction of wind force will change with time and space. The influence of wind force on the flight stability of the drone can be quantified by the change trend of the actual wind force of the drone at different times. The greater the change trend, the greater the wind field disturbance coefficient. The wind field disturbance coefficient of the drone is obtained.

[0082] Preferably, in one embodiment of the present invention, the method for obtaining the wind field disturbance coefficient includes:

[0083] For any direction, the wind field disturbance coefficient of the UAV in the corresponding direction is obtained based on the difference in actual wind force between the UAV at each moment and the adjacent previous moment, and the difference is positively correlated with the wind field disturbance coefficient.

[0084] Among them, the positive correlation means that the dependent variable will increase as the independent variable increases, and the dependent variable will decrease as the independent variable decreases, that is, the greater the difference, the greater the difference in actual wind force at adjacent moments, the more unstable the wind force change, and the greater the wind field disturbance coefficient.

[0085] In one embodiment of the present invention, the formula for the wind field disturbance coefficient is expressed as:

[0086]

[0087] Among them, FL x represents the wind field disturbance coefficient of the UAV in the x-axis direction; ML represents the number of moments; ΔFZ x.l It represents the difference between the actual wind force of the UAV in the x-axis direction between the lth moment and the previous moment; || represents the absolute value.

[0088] In the formula of wind field disturbance coefficient, It means calculating the mean of the square of the absolute value of the difference between the actual wind force of the drone at all times and the previous moment in the x-axis direction. The larger the mean, the larger the square of the absolute value of the difference, that is, the greater the difference in actual wind force between moments, the more inconsistent the influence of wind force, and the greater the wind field disturbance coefficient.

[0089] Because the UAV is affected by the external wind and causes the flight track to deviate, the adjustment system of the UAV will automatically generate a reverse resistance force to counteract the interference of the wind and return the UAV to the preset route. The greater the maximum deviation distance, the greater the track deviation scale; the greater the monitored wind force, the more likely the UAV will generate a greater reverse resistance force, and the smaller the track deviation scale; therefore, the track deviation scale of the UAV is obtained based on the maximum deviation distance and the monitored wind level of the UAV during each deviation process.

[0090] Preferably, in one embodiment of the present invention, the method for obtaining the track deviation scale includes:

[0091] For any direction, the average monitored wind level of the UAV during each deviation process is taken; based on the maximum deviation distance of the UAV during each deviation process and the average monitored wind level, the track deviation distance of the UAV in each direction is obtained. The maximum deviation distance is positively correlated with the track deviation distance, and the average monitored wind level is negatively correlated with the track deviation distance.

[0092] Among them, the positive correlation relationship indicates that the dependent variable will increase as the independent variable increases, and the dependent variable will decrease as the independent variable decreases, that is, the larger the maximum deviation distance, the larger the track deviation scale; the negative correlation relationship indicates that the dependent variable will decrease as the independent variable increases, and the dependent variable will increase as the independent variable decreases, that is, the greater the average monitored wind level, the greater the resistance generated and the smaller the track deviation scale.

[0093] It should be noted that, in some embodiments of the present invention, the average monitored wind level can be obtained by accumulating the monitored wind forces at all times during each deviation process and finding the average value to obtain the average monitored wind level; it can also be obtained by obtaining a fitting function of the monitored wind forces at certain moments, integrating the fitting function within the time range of each deviation process, and calculating the value per unit time, that is, calculating the ratio of the integral result and the corresponding time range to obtain the average monitored wind level; the greater the monitored wind force at each moment, the greater the average monitored wind level.

[0094] In one embodiment of the present invention, the formula for the track deviation scale is expressed as:

[0095]

[0096] Among them, PF x represents the track deviation scale of the UAV in the x-axis direction; MC represents the number of deviations of the UAV; pl x,m Indicates the maximum offset distance of the UAV during the mth deviation in the x-axis direction; t m represents the time range of the mth deviation process; f x (t) represents the fitting function of monitoring wind force.

[0097] In the formula for orbital deviation scale, The fitting function representing the monitored wind force during the mth deviation is integrated. The larger the value, the greater the monitored wind force during the deviation. It indicates the monitored wind force within the unit time, i.e. the average monitored wind force level; It represents the ratio between the maximum deviation distance and the average monitored wind level during the calculation of the mth deviation. The larger the average monitored wind level, the greater the reverse resistance force generated to counteract the wind interference, and the smaller the track deviation scale. It means that the ratio of all deviation processes is averaged, that is, the track deviation distance. The larger the average value, the larger the maximum deviation distance, the smaller the average monitored wind level, and the larger the track deviation scale.

[0098] It should be noted that, in the embodiment of the present invention, existing fitting methods such as least squares method or polynomial fitting can be used to fit the monitored wind force at all times to obtain a fitting function. The specific means are well known to those skilled in the art and will not be described in detail here.

[0099] The orbital deviation scale is an important parameter to quantify the degree of deviation of the UAV from the predetermined orbit, reflecting the stability and control performance of the UAV during flight. Wind force is one of the important factors affecting the flight stability and trajectory of the UAV. The actual wind force at the initial moment provides basic data for the environmental conditions when the UAV takes off. The wind field is constantly changing, and its degree of change can be quantified by the wind field disturbance coefficient, reflecting the potential impact of the wind field on the flight trajectory of the UAV. By calculating the wind field disturbance coefficient, the wind force that the UAV may be affected by at different flight stages can be evaluated. Combined analysis can be used to more comprehensively evaluate the maximum deviation scale of the UAV. According to the orbital deviation scale of the UAV, the actual wind force at the initial moment, and the wind field disturbance coefficient, the maximum deviation scale of the UAV at each moment is obtained.

[0100] Preferably, in one embodiment of the present invention, the method for obtaining the maximum deviation scale is as follows: Figure 3 , which shows a flow chart of a method for obtaining a maximum deviation scale provided by an embodiment of the present invention, the method comprising:

[0101] Step S301: For any direction, calculate the product between the wind field disturbance coefficient of the UAV and the time, and obtain the wind field disturbance scale of the UAV at the corresponding time in each direction.

[0102] The wind field disturbance coefficient reflects the potential impact of the wind field on the flight trajectory of the UAV. By multiplying the wind field disturbance coefficient by the time, we can quantify the impact of the wind field on the UAV at each moment, that is, the wind field disturbance scale. This helps to understand the specific impact of the wind field on the UAV's flight trajectory in different time periods.

[0103] Step S302: Obtain the sum of the wind field disturbance scale of the UAV at the corresponding moment and the actual wind force at the initial moment as the updated actual wind force.

[0104] The actual wind force at the initial moment represents the wind conditions in the environment when the UAV takes off, and the wind field disturbance scale represents the additional wind impact caused by the dynamic changes in the wind field. By adding the wind field disturbance scale and the initial actual wind force, a value that more accurately and dynamically reflects the actual wind force in the navigation environment is obtained.

[0105] Step S303: weighting the orbital deviation scale of the UAV according to the updated actual wind force to obtain the maximum deviation scale of the UAV in each direction at each moment.

[0106] In one embodiment of the present invention, a method for obtaining the maximum deviation scale includes:

[0107] LP x,max (t) = PF x ×(FZ x,0 +FL x ×t);

[0108] Among them, LP x,max (t) represents the maximum deviation scale of the UAV in the x-axis direction at the tth moment; PF x Indicates the track deviation scale of the drone in the x-axis direction; FZ x,0 Indicates the actual wind force of the drone in the x-axis direction at the initial moment; FL x represents the wind field disturbance coefficient of the UAV in the x-axis direction; t represents the corresponding time.

[0109] In the formula for the maximum deviation scale, FL x ×t represents the product of the wind field disturbance coefficient of the UAV and the time, that is, the wind field disturbance scale. The larger the wind field disturbance scale, the larger the wind field disturbance coefficient, and the greater the environmental wind interference; FZ x,0 +FL x ×t represents the sum of the wind field disturbance scale of the UAV at the corresponding moment and the actual wind force at the initial moment, that is, updating the actual wind force. The larger the updated actual wind force, the larger the wind field disturbance scale and the initial actual wind force, which will have a continuous offset effect on the UAV, and the greater the impact on the track deviation scale, and the larger the maximum deviation scale.

[0110] In actual flight, due to factors such as the inaccuracy of the navigation system, environmental interference, and limitations of the control algorithm, the actual navigation trajectory of the UAV often cannot completely coincide with the ideal navigation position, resulting in displacement deviation. By analyzing the maximum deviation scale in different directions, the range of changes after being affected is evaluated; according to the maximum deviation scale of the UAV in different directions and the ideal navigation position at each moment, the error range of the UAV at each moment is obtained.

[0111] Preferably, in one embodiment of the present invention, the method for obtaining the error range includes:

[0112] The ideal navigation position of the UAV at each moment is taken as the center of the fitting ellipse, the maximum deviation scale of the UAV in different directions at each moment is taken as the radius of the fitting ellipse, and the corresponding fitting ellipse area is taken as the error range of the UAV at each moment.

[0113] Step S4: According to the distribution between the threat range of different point cloud segmentation objects and the error range of the drone at each moment within the preset time range, the overall collision probability between the drone and the point cloud segmentation object is obtained.

[0114] The threat range reflects the possible collision space between the object and the drone. The error range of the drone determines the extent to which it may deviate from the scheduled route during navigation. By considering the distribution of the threat range of different point cloud segmentation objects at each moment in the time range, the collision probability between the drone and the point cloud segmentation object can be comprehensively and continuously evaluated. Therefore, according to the distribution between the threat range of different point cloud segmentation objects and the error range of the drone at each moment in the preset time range, the overall collision probability between the drone and the point cloud segmentation object is obtained.

[0115] Preferably, in one embodiment of the present invention, the method for obtaining the overall collision probability can be found in Figure 4 , which shows a flow chart of a method for obtaining an overall collision probability provided by an embodiment of the present invention, the method comprising:

[0116] Step S401: At any moment, if the threat range of the point cloud segmented object and the error range of the drone do not have an intersection, the local collision probability is 0.

[0117] There is no intersection between the threat range of the point cloud segmented object and the error range of the drone, that is, they do not overlap in space, so collision is impossible.

[0118] Step S402: If there is an intersection between the threat range of the point cloud segmented object and the error range of the drone, obtain a first probability value corresponding to the probability density function of the intersection in the threat range, obtain a second probability value corresponding to the probability density function of the intersection in the error range, and calculate the product of the first probability value and the second probability value as the corresponding local collision probability.

[0119] When there is an intersection between the threat range of the point cloud segmented object and the error range of the drone, there is a possibility of collision. In order to quantify this possibility, the probability density function is used to represent the density or intensity of the threat range of the point cloud segmented object and the error range of the drone at the intersection. The product of the probability density values ​​of the two ranges at the intersection is calculated to quantify the possibility of a collision at the intersection, that is, the local collision probability.

[0120] It should be noted that the probability density function of the corresponding range is the density function of the three-dimensional random variable (x, y, z) obeying the three-dimensional normal distribution, which describes the distribution in the three-dimensional space; the specific probability density function is a technical means well known to technical personnel in this field and will not be elaborated here.

[0121] Step S403: If the threat range of the point cloud segmented object intersects with the error range of the drone and is greater than one intersection point, obtain a first ratio of the intersection range to the threat range, obtain a second ratio of the intersection range to the error range, calculate the product of the first ratio and the second ratio, and obtain the corresponding local collision probability.

[0122] The intersection range reflects the range in which the threat range and the error range coexist. By calculating the area ratio between the intersection range and the two ranges, obtaining the first ratio and the second ratio, and taking the product of the ratios, a local collision probability related to the size of the intersection range can be intuitively obtained.

[0123] Step S404: According to the local collision probability between the threat range of different point cloud segmentation objects and the error range of the drone at each moment within the preset time range, the overall collision probability between the drone and the point cloud segmentation object is obtained, and the local collision probability is positively correlated with the overall collision probability.

[0124] Among them, the positive correlation means that the dependent variable will increase as the independent variable increases, and the dependent variable will decrease as the independent variable decreases, that is, the larger the local collision probability, the greater the overall collision probability is when all local collision probabilities are accumulated to obtain the overall collision probability.

[0125] It should be noted that, in one embodiment of the present invention, the preset time range is one minute of navigation; in other embodiments of the present invention, the size of the preset time range can be set according to specific circumstances, and is not limited or elaborated here.

[0126] During the flight of a drone, its position, speed, direction and other parameters will change over time. Therefore, the probability of collision with objects segmented by different point clouds will also change over time. Obtaining the overall collision probability within a preset time range is an important indicator for evaluating the navigation safety of the drone.

[0127] In one embodiment of the present invention, the formula for the overall collision probability is expressed as:

[0128]

[0129] Among them, P represents the overall collision probability between the drone and the point cloud segmented object; PU i,t PW represents the threat range of the i-th point cloud segmentation object at the t-th moment; t represents the error range of the drone at the tth moment; IM represents the number of point cloud segmentation objects; MT represents the number of moments within the preset time range; d(PU i,t ,PW t ) represents the local collision probability between the threat range of the i-th point cloud segmentation object at the t-th moment and the error range of the UAV at the t-th moment.

[0130] In the formula for the overall collision probability, It means that at all times, the cumulative sum of the local collision probabilities between the threat range of the i-th point cloud segmentation object and the error range of the drone is calculated. It represents the cumulative sum of the local collision probabilities between the threat range of all point cloud segmented objects at all times and the error range corresponding to the drone. The larger the cumulative sum, the greater the local collision probability, and the more likely there will be more collision probabilities.

[0131] Step S5: Determine whether the preset route needs to be adjusted based on the overall collision probability.

[0132] If the overall collision probability is high, it means that the preset route is not compatible with the surrounding environment and there is a high risk of collision. By adjusting the preset route, you can avoid areas with high collision probability, thereby reducing the risk of collision between the drone and obstacles. Determine whether the preset route needs to be adjusted based on the overall collision probability.

[0133] Preferably, in an embodiment of the present invention, judging whether the preset route needs to be adjusted according to the overall collision probability includes: if the overall collision probability is greater than a preset collision threshold, judging that the preset route needs to be adjusted.

[0134] It should be noted that, in one embodiment of the present invention, the preset collision threshold is 0.3; in other embodiments of the present invention, the size of the preset collision threshold can be set according to specific circumstances, which is not limited or elaborated here.

[0135] After determining that the preset route needs to be adjusted, the drone's navigation route can be replanned based on the drone system's laser scanning radar and navigation positioning technology to ensure the safety of the route.

[0136] In summary, the present invention obtains the speed fluctuation scale of each point cloud segmented object in each direction by analyzing the point cloud data; and then obtains the threat range of each point cloud segmented object at each moment; for any direction, according to the maximum offset distance of the drone during each deviation and the monitored wind level, obtains the orbital offset scale of the drone; combines the actual wind force and wind field disturbance coefficient at the initial moment to obtain the maximum deviation scale of the drone at each moment; combines the ideal navigation position to obtain the error range of the drone at each moment; obtains the overall collision probability of the drone and the point cloud segmented object; and determines whether the preset route needs to be adjusted. The present invention improves the accuracy of judging the safety of the route through the accurate collision probability of the drone on the preset route.

[0137] It should be noted that the sequence of the above embodiments of the present invention is only for description and does not represent the advantages and disadvantages of the embodiments. The processes depicted in the accompanying drawings do not necessarily require the specific order or continuous order shown to achieve the desired results. In some embodiments, multitasking and parallel processing are also possible or may be advantageous.

[0138] The various embodiments in this specification are described in a progressive manner, and the same or similar parts between the various embodiments can be referenced to each other, and each embodiment focuses on the differences from other embodiments.

Claims

1. A UAV navigation control method based on navigation positioning, characterized in that: The method comprises: Based on the preset route, obtain the point cloud data of the drone's environment, the monitored wind speed at each moment in different preset directions, and the maximum offset distance during each deviation process; A plurality of point cloud segmentation objects corresponding to the point cloud data are obtained, and speed data of each point cloud segmentation object in each direction at each time is determined; a speed fluctuation scale of each point cloud segmentation object in each direction is obtained according to the speed data distribution of each point cloud segmentation object in each direction; a threat range of each point cloud segmentation object at each time is obtained according to the speed fluctuation scale of each point cloud segmentation object in different directions and the speed data at each time; For any direction, the actual wind force of the drone at each moment is obtained according to the monitored wind force and preset flight wind resistance of the drone at each moment, and the wind field disturbance coefficient of the drone is obtained; the orbital deviation scale of the drone is obtained according to the maximum deviation distance and monitored wind force level of the drone during each deviation process; the maximum deviation scale of the drone at each moment is obtained according to the orbital deviation scale of the drone, the actual wind force at the initial moment, and the wind field disturbance coefficient; the error range of the drone at each moment is obtained according to the maximum deviation scale of the drone in different directions at each moment and the ideal navigation position; According to the distribution of the threat range of different point cloud segmentation objects and the error range of the drone at each moment within the preset time range, the overall collision probability between the drone and the point cloud segmentation object is obtained; It is determined whether the preset route needs to be adjusted according to the overall collision probability.

2. The method for controlling the navigation of an unmanned aerial vehicle based on navigation positioning according to claim 1, characterized in that: The method for obtaining the speed fluctuation scale includes: According to the fluctuation degree of all speed data of each point cloud segmentation object in each direction, the speed fluctuation scale of each point cloud segmentation object in each direction is obtained, and the fluctuation degree is positively correlated with the speed fluctuation scale.

3. The method for controlling the navigation of an unmanned aerial vehicle based on navigation positioning according to claim 1, characterized in that: The method for obtaining the threat range includes: For any direction, the speed data at each moment is used as the simulated uniform speed data, and the position of each point cloud segmentation object at each moment in each direction is obtained based on the uniform motion formula to form the spatial position of each point cloud segmentation object at each moment; Calculate the product of the velocity fluctuation scale and the corresponding time in each direction of each point cloud segmented object as the displacement deviation; The spatial position of each point cloud segmentation object at each moment is taken as the center of the fitting ellipse, the displacement deviation in different directions is taken as the radius of the fitting ellipse, and the corresponding fitting ellipse area is taken as the threat range of each point cloud segmentation object at each moment.

4. The method for controlling the navigation of an unmanned aerial vehicle based on navigation positioning according to claim 1, characterized in that: The method for obtaining the actual wind force comprises: The difference between the monitored wind force of the UAV in each direction at each moment and the preset flight wind resistance is calculated as the actual wind force of the UAV in each direction at each moment.

5. The method for controlling the navigation of an unmanned aerial vehicle based on navigation positioning according to claim 1, characterized in that: The method for obtaining the wind field disturbance coefficient includes: For any direction, the wind field disturbance coefficient of the UAV in the corresponding direction is obtained based on the difference in actual wind force between the UAV at each moment and the adjacent previous moment, and the difference is positively correlated with the wind field disturbance coefficient.

6. The method for controlling the navigation of an unmanned aerial vehicle based on navigation positioning according to claim 1, characterized in that: The method for obtaining the track deviation scale includes: For any direction, the average monitored wind level of the UAV during each deviation process is obtained; based on the maximum deviation distance of the UAV during each deviation process and the average monitored wind level, the track deviation distance of the UAV in each direction is obtained. The maximum deviation distance is positively correlated with the track deviation distance, and the average monitored wind level is negatively correlated with the track deviation distance.

7. The method for controlling the navigation of an unmanned aerial vehicle based on navigation positioning according to claim 1, characterized in that: The method for obtaining the maximum deviation scale includes: For any direction, calculate the product between the wind field disturbance coefficient of the UAV and the time, and obtain the wind field disturbance scale of the UAV at the corresponding time in each direction; The sum of the wind field disturbance scale of the UAV at the corresponding moment and the initial actual wind force is obtained as the updated actual wind force; The orbital deviation scale of the UAV is weighted according to the updated actual wind force to obtain the maximum deviation scale of the UAV in each direction at each moment.

8. The method for controlling the navigation of an unmanned aerial vehicle based on navigation positioning according to claim 1, characterized in that: The method for obtaining the error range includes: The ideal navigation position of the UAV at each moment is taken as the center of the fitting ellipse, the maximum deviation scale of the UAV in different directions at each moment is taken as the radius of the fitting ellipse, and the corresponding fitting ellipse area is taken as the error range of the UAV at each moment.

9. The method for controlling the navigation of an unmanned aerial vehicle based on navigation positioning according to claim 1, characterized in that: The method for obtaining the overall collision probability includes: At any moment, if the threat range of the point cloud segmented object and the error range of the drone do not have an intersection, the local collision probability is 0; If there is an intersection between the threat range of the point cloud segmented object and the error range of the drone, obtain a first probability value corresponding to the probability density function of the intersection in the threat range, obtain a second probability value corresponding to the probability density function of the intersection in the error range, and calculate the product of the first probability value and the second probability value as the corresponding local collision probability; If the threat range of the point cloud segmented object intersects with the error range of the drone and is greater than one intersection point, a first ratio of the intersection range to the threat range is obtained, a second ratio of the intersection range to the error range is obtained, and the product of the first ratio and the second ratio is calculated to obtain the corresponding local collision probability; According to the local collision probability between the threat range of different point cloud segmentation objects and the error range of the drone at each moment within the preset time range, the overall collision probability between the drone and the point cloud segmentation object is obtained. The local collision probability is positively correlated with the overall collision probability.

10. The method for controlling the navigation of an unmanned aerial vehicle based on navigation positioning according to claim 1, characterized in that: The determining whether the preset route needs to be adjusted according to the overall collision probability includes: If the overall collision probability is greater than the preset collision threshold, it is determined that the preset route needs to be adjusted.

Images