Method and system for fly-by-wire based on radar laser point cloud ranging

Abstract

The application provides a kind of method and system based on radar laser point cloud ranging for line flight simulation, it is related to unmanned aerial vehicle flight control technical field, the point cloud data of the environment around unmanned aerial vehicle is collected in real time and the point cloud density is analyzed, and low-density areas are removed. The polar coordinates of the remaining point cloud are converted to Cartesian coordinates, and the point cloud data is divided into guide line point cloud and obstacle point cloud using the Euclidean clustering algorithm. The extended trajectory of the guide line point cloud is used as the target path. According to the real-time position of the unmanned aerial vehicle, the directional deviation of the unmanned aerial vehicle from the target path is calculated, and the roll angle, yaw angle and throttle are adjusted. Finally, by calculating the Euclidean distance between the unmanned aerial vehicle and the nearest obstacle and comparing it with the set dangerous distance threshold, the yaw angle and throttle are adjusted in real time to ensure flight safety. This method combines point cloud processing, path planning and dynamic obstacle avoidance technology, providing an effective solution for the safe flight of unmanned aerial vehicles in complex environments.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of unmanned aerial vehicle flight control, in particular to a method and system for line-imitating flight based on radar laser point cloud ranging. BACKGROUND

[0002] At present, unmanned aerial vehicle inspection in the power industry is mostly based on fixed routes to inspect towers or passages between towers, but rarely inspects tower conductors and ground wires. If the conductors and ground wires need to be inspected, there are two ways: manual inspection and automatic inspection. Manual inspection requires higher professional requirements for unmanned aerial vehicle operators, who must pay attention to the safety of unmanned aerial vehicle flight at all times. Automatic inspection is line-imitating flight according to a predetermined route, which is safer, but a route must be planned first to enable automatic inspection. This fixed route inspection method is relatively complex in terms of process, because professional route planning software must be used to plan a route before regular inspection can be carried out, and the inspection effect is not good, and the conductor and ground wire extension trajectories cannot be accurately fitted.

[0003] In the prior art, CN115981366B discloses a method for unmanned aerial vehicle line-imitating flight control based on real-time identification of power line point clouds, which uses three-dimensional laser point cloud data to predict the line-imitating flight path of the unmanned aerial vehicle to obtain a line-imitating inspection self-flight path, realizes autonomous line-imitating flight of the unmanned aerial vehicle, and simultaneously updates the line-imitating inspection self-flight path in real time to reduce the time for determining the path during line-imitating flight of the unmanned aerial vehicle.

[0004] The main problem of the above-mentioned scheme is that it emphasizes real-time updating of the path, but in a complex environment, frequent path adjustment may cause attitude oscillation or trajectory jitter of the unmanned aerial vehicle, reducing flight stability, and the above-mentioned scheme does not take effective measures to avoid this problem; and it relies on continuous high-quality point cloud input, requiring high quality of the point cloud, and path prediction failure may occur.

[0005] The above information disclosed in the background section is only used to enhance the understanding of the background of the present disclosure, and therefore it can include information that does not constitute prior art known to those of ordinary skill in the art. SUMMARY

[0006] The present application aims to provide a method and system for line-imitating flight based on radar laser point cloud ranging to solve the problems raised in the background.

[0007] To achieve the above-mentioned purpose, the present application provides the following technical scheme:

[0008] A method for line-imitating flight based on radar laser point cloud ranging, comprising the following specific steps:

[0009] Step 1: Collect point cloud data of the environment around the drone in real time. The environment around the drone includes wires and obstacles. The point cloud data includes horizontal distance, vertical distance, scanning angle and point cloud density.

[0010] Step 2: Set a point cloud density threshold, remove areas with point cloud density below the threshold, generate polar coordinates of the remaining points in the point cloud based on horizontal distance, vertical distance and scanning angle, and convert the polar coordinates to Cartesian coordinates;

[0011] Step 3: For the point cloud data converted to Cartesian coordinates, it is divided into guide point cloud and obstacle point cloud based on Euclidean clustering. The extended trajectory of the guide point cloud is used as the target path for the UAV to fly along the line. The directional deviation angle of the UAV to the target path is calculated based on the UAV's real-time position. Based on the vertical distance and directional deviation, the roll angle, yaw angle and throttle three attitude data of the UAV are calculated in real time.

[0012] Step 4: Collect the point cloud position of the obstacle closest to the real-time position of the UAV, calculate the Euclidean distance between the two, and set a danger distance threshold. When the Euclidean distance is less than the danger distance threshold, adjust the yaw angle and throttle of the UAV based on the closest obstacle point cloud position. When the Euclidean distance is greater than the danger distance threshold, there is no need to adjust the attitude data of the UAV.

[0013] Furthermore, the polar coordinates of the point are:

[0014] p i ′=[d i (l),d i (h),θ i ]

[0015] Where, p i ' represents the polar coordinates of the i-th point, i represents the index of the remaining point in the point cloud after removing regions with point cloud density below the threshold, i∈[1,n], n represents the number of remaining points in the point cloud after removing regions with point cloud density below the threshold, and d i (l) represents the horizontal distance from the i-th point to the drone, d i (h) represents the vertical distance from the i-th point to the drone, θ i This represents the scanning angle of the i-th point when the drone collects point cloud data;

[0016] The roll axis, pitch axis, and yaw axis of the UAV are used as the horizontal, vertical, and longitudinal axes of the Cartesian coordinate system, respectively.

[0017] The formula for converting the polar coordinates of a point cloud to Cartesian coordinates is as follows:

[0018] x i =d i (l)×cosθ i

[0019] y i =d i (h)×sinθ i

[0020] z i =d i (h)

[0021] p i =(x i ,y i ,z i )

[0022] Where, x i y i z i Let p represent the x, y, and y coordinates of the i-th point in the Cartesian coordinate system, respectively. i Let represent the Cartesian coordinates of the i-th point.

[0023] Furthermore,

[0024] The principle underlying point clouds that distinguish between wires and obstacles based on Euclidean clustering is:

[0025] Construct a 3D point set of all point clouds in Cartesian coordinates, P = {p1, p2, ..., p...} n}, where P represents the set of coordinates of all points in the point cloud in the Cartesian coordinate system;

[0026] Set the search radius of Euclidean clustering to twice the diameter of the wire, for each point p i Find all points in P whose distance is within the search radius:

[0027] N ∈ (p i )={q i ∈P,i∈[1,n]|||p i -q i ||≤∈}

[0028]

[0029] Where, N ∈ (p i ) represents all elements in P that are related to p. i The set of points whose distance is within the search radius, q i This indicates that within set P, it is different from set p. i For any point, ∈ represents the search radius, ||p i -q|| indicates p i and q i The Euclidean distance between them They represent points q respectively.i horizontal, vertical, and longitudinal coordinates of the point p

[0030] statistical point set N ∈ (p i ) contains the number of points N(p i ), when n(p i ) ≥ N0, then the point p i is a core point; N0 represents the minimum neighborhood point threshold;

[0031] For each core point, combine all points within its search radius to generate a cluster;

[0032] Perform PCA principal component analysis on each cluster to obtain the distribution variance of the cluster in the primary direction and the secondary direction, the primary direction representing the direction of the most concentrated distribution of the cluster point cloud, and the secondary direction representing the direction of the second most concentrated distribution in the plane orthogonal to the primary direction;

[0033] When , the cluster point cloud is determined to be a guide line, otherwise it is an obstacle, where λ1 represents the distribution variance of the primary direction, and λ2 represents the distribution variance of the secondary direction.

[0034] Further, the principle on which the direction deviation angle of the unmanned aerial vehicle to the target path is calculated is:

[0035] The position of the unmanned aerial vehicle at time t is u(t) = [x u (t), y u (t), z u (t)], and the position of the unmanned aerial vehicle at time t-Δt is u(t-Δt) = [x u (t-Δt), y u (t-Δt), z u (t-Δt)], where Δt represents the time interval between time t and time t-Δt.

[0036] where t represents the current time, u(t) represents the real-time coordinates of the unmanned aerial vehicle at time t, x u (t), y u (t), and z u (t) represent the real-time horizontal, vertical, and longitudinal coordinates of the unmanned aerial vehicle at time t, respectively, u(t-Δt) represents the position of the unmanned aerial vehicle at time t-Δt, x u (t-Δt), y u (t-Δt), and z u (t-Δt) represent the real-time horizontal, vertical, and longitudinal coordinates of the unmanned aerial vehicle at time t-Δt, respectively;

[0037] At time t, select the point closest to u(t) on the guide line point cloud extension trajectory as the reference point at time t, and the coordinates of the reference point are:

[0038] u′(t)=[x′ u (t),y u ′(t),z′ u (t)]

[0039] Where u′(t) represents the coordinates of the reference point, x′ u (t), y u ′(t), z′ u (t) represent the x, y, and y coordinates of the reference point, respectively;

[0040] Obtain the coordinates of the reference point closest to u(t-Δt) on the extended trajectory of the traverse point cloud, and calculate the tangent vector between the reference points using the following formula:

[0041] u′(t-Δt)=[x′ u (t-Δt),y u ′(t-Δt),z′ u (t-Δt)]

[0042] T(t)=u′(t)-u′(t-Δt)

[0043] Where u′(t-Δt) represents the coordinates of the reference point closest to u(t-Δt), and x′ u (t-Δt), y′ u (t-Δt), z′ u (t-Δt) represent the horizontal, vertical, and triangular coordinates of u′(t-Δt), respectively, and T(t) represents the tangent vector;

[0044] The velocity vector of the drone is:

[0045]

[0046] Among them, v u (t) represents the velocity vector of the UAV at time t;

[0047] The directional deviation angle is:

[0048]

[0049] Where α(t) represents the directional deviation angle of the UAV at time t, ||v u (t)|| and ||T(t)|| represent the magnitudes of the velocity vector and the tangent vector, respectively.

[0050] Furthermore, the principle underlying the real-time calculation of the three attitude data of the UAV—roll angle, yaw angle, and throttle—is as follows:

[0051] The formula used to calculate the roll angle of a drone is:

[0052]

[0053] wherein φ(t) represents the roll angle of the UAV at time t, K p,1 , k d,1 represent the proportional gain and the differential gain of the roll angle, respectively;

[0054] The formula for calculating the yaw angle of the UAV is:

[0055]

[0056] wherein γ(t) represents the yaw angle of the UAV at time t, K p,2 , K d,2 represent the proportional gain and the differential gain of the yaw angle, respectively;

[0057] The formula for calculating the throttle of the UAV is:

[0058]

[0059] wherein Y(t) represents the throttle of the UAV at time t, K p,3 , K d,3 represent the proportional gain and the differential gain of the throttle, respectively.

[0060] Further, the principle for adjusting the yaw angle and the throttle of the UAV based on the closest obstacle position is:

[0061] The formula for calculating the Euclidean distance between the coordinates of the closest obstacle and the real-time position of the UAV is:

[0062]

[0063] wherein D(t) represents the Euclidean distance between the real-time position of the UAV at time t and the coordinates of the closest obstacle, x o (t), y o (t), z o (t) represent the horizontal, vertical and vertical coordinates of the closest obstacle, respectively;

[0064] The azimuth angle of the closest obstacle relative to the UAV is calculated as:

[0065] α0(t) = arctan2{[x u (t) - x o (t)] 2 , [y u (t) - y o (t)] 2}

[0066] Wherein, a0(t) represents the azimuth angle of the obstacle relative to the unmanned aerial vehicle at t time;

[0067] The adjusted yaw angle is:

[0068] γ new (t) = γ(t) + M avoid × [a0(t) - γ(t)]

[0069] Wherein, γ new (t) represents the adjusted yaw angle at t time, M avoid represents the obstacle avoidance gain coefficient;

[0070] The adjusted throttle is:

[0071]

[0072] Wherein, Y new (t) represents the adjusted throttle at t time, M H represents the height gain coefficient, and D0 represents the dangerous distance threshold.

[0073] The application also provides a system for line-imitating flight based on radar laser point cloud ranging, which is used to realize the method for line-imitating flight based on radar laser point cloud ranging, and specifically comprises:

[0074] A point cloud acquisition module is configured to acquire point cloud data of the environment around the unmanned aerial vehicle in real time, wherein the environment around the unmanned aerial vehicle comprises a conductor and an obstacle, and the point cloud data comprises a horizontal distance, a vertical distance, a scanning angle and a point cloud density;

[0075] A coordinate calculation module is configured to set a point cloud density threshold, eliminate areas with a point cloud density lower than the threshold, generate polar coordinates of the remaining points in the point cloud based on the horizontal distance, the vertical distance and the scanning angle, and convert the polar coordinates into Cartesian coordinates;

[0076] An attitude calculation module is configured to divide the point cloud data converted into Cartesian coordinates into conductor point cloud and obstacle point cloud based on Euclidean clustering, take the extended trajectory of the conductor point cloud as a target path for line-imitating flight of the unmanned aerial vehicle, calculate a directional deviation angle of the unmanned aerial vehicle to the target path based on the real-time position of the unmanned aerial vehicle, and calculate three attitude data of the unmanned aerial vehicle, i.e., a roll angle, a yaw angle and a throttle, in real time based on the vertical distance and the directional deviation;

[0077] An attitude adjustment module is configured to acquire the position of the closest obstacle point cloud to the real-time position of the unmanned aerial vehicle, calculate the Euclidean distance between the two, set a dangerous distance threshold, adjust the yaw angle and the throttle of the unmanned aerial vehicle based on the position of the closest obstacle point cloud when the Euclidean distance is less than the dangerous distance threshold, and not adjust the attitude data of the unmanned aerial vehicle when the Euclidean distance is greater than the dangerous distance threshold.

[0078] Compared with the prior art, the present application has the beneficial effects that:

[0079] The present application scans the point cloud data in real time during the flight process of the unmanned aerial vehicle, and distinguishes the conductors and obstacles, identifies the obstacles to avoid collision while limiting the flight path of the unmanned aerial vehicle, reduces the flight risk, and improves the flexible response capability of the unmanned aerial vehicle in complex environment.

[0080] The present application also determines the real-time position of the unmanned aerial vehicle, finds the reference point on the line extension path, adjusts the roll angle, yaw angle and throttle of the unmanned aerial vehicle in real time, enhances the stability of the flight, and ensures that the unmanned aerial vehicle can correct the heading in time; by adjusting the flight strategy in real time, the need for human intervention in the flight process is reduced, the operation of the unmanned aerial vehicle is more convenient, and the application range of the unmanned aerial vehicle is improved. By real-time acquisition of the position of the obstacle closest to the position of the unmanned aerial vehicle, and calculation of the Euclidean distance between the two, the potential collision risk is evaluated, the safety of the line flight is improved, and the yaw angle and throttle of the unmanned aerial vehicle are dynamically adjusted according to the distance between the unmanned aerial vehicle and the obstacle to avoid collision, enhance the flexibility and adaptability of the unmanned aerial vehicle, and enable the unmanned aerial vehicle to still perform safe and reliable line flight in complex environment. BRIEF DESCRIPTION OF DRAWINGS

[0081] Figure 1 The present application is an embodiment method flowchart;

[0082] Figure 2 The present application is an embodiment yaw angle change point line graph;

[0083] Figure 3 The present application is an embodiment azimuth angle adjustment diagram of the yaw angle;

[0084] Figure 4 The present application is an embodiment throttle change point line graph;

[0085] Figure 5 The present application is an embodiment throttle adjustment diagram;

[0086] Figure 6 The present application is an embodiment system module diagram. DETAILED DESCRIPTION

[0087] In order to make the purpose, technical scheme and advantages of the present application clearer and more apparent, the present application will be further described in detail below in combination with specific embodiments.

[0088] It should be noted that, unless otherwise defined, technical terms or scientific terms used in the present application shall have the common meaning understood by one of ordinary skill in the art to which the present application pertains. The terms "first", "second", and similar terms used in the present application do not denote any order, number, or importance, but are only used to distinguish different components. The terms "include", "contain", and similar terms mean that the elements or objects before the terms encompass the elements or objects listed after the terms and their equivalents, and do not exclude other elements or objects. The terms "connected" or "connected" and similar terms are not limited to physical or mechanical connections, but can include electrical connections, whether direct or indirect. The terms "up", "down", "left", "right", and the like are only used to represent relative positional relationships, which can change accordingly when the absolute position of the described object changes.

[0089] Embodiment:

[0090] Please refer to Figures 1 to 5 The present application provides a technical solution:

[0091] A method for simulating line flight based on radar laser point cloud ranging, the specific steps comprising:

[0092] Step 1: Real-time acquisition of point cloud data of the environment around the unmanned aerial vehicle, the environment around the unmanned aerial vehicle including the conductor and the obstacle, the point cloud data including the horizontal distance, the vertical distance, the scanning angle and the point cloud density;

[0093] In this embodiment, a radar scanner is carried on the unmanned aerial vehicle, and the point cloud data of the surrounding environment is collected in real time by the radar scanner. The model of the radar scanner is RIEGL three-dimensional laser scanner. The radar scanner carried by the unmanned aerial vehicle is taken as the coordinate origin, and the horizontal distance, the vertical distance and the scanning angle of the point cloud relative to the coordinate origin of the conductor and the obstacle around the unmanned aerial vehicle are scanned respectively, and the polar coordinates of each point in the point cloud are generated.

[0094] Step 2: Set the point cloud density threshold, after eliminating the area with point cloud density lower than the threshold, generate the polar coordinates of the remaining points in the point cloud based on the horizontal distance, the vertical distance and the scanning angle, and convert the polar coordinates into Cartesian coordinates;

[0095] In this embodiment, the setting of the point cloud density threshold directly affects the accuracy of noise filtering. The point cloud density represents the number of effective points in a unit volume. For each point, the number of points N in the neighborhood with a search radius ∈ is counted. i The point cloud density is: For the entire scanning area, the average point cloud density is calculated Where V represents the volume of the entire scanning area, and the point cloud density threshold is determined based on the average point cloud density, Wherein, b represents the point cloud density adjustment coefficient, and b [0, 1], the value of b tends to 1, the filtering of the point cloud is more strict, and the sparse noise including flying birds, dust and the like can be removed more effectively, but the point cloud density of the lead wire is low at a long distance, which can cause the real track to be mistakenly deleted, and the point cloud of the thin lead wire is completely filtered, the value of b tends to 0, the filtering of the point cloud is more relaxed, and the point cloud of the lead wire at a long distance and the thin lead wire can be ensured not to be missed, but the noise irrelevant point cloud can be retained, increasing the risk of false detection, based on the actual detection demand, the specific value of b is determined by expert scoring.

[0096] The polar coordinates of the point are:

[0097] p i ′=[d i (l),d i (h),θ i ]

[0098] Wherein, p i ′ represents the polar coordinates of the i-th point, i represents the index of the remaining points in the point cloud after removing the region with point cloud density lower than the threshold, i [1, n], n represents the number of remaining points in the point cloud after removing the region with point cloud density lower than the threshold, d i (l) represents the horizontal distance of the i-th point from the unmanned aerial vehicle, d i (h) represents the vertical distance of the i-th point from the unmanned aerial vehicle, and θ i represents the scanning angle of the i-th point when the unmanned aerial vehicle collects point cloud data.

[0099] The roll axis, pitch axis and yaw axis of the unmanned aerial vehicle are respectively taken as the horizontal, vertical and vertical axes of the Cartesian coordinate system.

[0100] The polar coordinates of the point cloud are converted into Cartesian coordinates, and the formula is:

[0101] x i =d i (l)×cosθ i

[0102] y i =d i (h)×sinθ i

[0103] z i =d i (h)

[0104] p i =(x i ,y i ,z i )

[0105] Wherein, x i , yi , z i , z i , z

[0106] Step 3: For the point cloud data converted into the Cartesian coordinate system, it is divided into the guide line point cloud and the obstacle point cloud based on the Euclidean clustering, the trajectory extended from the guide line point cloud is taken as the target path for the simulation flight of the unmanned aerial vehicle, the vertical distance and the direction deviation angle of the unmanned aerial vehicle to the target path are calculated based on the real-time position of the unmanned aerial vehicle, and the roll angle, the yaw angle and the throttle of the unmanned aerial vehicle are calculated in real time based on the vertical distance and the direction deviation;

[0107] In the embodiment, the principle for distinguishing the point cloud of the guide line and the obstacle based on the Euclidean clustering is as follows:

[0108] A three-dimensional point set of all point clouds in the Cartesian coordinate system is constructed, P = {p1, p2, …, pn}, where P represents the coordinate set of all points in the point cloud in the Cartesian coordinate system; n

[0109] The search radius of the Euclidean clustering is set to be 2 times the diameter of the guide line, and for each point p i , all points in P within the search radius are searched:

[0110] N ∈ (p i ) = {q i ∈ P, i ∈ [1, n] || |p i -q i || ≤ ∈}

[0111]

[0112] where N ∈ (p i ) represents the point set in P within the search radius from p i , q i represents any point in P different from p i , ∈ represents the search radius, and ||p i -q|| represents the Euclidean distance between p i and q i , , z i , z

[0113] The number of points N(p ∈ ) contained in the point set N i (p i ) is counted, and when N(p i ) ≥ N0, the point p​i is a core point; N0 represents a minimum neighborhood point number threshold value;

[0114] For each core point, all points within its search radius are combined to generate a cluster;

[0115] The cluster of the core point is the combination of all points within the search radius, and the search radius is ε, which is called an ε-neighborhood, which refers to a spherical space region with p i as the center and a radius of ε, and all points in the region are regarded as the neighbors of p i i When the number of points in the neighborhood exceeds the set minimum neighborhood point number threshold value, p i is regarded as a core point, and p i is combined with all points in its neighborhood to generate a cluster with p i as the core point. When the minimum neighborhood point number threshold value N0 is set, if N0 is too small, extremely sparse clusters can be detected, but isolated noise points may be mistaken for core points to generate false clusters, and if N0 is too large, only high-density regions can be identified, and part of the sparse wire point cloud may not form a cluster. Based on the actual detection requirements, the specific N0 value is determined through expert scoring;

[0116] PCA principal component analysis is performed on each cluster to obtain the distribution variances of the cluster in the main direction and the secondary direction, the main direction represents the direction in which the cluster point cloud is most concentrated, and the secondary direction represents the direction in which the cluster point cloud is less concentrated in the plane orthogonal to the main direction;

[0117] When the cluster point cloud is determined to be a wire, otherwise it is an obstacle, wherein λ1 represents the distribution variance of the main direction, λ2 represents the distribution variance of the secondary direction, and λ3 represents the distribution variance of the least important direction.

[0118] The feature of the wire point cloud is the main direction, that is, the wire extension direction has the maximum extension, and the distribution variance is much larger than that in other directions. The secondary direction represents the width direction of the wire cross section, and when , it means that λ1 >> λ2, which meets the characteristics of the wire, and the shape of the point cloud appears as an elongated linear structure.

[0119] The principle for calculating the directional deviation angle of the unmanned aerial vehicle to the target path is as follows:

[0120] The position of the unmanned aerial vehicle at time t is u(t) = [x u (t), y u (t), z u (t)], and the position of the unmanned aerial vehicle at time t-Δt is u(t-Δt) = [x u (t-Δt), y u ​(t-Δt),z u (t-Δt),z

[0121] wherein t represents current time, u(t) represents real-time coordinates of the UAV at time t, x u (t),y u (t),z u (t) respectively represent real-time horizontal, vertical and vertical coordinates of the UAV at time t, u(t-Δt) represents coordinates of the UAV at time t-Δt, x u (t-Δt),y u (t-Δt),z u (t-Δt) respectively represent real-time horizontal, vertical and vertical coordinates of the UAV at time t-Δt;

[0122] Δt affects the control performance and path tracking accuracy of the UAV, in the process of flight simulation, the UAV needs to quickly respond to environmental changes, so as to avoid obstacles, change direction and other operations, Δt should be small, and Δt=0.1s is taken to improve the response speed.

[0123] At time t, the point closest to u(t) on the extended trajectory of the guide point cloud is selected as the reference point at time t, and the coordinates of the reference point are:

[0124] u'(t)=[x' u (t),y u '(t),z' u (t)]

[0125] wherein u'(t) represents coordinates of the reference point, x' u (t),y u '(t),z' u (t) respectively represent horizontal, vertical and vertical coordinates of the reference point;

[0126] The coordinates of the reference point closest to u(t-Δt) on the extended trajectory of the guide point cloud are obtained, and the tangent vector between the reference points is calculated, and the formula is:

[0127] u'(t-Δt)=[x' u (t-Δt),y u '(t-Δt),z' u (t-Δt)]

[0128] T(t)=u'(t)-u'(t-Δt)

[0129] wherein u'(t-Δt) represents coordinates of the reference point closest to u(t-Δt), x' u (t-Δt),y' u(t-Δt), z' u (t-Δt) respectively represent the horizontal, vertical and vertical coordinates of u'(t-Δt), and T(t) represents a tangent vector;

[0130] The velocity vector of the UAV is:

[0131]

[0132] wherein v u (t) represents the velocity vector of the UAV at time t;

[0133] The directional deviation angle is:

[0134]

[0135] wherein α(t) represents the directional deviation angle of the UAV at time t, and ||v u (t)|| and ||T(t)|| respectively represent the lengths of the velocity vector and the tangent vector.

[0136] The directional deviation angle represents the included angle between the velocity direction of the UAV and the extension direction of the target path, and reflects the deviation degree of the flight direction of the UAV from the wire trajectory. The directional deviation angle directly measures whether the UAV moves along the wire extension trajectory, and avoids the cumulative error caused by the directional deviation. When α(t) = 0°, it indicates that the heading direction of the UAV is completely consistent with the wire direction, and no heading adjustment is needed. When α(t) ≠ 0°, the yaw angle control is needed. The value of the directional deviation angle is determined by the current velocity vector direction of the UAV and the tangent vector of the reference point. The velocity vector direction reflects the target direction of the UAV being flown, and the tangent vector direction of the reference point reflects the extension direction of the path. The directional deviation angle is calculated based on the velocity vector and the tangent vector, and reflects the deviation degree of the UAV from the path. When the two vectors are completely in the same direction, the deviation angle is 0°. When the two vectors are orthogonal, the deviation angle is 90°, and at this time, the maximum deviation is reached. When the two vectors are opposite, the deviation angle is 180°, and the complete deviation is reached;

[0137] The principle for real-time calculation of the three attitude data of the roll angle, the yaw angle and the throttle of the UAV is:

[0138] The formula for calculating the roll angle of the UAV is:

[0139]

[0140] wherein φ(t) represents the roll angle of the UAV at time t, K p,1 , K d,1 respectively represent the proportional gain and the differential gain of the roll angle;

[0141] The roll angle represents the angle of rotation of the UAV around the x-axis, i.e., the roll axis, for controlling the left-right tilt of the UAV. The roll angle will cause the UAV to generate a lateral acceleration, thereby changing its position in the y-axis direction. p,1 ×[y u (t)-y u ′(t)] according to the current horizontal deviation. The greater the deviation, the greater the adjustment range, and y u (t)-y u ′(t) > 0 indicates that the UAV is on the right side of the guide line and needs to tilt left, and when y u (t)-y u ′(t) < 0 indicates that the UAV is on the left side of the guide line and needs to tilt right. p,1 The greater K p,1 , the faster the response speed, but it may overshoot or oscillate. p,1 Too small, it may appear slow response, tracking lag phenomenon; It represents providing damping according to the rate of change of the deviation, suppressing oscillation, and avoiding overshoot of the roll angle adjustment range. If K d,1 is too large, the adjustment response of the roll angle will be slow, and if K d,1 is too small, it cannot effectively suppress oscillation; based on experimental parameter adjustment to determine K p,1 and K d,1 , start from a small value, K p,1 = 0.1, K d,1 = 0, ensure that the UAV starts in a stable state, gradually increase K p,1 , until the UAV starts to oscillate, which indicates that the UAV is shaking back and forth on the path of the guide line extension track, record K p,1 at this time, start from K d,1 = 0.1K p,1 , gradually increase K d,1 , until the UAV is stable and adheres to the guide line, and the oscillation disappears, record K d,1 .

[0142] The formula for calculating the yaw angle of the UAV is:

[0143]

[0144] where γ(t) represents the yaw angle of the UAV at time t, K p,2 and K d,2 represent the proportional gain and differential gain for adjusting the yaw angle, respectively.

[0145] The yaw angle represents the angle of the UAV rotating around the vertical axis, and its function is to keep the flight direction of the UAV consistent with the direction of the path of the guide line, which is manifested as controlling the UAV to turn the head left or right. When the heading of the UAV deviates from the direction of the guide line, i.e., a(t)≠0°, the yaw control makes the UAV turn to the direction of the path of the guide line, and the greater a(t) is, the stronger the correction is. For suppressing oscillation caused by inertia or external disturbance, for limiting the yaw angle change rate, and for avoiding excessive oscillation amplitude, when it indicates that the UAV is deviating from the guide line, and at this time, a reverse torque is generated to slow down the deviation speed. If K p,2 is too large, the UAV turns sharply, and overshoot or oscillation may occur. If K p,2 is too small, the UAV responds slowly and is difficult to quickly align with the path. If K d,2 is too large, the system responds slowly, and the UAV turns slowly and cannot correct the deviation in time. If K d,2 is too small, the oscillation cannot be effectively suppressed, and the heading of the UAV repeatedly swings. Based on experimental parameter tuning, K p,2 and K d,2 , the change of the yaw angle of the UAV with the change of the direction deviation angle is shown in Table 1, K p,2 =0.5, and K d,2 =0.2.

[0146] Table 1. Yaw angle change table

[0147]

[0148] The formula for calculating the throttle of the UAV is:

[0149]

[0150] where Y(t) represents the throttle of the UAV at time t, K p,3 and K d,3 represent the proportional gain and the differential gain for adjusting the throttle, respectively.

[0151] The throttle is used to control the vertical lift of the UAV and directly affects the height of the UAV, i.e., the position on the z-axis. In the loxodrome flight, without considering obstacles, the target of the throttle is to keep the vertical distance between the UAV and the guide line during the flight. The throttle output is calculated based on the deviation of the height of the current position of the UAV from the height of the reference point, and K p,3 ×[z u (t)-z′ u (t)] represents the adjustment amplitude of the throttle based on the height deviation. If the UAV is lower than the reference point height, the throttle is increased for climbing. If the UAV is higher than the reference point height, the throttle is decreased for descending. However, only adjusting by the proportional term will cause the UAV to repeatedly oscillate near the target height. Oscillation is inhibited by the change rate of height deviation, if the UAV is close to the target height, the throttle is reduced to avoid overshoot, if the UAV is away from the target, the throttle is increased; K p,3 and K d,3 The throttle change of the UAV is shown in Table 2, taking K p,3 = 0.5, K d,3 = 0.1.

[0152] Table 2. Throttle change table

[0153]

[0154]

[0155] Step 4: Collect the position of the closest obstacle point cloud to the real-time position of the UAV, calculate the Euclidean distance between the two, and set a dangerous distance threshold. When the Euclidean distance is less than the dangerous distance threshold, adjust the yaw angle and throttle of the UAV based on the closest obstacle point cloud position, when the Euclidean distance is greater than the dangerous distance threshold, the attitude data of the UAV does not need to be adjusted.

[0156] In this embodiment, the principle of adjusting the yaw angle and throttle of the UAV based on the closest obstacle position is:

[0157] The formula for calculating the Euclidean distance between the closest obstacle coordinates and the real-time position of the UAV is:

[0158]

[0159] Where D(t) represents the Euclidean distance between the real-time position of the UAV at time t and the coordinates of the closest obstacle, x o (t), y o (t), z o (t) represent the horizontal, vertical and vertical coordinates of the closest obstacle respectively;

[0160] Calculate the azimuth angle of the closest obstacle relative to the UAV:

[0161] N0(t) = arctan2{[x u (t)-x o (t)] 2 ,[y u (t)-y o (t)] 2}

[0162] Where α0(t) represents the azimuth angle of the obstacle relative to the UAV at time t;

[0163] The adjusted yaw angle is:

[0164] γ new (t) = γ(t) + M avoid × [α0(t) - γ(t)]

[0165] where γ new (t) represents the yaw angle adjusted at time t, M avoid represents the obstacle avoidance gain coefficient;

[0166] The adjusted throttle is:

[0167]

[0168] where Y new (t) represents the throttle adjusted at time t, M H represents the height gain coefficient, and D0 represents the dangerous distance threshold.

[0169] The yaw angle and the throttle affect the horizontal direction and the vertical direction of the UAV, respectively. Therefore, the two attitude data are mainly adjusted. The azimuth angle of the obstacle is calculated based on the relative position of the UAV and the nearest obstacle. The yaw angle is adjusted according to the position of the obstacle relative to the UAV. If the obstacle is located on the right side of the UAV, the yaw angle is increased, and vice versa. The obstacle avoidance gain coefficient M avoid determines the sensitivity of the UAV to the azimuth angle of the obstacle. The larger M avoid is, the more sensitive the UAV is to the adjustment of the azimuth angle, and the more obvious the change in the yaw angle. M avoid = 0.3 is taken to balance the stability and flexibility of the UAV. The throttle controls the lift of the UAV, directly adjusts the height, and thus avoids the obstacle. The height is adjusted based on the relative position of the obstacle and the UAV. If the obstacle is above, the throttle is reduced to reduce the height. If the obstacle is below, the throttle is increased to lift the height. The adjustment amplitude is smoothly attenuated as the distance decreases to avoid over-adjustment. The height gain coefficient M H reflects the sensitivity of the UAV to the vertical direction during obstacle avoidance. The larger M H is, the faster the response speed of the UAV is during obstacle avoidance, but the adjustment process may be unstable. The smaller M H is, it may lead to slow response. M H ∈ [0.2, 0.5] is taken. The adjustment of the roll angle will make the UAV tilt and produce lateral displacement. However, the power line conductor environment is usually narrow, and lateral displacement is easy to cause side collision. Therefore, no adjustment is made. The adjusted yaw angle is shown in Table 3, and the adjusted throttle is shown in Table 4.

[0170] Table 3. Yaw angle adjustment table

[0171]

[0172]

[0173] Table 4. Throttle adjustment table.

[0174]

[0175]

[0176] Referring to Figure 6 The application also provides a system for line-imitating flight based on radar point cloud ranging, which is used to implement the method for line-imitating flight based on radar point cloud ranging, and specifically comprises:

[0177] a point cloud collecting module, which is used to collect point cloud data of the surrounding environment of the unmanned aerial vehicle in real time, the surrounding environment of the unmanned aerial vehicle comprising a guide wire and an obstacle, and the point cloud data comprising a horizontal distance, a vertical distance, a scanning angle and a point cloud density;

[0178] a coordinate calculating module, which is used to set a point cloud density threshold, eliminate an area with a point cloud density lower than the threshold, generate polar coordinates of remaining points in the point cloud based on the horizontal distance, the vertical distance and the scanning angle, and convert the polar coordinates into Cartesian coordinates;

[0179] an attitude calculating module, which is used to distinguish the point cloud data converted into Cartesian coordinates into guide wire point cloud and obstacle point cloud based on Euclidean clustering, take an extended trajectory of the guide wire point cloud as a target path for line-imitating flight of the unmanned aerial vehicle, calculate a directional deviation angle of the unmanned aerial vehicle to the target path based on a real-time position of the unmanned aerial vehicle, and calculate three attitude data of the unmanned aerial vehicle, i.e., a roll angle, a yaw angle and a throttle, based on the vertical distance and the directional deviation in real time;

[0180] an attitude adjusting module, which is used to collect a position of the closest obstacle point cloud to the real-time position of the unmanned aerial vehicle, calculate Euclidean distance between the two, set a dangerous distance threshold, adjust the yaw angle and the throttle of the unmanned aerial vehicle based on the position of the closest obstacle point cloud when the Euclidean distance is smaller than the dangerous distance threshold, and not need to adjust the attitude data of the unmanned aerial vehicle when the Euclidean distance is greater than the dangerous distance threshold.

[0181] The above formulas are all dimensionless values, and the formulas are obtained by software simulation of a large amount of data to obtain a formula closest to the real situation, and the preset parameters in the formula are set by a person skilled in the art according to the actual situation.

[0182] The above-described embodiments can be implemented in whole or in part by software, hardware, firmware, or any combination thereof. When implemented by software, the above-described embodiments can be implemented in whole or in part in the form of a computer program product. A person of ordinary skill in the art can be aware that units and algorithm steps of the examples described in connection with the embodiments disclosed herein can be implemented by electronic hardware, or a combination of computer software and electronic hardware. Whether the functions are performed by hardware or software depends on the specific application and design constraints of the technical solutions.

[0183] The units described as separate components can or can not be physically separate, and the components shown as units can or can not be physical units, and can be located in one place or distributed on multiple network units. Part or all of the units can be selected to achieve the purpose of the embodiments according to actual needs.

[0184] The above is only a specific implementation of the present application, but the protection scope of the present application is not limited thereto, and any person skilled in the art can easily think of changes or replacements within the technical scope disclosed in the present application, which should be covered within the protection scope of the present application.

Claims

1. A method for line-following flight based on radar laser point cloud ranging, characterized in that, The specific steps include: Step 1: Collect point cloud data of the environment around the drone in real time. The environment around the drone includes wires and obstacles. The point cloud data includes horizontal distance, vertical distance, scanning angle and point cloud density. Step 2: Set a point cloud density threshold, remove areas with point cloud density below the threshold, generate polar coordinates of the remaining points in the point cloud based on horizontal distance, vertical distance and scanning angle, and convert the polar coordinates to Cartesian coordinates; Step 3: For the point cloud data converted to Cartesian coordinates, it is divided into guide point cloud and obstacle point cloud based on Euclidean clustering. The extended trajectory of the guide point cloud is used as the target path for the UAV to fly along the line. The directional deviation angle of the UAV to the target path is calculated based on the UAV's real-time position. Based on the vertical distance and directional deviation, the roll angle, yaw angle and throttle three attitude data of the UAV are calculated in real time. Step 4: Collect the point cloud position of the obstacle closest to the real-time position of the drone, calculate the Euclidean distance between the two, and set a danger distance threshold. When the Euclidean distance is less than the danger distance threshold, adjust the drone's yaw angle and throttle based on the closest obstacle point cloud position. When the Euclidean distance is greater than the danger distance threshold, there is no need to adjust the drone's attitude data. The principle underlying the calculation of the directional deviation angle of the UAV's path to the target is as follows: The position of the drone at time t is u(t) = [x u (t),y u (t),z u (t)], the position of the drone at time t-Δt is u(t-Δt=[x u (t-Δt),y u (t-Δt),z u [(t-Δt)], where Δt represents the time interval between time t and time t-Δt; Where t represents the current time, u(t) represents the real-time coordinates of the UAV at time t, and x u (t), y u (t), z u (t) represents the real-time horizontal, vertical, and triangular coordinates of the UAV at time t, respectively, and u(t-Δt) represents the position of the UAV at time t-Δt. u (t-Δt), y u (t-Δt), z u (t-Δt) represent the real-time horizontal, vertical, and triangular coordinates of the UAV at time t-Δt, respectively; At time t, the point on the extended trajectory of the traverse point cloud that is closest to u(t) is selected as the reference point at time t. The coordinates of the reference point are: u′(t)=[x′ u (t),y′ u (t),z′ u (t)] Where u′(t) represents the coordinates of the reference point, x′ u (t), y′ u (t), z′ u (t) represent the x, y, and y coordinates of the reference point, respectively; Obtain the coordinates of the reference point closest to u(t-Δt) on the extended trajectory of the traverse point cloud, and calculate the tangent vector between the reference points using the following formula: u′(t-Δt)=[x′ u (t-Δt),y′ u (t-Δt),z′ u (t-Δt)] T(t)=u′(t)-u′(t-Δt) Where u′(t-Δt) represents the coordinates of the reference point closest to u(t-Δt), and x′ u (t-Δt), y′ u (t-Δt), z′ u (t-Δt) represent the horizontal, vertical, and triangular coordinates of u′(t-Δt), respectively, and T(t) represents the tangent vector; The velocity vector of the drone is: Among them, v u (t) represents the velocity vector of the UAV at time t; The directional deviation angle is: Where α(t) represents the directional deviation angle of the UAV at time t, ||v u (t)|| and ||T(t)|| represent the magnitudes of the velocity vector and the tangent vector, respectively.

2. The method for line-following flight based on radar laser point cloud ranging according to claim 1, characterized in that: The polar coordinates of the point in step 2 are: p′ i =[d i (l),d i (h),θ i ] Where, p′ i Let d represent the polar coordinates of the i-th point, where i represents the index of the remaining point in the point cloud after removing regions with point cloud density below the threshold, i∈[1,n], and n represents the number of remaining points in the point cloud after removing regions with point cloud density below the threshold. i (l) represents the horizontal distance from the i-th point to the drone, d i (h) represents the vertical distance from the i-th point to the drone, θ i This represents the scanning angle of the i-th point when the drone collects point cloud data; The roll axis, pitch axis, and yaw axis of the UAV are used as the horizontal, vertical, and longitudinal axes of the Cartesian coordinate system, respectively. The formula for converting the polar coordinates of a point cloud to Cartesian coordinates is as follows: x i =d i (l)×cosθ i y i =d i (h)×sinθ i z i =d i (h) p i =(x i ,y i ,z i ) Where, x i y i z i Let p represent the x, y, and y coordinates of the i-th point in the Cartesian coordinate system, respectively. i Let represent the Cartesian coordinates of the i-th point.

3. The method for linear flight based on radar laser point cloud ranging according to claim 2, characterized in that: The principle underlying the point cloud used in step 3 to distinguish between wires and obstacles based on Euclidean clustering is as follows: Construct a 3D point set of all point clouds in Cartesian coordinates, P = {p1, p2, ..., p...} n }, where P represents the set of coordinates of all points in the point cloud in the Cartesian coordinate system; Set the search radius of Euclidean clustering to twice the diameter of the wire, for each point p i Find all points in P whose distance is within the search radius: N ∈ (p i )={q i ∈P,i∈[1,n]|||p i -q i ||≤∈} Where, N ∈ (p i ) represents all elements in P that are related to p. i The set of points whose distance is within the search radius, q i This indicates that within set P, it is different from set p. i For any point, ∈ represents the search radius, ||p i -q|| indicates p i and q i The Euclidean distance between them They represent points q respectively. i The horizontal, vertical, and axial coordinates; Statistical point set N ∈ (p i The number of points N(p) contained in ) i ), when n(p i When )≥N0, then point p i The core point is N0, which represents the minimum threshold number of neighboring points. For each core point, all points within its search radius are combined to generate a cluster; For each cluster, perform principal component analysis (PCA) to obtain the variance of the cluster distribution in the primary and secondary directions. The primary direction represents the direction in which the cluster point cloud is most concentrated, and the secondary direction represents the direction in which the cluster point cloud is less concentrated in a plane orthogonal to the primary direction. when When the clustered point cloud is a conductor, it is determined that it is an obstacle; otherwise, it is an obstacle. Here, λ1 represents the distribution variance of the primary direction and λ2 represents the distribution variance of the secondary direction.

4. The method for line-following flight based on radar laser point cloud ranging according to claim 1, characterized in that: The principle underlying the real-time calculation of the three attitude data of the UAV—roll angle, yaw angle, and throttle—in step 3 is as follows: The formula used to calculate the roll angle of a drone is: Where φ(t) represents the roll angle of the UAV at time t, and K p,1 K d,1 These represent the proportional gain and differential gain of the roll angle, respectively. The formula used to calculate the yaw angle of a drone is: Where γ(t) represents the yaw angle of the UAV at time t, and K p,2 K d,2 These represent the proportional gain and derivative gain used to adjust the yaw angle, respectively. The formula used to calculate the drone's throttle is: Where Y(t) represents the throttle of the drone at time t, and K p,3 K d,3 These represent the proportional gain and derivative gain for adjusting the throttle, respectively.

5. The method for linear flight based on radar laser point cloud ranging according to claim 4, characterized in that: The principle behind adjusting the drone's yaw angle and throttle based on the nearest obstacle position in step 4 is as follows: The formula used to calculate the coordinates of the nearest obstacle and its Euclidean distance from the drone's real-time position is as follows: Where D(t) represents the Euclidean distance between the real-time position of the UAV at time t and the coordinates of the nearest obstacle, x o (t), y o (t), z o (t) represent the horizontal, vertical, and axial coordinates of the nearest obstacle, respectively; Calculate the azimuth angle of the nearest obstacle relative to the drone: α0(t)=arctan2{[x u (t)-x o (t)] 2 ,[y u (t)-y o (t)] 2 } Where α0(t) represents the azimuth angle of the obstacle relative to the UAV at time t; The adjusted yaw angle is: c new (t)=γ(t)+M avoid ×[α0(t)-γ(t)] Where, γ new (t) represents the yaw angle adjusted for time t, M avoid Indicates the obstacle avoidance gain coefficient; The adjusted throttle is as follows: Among them, Y new (t) represents the throttle position after time t, M H D represents the height gain coefficient, and D0 represents the danger distance threshold.

6. A system for linear flight based on radar laser point cloud ranging, characterized in that: The system is used to implement the method for line-following flight based on radar laser point cloud ranging as described in any one of claims 1-5, specifically including: The point cloud acquisition module is used to acquire point cloud data of the environment around the UAV in real time. The environment around the UAV includes wires and obstacles. The point cloud data includes horizontal distance, vertical distance, scanning angle and point cloud density. The coordinate calculation module is used to set the point cloud density threshold, remove areas where the point cloud density is lower than the threshold, generate the polar coordinates of the remaining points in the point cloud based on the horizontal distance, vertical distance and scanning angle, and convert the polar coordinates into Cartesian coordinates. The attitude calculation module is used to classify point cloud data converted into Cartesian coordinates into guide point clouds and obstacle point clouds based on Euclidean clustering. The extended trajectory of the guide point cloud is used as the target path for the UAV to fly along the line. The directional deviation angle of the UAV to the target path is calculated based on the UAV's real-time position. Based on the vertical distance and directional deviation, the roll angle, yaw angle and throttle three attitude data of the UAV are calculated in real time. The attitude adjustment module is used to collect the point cloud position of the obstacle closest to the real-time position of the UAV, calculate the Euclidean distance between the two, and set a danger distance threshold. When the Euclidean distance is less than the danger distance threshold, the yaw angle and throttle of the UAV are adjusted based on the position of the closest obstacle point cloud. When the Euclidean distance is greater than the danger distance threshold, there is no need to adjust the attitude data of the UAV.

Images