Method and system for simulating line flight based on radar laser point cloud distance measurement

Abstract

The invention provides a method and a system for simulating line flight based on radar laser point cloud distance measurement, relates to the technical field of unmanned aerial vehicle flight control, and aims to eliminate low-density areas by collecting point cloud data of the surrounding environment of an unmanned aerial vehicle in real time and analyzing the point cloud density. And polar coordinates of the residual point clouds are converted into Cartesian coordinates, the Euclidean clustering algorithm is used to divide the point cloud data into a traverse point cloud and an obstacle point cloud, and an extension track of the traverse point cloud is used as a target path. The direction deviation between the unmanned aerial vehicle and the target path is calculated according to the real-time position of the unmanned aerial vehicle, and therefore the roll angle, the yaw angle and the accelerator are adjusted. Finally, the Euclidean distance between the unmanned aerial vehicle and the nearest obstacle is calculated and compared with a set dangerous distance threshold value, the yaw angle and an accelerator are adjusted in real time, and flight safety is ensured. According to the method, point cloud processing, path planning and dynamic obstacle avoidance technologies are combined, and an effective solution is provided for safe flight of the unmanned aerial vehicle in a complex environment.

Description

Technical Field

[0001] The present invention relates to the technical field of unmanned aerial vehicle (UAV) flight control, and in particular to a method and system for performing line-mimicking flight based on radar laser point cloud ranging. Background Art

[0002] Currently, drone inspections in the power industry mostly follow established routes to inspect towers or passageways between towers, but rarely inspect the conductors and ground wires on towers. Conductor and ground wire inspections require manual or automatic methods. Manual inspections require a high level of expertise from drone operators, who must constantly monitor drone flight safety. Automatic inspections, which follow a predetermined route, offer greater safety, but require route planning before automated inspections can be performed. This route-based inspection method is complex, requiring specialized route planning software before routine inspections can be conducted. Furthermore, inspections are ineffective, as they fail to accurately track the trajectory of conductors and ground wires.

[0003] In the existing technology, publication number CN115981366B discloses a UAV line-flight control method based on real-time recognition of power line point cloud targets. The method uses three-dimensional laser point cloud data to predict the UAV line-flight path to obtain the line-flight inspection self-flight path, thereby realizing the autonomous line-flight of the UAV. At the same time, the line-flight inspection self-flight path is updated in real time, reducing the time for path judgment during the UAV line-flight process.

[0004] The main problems with the above scheme are: it emphasizes real-time path updates, but in complex environments, frequent path adjustments may cause drone attitude oscillation or trajectory jitter, reducing flight stability. The above scheme does not take effective measures to avoid this problem; and it relies on continuous high-quality point cloud input, which has high requirements on the quality of the point cloud and is prone to path prediction failure.

[0005] The above information disclosed in this Background section is only for enhancement of understanding of the background of the present disclosure and therefore it may contain information that does not form the prior art that is already known to a person of ordinary skill in the art. Summary of the Invention

[0006] The purpose of the present invention is to provide a method and system for performing linear flight based on radar laser point cloud ranging to solve the problems raised in the above background technology.

[0007] To achieve the above object, the present invention provides the following technical solutions:

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

[0009] Step 1: Real-time acquisition of point cloud data of the drone's surroundings, including 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 into Cartesian coordinates;

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

[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 the danger distance threshold. When the Euclidean distance is less than the danger distance threshold, adjust the UAV'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 UAV's attitude data.

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

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

[0015] Among them, 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 area where the point cloud density is lower than the threshold, i∈[1,n], n represents the number of remaining points in the point cloud after removing the area where the point cloud density is lower than the threshold, d i (l) represents the horizontal distance from the i-th point to the UAV, d i (h) represents the vertical distance from the i-th point to the drone, θ i It represents the scanning angle of the i-th point when the UAV collects point cloud data;

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

[0017] The formula for converting the polar coordinates of the point cloud into Cartesian coordinates is:

[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] Among them, x i 、y i 、z i Respectively represent the horizontal, vertical, and vertical coordinates of the i-th point in the Cartesian coordinate system, p i Represents the Cartesian coordinates of the i-th point.

[0023] Furthermore,

[0024] The principle of distinguishing point clouds of wires and obstacles based on Euclidean clustering is:

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

[0026] The search radius of Euclidean clustering is set to 2 times 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] Among them, N ∈ (p i ) represents all the elements in P that are related to p i The point set whose distance is within the search radius, q i Indicates that in the set P, it is different from p i Any point, ∈ represents the search radius, ||p i -q|| means p i and q i The Euclidean distance between Represents point qi The horizontal, vertical and vertical coordinates of

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

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

[0032] Perform PCA principal component analysis on each cluster to obtain the cluster distribution variance in the main direction and 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.

[0033] when When , the clustered point cloud is judged to be a wire, otherwise it is an obstacle, where λ1 represents the distribution variance in the main direction and λ2 represents the distribution variance in the secondary direction.

[0034] Furthermore, the principle for calculating the directional deviation angle of the drone to the target path is:

[0035] The position of the drone at time t is u(t) = [x u (t),y u (t),z u (t)], the position of the UAV at time t-Δt is u(t-Δt)=[x u (t-Δt),y u (t-Δt),z u (t-Δt)], Δt represents the time interval between time t and time t-Δt;

[0036] Among them, t represents the current time, u(t) represents the real-time coordinates of the drone at time t, and x u (t), y u (t), z u (t) represents the real-time horizontal, vertical, and vertical coordinates of the UAV at time t, u(t-Δt) represents the position of the UAV at time t-Δt, and x u (t-Δt), y u (t-Δt), z u (t-Δt) represents the real-time horizontal, vertical, and vertical coordinates of the UAV at time t-Δt, respectively;

[0037] At time t, the point closest to u(t) on the extended trajectory of the wire point cloud is selected as the reference point at time t. The coordinates of the reference point are:

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

[0039] Among them, u′(t) represents the coordinates of the reference point, x′ u (t), y u ′(t), z′ u (t) represent the horizontal, vertical and vertical 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 wire point cloud and calculate the tangent vector between the reference points based on 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] Among them, u′(t-Δt) represents the coordinate of the reference point closest to u(t-Δt), x′ u (t-Δt), y u ′(t-Δt, zu′t-Δt represent the horizontal, vertical and vertical coordinates of u′t-Δt respectively, Tt 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 direction deviation angle is:

[0048]

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

[0050] Furthermore, the principle for real-time calculation of the UAV's roll angle, yaw angle, and throttle attitude data is based on:

[0051] The formula for calculating the roll angle of the drone is:

[0052]

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

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

[0055]

[0056] Among them, γ(t) represents the yaw angle of the UAV at time t, K p,2 , K d,2 Respectively represent the proportional gain and differential gain for adjusting the yaw angle;

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

[0058]

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

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

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

[0062]

[0063] 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, and x o (t), y o (t), z o (t) represents the horizontal, vertical and vertical coordinates of the nearest obstacle respectively;

[0064] Calculate the azimuth of the closest obstacle relative to the drone:

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

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

[0067] The adjusted yaw angle is:

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

[0069] Among them, γ new (t) represents the yaw angle after adjustment at time t, M avoid represents the obstacle avoidance gain coefficient;

[0070] The adjusted throttle is:

[0071]

[0072] Among them, Y new (t) represents the throttle after adjustment at time t, M H represents the height gain coefficient, and D0 represents the danger distance threshold.

[0073] The present invention also provides a system for performing line-flight simulation based on radar laser point cloud ranging, which is used to implement the above-mentioned method for performing line-flight simulation based on radar laser point cloud ranging, specifically comprising:

[0074] A point cloud acquisition module is used to collect point cloud data of the drone's surrounding environment in real time, wherein the drone's surrounding environment includes wires and obstacles, and the point cloud data includes horizontal distance, vertical distance, scanning angle, and point cloud density;

[0075] The coordinate calculation module is used to set the point cloud density threshold, eliminate 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;

[0076] The attitude calculation module is used to divide the point cloud data converted into Cartesian coordinates into wire point cloud and obstacle point cloud based on Euclidean clustering. The extended trajectory of the wire point cloud is used as the target path for the UAV to imitate the line flight. The directional deviation angle from the UAV to the target path is calculated based on the UAV's real-time position. The UAV's roll angle, yaw angle, and throttle angle are calculated in real time based on the vertical distance and directional deviation.

[0077] The attitude adjustment module is used to 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 the danger distance threshold. When the Euclidean distance is less than the danger distance threshold, the yaw angle and throttle of the drone are adjusted 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.

[0078] Compared with the prior art, the present invention has the following beneficial effects:

[0079] The present invention scans the point cloud data of the drone in real time during flight and distinguishes between wires and obstacles. While limiting the drone's flight path, it also identifies obstacles to avoid collisions, reducing flight risks and improving the drone's ability to respond flexibly in complex environments.

[0080] The present invention also enhances flight stability by determining the drone's real-time position and finding reference points along the extended path of the simulated line, adjusting the drone's roll angle, yaw angle, and throttle in real time to ensure timely course corrections. By adjusting flight strategies based on real-time data, the need for human intervention during flight is reduced, making drone operation easier and expanding its application range. The safety of simulated line flight is improved by collecting the position of the closest obstacle to the drone in real time and calculating the Euclidean distance between the two to assess potential collision risks. The drone's yaw angle and throttle are dynamically adjusted based on the distance between the drone and the obstacle to avoid collisions, enhancing the drone's flexibility and adaptability, allowing the drone to perform safe and reliable simulated line flight in complex environments. BRIEF DESCRIPTION OF THE DRAWINGS

[0081] Figure 1 This is a schematic diagram of a method flow in accordance with an embodiment of the present invention;

[0082] Figure 2 This is a point-line diagram of yaw angle change according to an embodiment of the present invention;

[0083] Figure 3 Schematic diagram of adjusting the azimuth angle to the yaw angle according to an embodiment of the present invention;

[0084] Figure 4 This is a point-line diagram of throttle change in an embodiment of the present invention;

[0085] Figure 5 A schematic diagram of throttle adjustment according to an embodiment of the present invention;

[0086] Figure 6 Schematic diagram of system modules according to an embodiment of the present invention. DETAILED DESCRIPTION

[0087] In order to make the objectives, technical solutions and advantages of the present invention more clearly understood, the present invention is further described in detail below with reference to specific embodiments.

[0088] It should be noted that, unless otherwise defined, the technical or scientific terms used in the present invention should have the usual meanings understood by people with ordinary skills in the field to which the present invention belongs. The "first", "second" and similar words used in the present invention do not indicate any order, quantity or importance, but are only used to distinguish different components. "Include" or "comprise" and similar words mean that the elements or objects appearing before the word include the elements or objects listed after the word and their equivalents, without excluding other elements or objects. "Connect" or "connected" and similar words are not limited to physical or mechanical connections, but may include electrical connections, whether direct or indirect. "Up", "down", "left", "right" and the like are only used to indicate relative position relationships. When the absolute position of the object being described changes, the relative position relationship may also change accordingly.

[0089] Example:

[0090] See also Figures 1 to 5 , the present invention provides a technical solution:

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

[0092] Step 1: Real-time acquisition of point cloud data of the drone's surroundings, including wires and obstacles. The point cloud data includes horizontal distance, vertical distance, scanning angle, and point cloud density.

[0093] In this implementation, a radar scanner is mounted on the drone to collect point cloud data of the surrounding environment in real time. The radar scanner model is a RIEGL three-dimensional laser scanner. With the radar scanner mounted on the drone as the coordinate origin, the horizontal distance, vertical distance and scanning angle of the point cloud of the wires and obstacles around the drone relative to the coordinate origin are scanned respectively to generate the polar coordinates of each point in the point cloud.

[0094] 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 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 valid points within a unit volume. For each point, the number of points N in the neighborhood of its search radius ∈ is counted. i , then the point cloud density is: Calculate the average point cloud density for the entire scan area 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. Among them, b represents the point cloud density adjustment coefficient, and b∈[0,1]. The closer the value of b is to 1, the stricter the point cloud filtering is, and sparse noise such as flying birds and dust can be more effectively removed. However, the point cloud density of wires at long distances is low, which may cause the true trajectory to be mistakenly deleted and the point cloud of thin wires to be completely filtered out. The closer the value of b is to 0, the looser the point cloud filtering is, which can ensure that the point clouds of distant wires and thin wires are not missed. However, irrelevant point clouds of noise may be retained, increasing the risk of false detection. The specific value of b is determined based on actual detection needs through expert scoring.

[0096] The polar coordinates of the point are:

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

[0098] Among them, 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 area where the point cloud density is lower than the threshold, i∈[1,n], n represents the number of remaining points in the point cloud after removing the area where the point cloud density is lower than the threshold, d i (l) represents the horizontal distance from the i-th point to the UAV, d i (h) represents the vertical distance from the i-th point to the drone, θ i It represents the scanning angle of the i-th point when the UAV collects point cloud data;

[0099] The roll axis, pitch axis and yaw axis of the drone are used as the horizontal, vertical and vertical axes of the Cartesian coordinate system respectively;

[0100] The formula for converting the polar coordinates of the point cloud into Cartesian coordinates 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] Among them, x i 、yi 、z i Respectively represent the horizontal, vertical, and vertical coordinates of the i-th point in the Cartesian coordinate system, p i Represents the Cartesian coordinates of the i-th point.

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

[0107] In this embodiment, the principle of distinguishing the point clouds of wires and obstacles based on Euclidean clustering is:

[0108] Construct a three-dimensional point set of all point clouds in the Cartesian coordinate system, P = {p1, p2, ..., p n}, where P represents the coordinate set of all points in the point cloud in the Cartesian coordinate system;

[0109] The search radius of Euclidean clustering is set to 2 times the diameter of the wire. For each point p i , find all points in P whose distance is within the search radius:

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

[0111]

[0112] Among them, N ∈ (p i ) represents all the elements in P that are related to p i The point set whose distance is within the search radius, q i Indicates that in the set P, it is different from p i Any point, ∈ represents the search radius, ||p i -q|| means p i and q i The Euclidean distance between Represents point q i The horizontal, vertical and vertical coordinates of

[0113] Statistical point set N ∈ (p i ) contains the number of points N(p i ), when N(p i )≥N0, then point pi is the core point; N0 represents the minimum neighborhood point threshold;

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

[0115] The clustering of core points is the combination of all points within the search radius. If the search radius is ∈, it is called ∈-neighborhood. ∈-neighborhood refers to the clustering of all points within the search radius. i As the center, the spherical space region with radius ∈, all points in the region are considered to be p i Neighbor, when p i When the number of points in the neighborhood exceeds the set minimum neighborhood point threshold, p i As the core point, p i Combine all points in its neighborhood to generate a point with p i For clustering of core points, when setting the minimum neighborhood point count threshold N0, if N0 is too small, extremely sparse clusters can be detected, but isolated noise points may be mistaken for core points, generating false clusters. If N0 is too large, only high-density areas can be identified, and some sparse wire point clouds may not form clusters. The specific N0 value is determined by expert scoring based on actual detection needs.

[0116] Perform PCA principal component analysis on each cluster to obtain the cluster distribution variance in the main direction and 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 clustered point cloud is judged to be a wire, otherwise it is an obstacle, where λ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 characteristics of the wire point cloud are the main direction, that is, the ductility of the wire extension direction is the largest, and the distribution variance is much larger than that of other directions. The secondary direction represents the width direction of the wire cross section. When , it means λ1>>λ2, which is consistent with the characteristics of the wire, and the shape of the point cloud appears as a slender linear structure.

[0119] The principle for calculating the directional deviation angle of the drone to the target path is:

[0120] The position of the drone at time t is u(t) = [x u (t),y u (t),z u (t)], the position of the UAV at time t-Δt is u(t-Δt)=[x u (t-Δt),y u(t-Δy),z u (t-Δy)], Δt represents the time interval between time t and time t-Δt;

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

[0122] Δt affects the control performance and path tracking accuracy of the UAV. During the line-flight process, the UAV needs to respond quickly to environmental changes to perform operations such as obstacle avoidance and heading change. The Δt value should be small, such as Δt = 0.1s, to improve the response speed.

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

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

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

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

[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] Among them, u′(t-Δt) represents the coordinate of the reference point closest to u(t-Δt), x′ u (t-Δt), y u′(t-Δt, zu′t-Δt represent the horizontal, vertical and vertical coordinates of u′t-Δt respectively, Tt represents the tangent vector;

[0130] The velocity vector of the drone is:

[0131]

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

[0133] The direction deviation angle is:

[0134]

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

[0136] The direction deviation angle represents the angle between the speed direction of the UAV and the extension direction of the target path, reflecting the degree of deviation between the flight direction of the UAV and the wire trajectory. The angle value is used to directly measure whether the UAV is moving along the extension trajectory of the wire to avoid the cumulative error caused by direction deviation. When α(t) = 0°, it means that the heading of the UAV is completely consistent with the direction of the wire and no heading adjustment is required. When α(t) ≠ 0°, it is necessary to control the yaw angle. The value of the direction deviation angle is determined by the current speed vector direction of the UAV and the tangent vector of the reference point. The direction of the speed vector reflects the target direction in which the UAV is flying, and the direction of the tangent vector of the reference point reflects the extension direction of the path. The direction deviation angle is calculated based on the speed vector and the tangent vector, reflecting the degree of deviation between the UAV and 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°, which is the maximum deviation. When the two vectors are in opposite directions, the deviation angle is 180°, which is a complete deviation.

[0137] The principle of real-time calculation of the UAV's roll angle, yaw angle and throttle attitude data is based on:

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

[0139]

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

[0141] The roll angle represents the angle of rotation of the drone around the x-axis, i.e. the roll axis. It is used to control the left and right tilt of the drone. The roll angle will cause the drone to generate lateral acceleration, thereby changing its position in the y-axis direction. p,1 ×[y u (t)-y u ′(t)] generates the control adjustment amount according to the current horizontal deviation. The larger the deviation, the larger the adjustment amplitude, and y u (t)-y u When ′(t)>0, it means that the drone is on the right side of the wire and needs to tilt to the left. u (t)-y u When ′(t)<0, it means that the UAV is on the left side of the wire and needs to tilt to the right. p,1 Affects the speed of response, K p,1 The larger the value, the faster the response, but it may overshoot or oscillate. p,1 If it is too small, slow response and tracking lag may occur; It means that damping is provided according to the rate of change of the deviation to suppress oscillation and avoid overshoot of the roll angle adjustment amplitude. If K d,1 If K is too large, the roll angle adjustment will be slow to respond. d,1 If it is too small, the oscillation cannot be effectively suppressed; K is determined based on experimental parameters. p,1 and K d,1 , starting from a small value, K p,1 =0.1, K d,1 = 0, ensure that the UAV starts in a stable state and gradually increase K p,1 Until the drone starts to oscillate, the oscillation means that the drone is shaking back and forth along the path of the wire extension trajectory, and record the K at this time p,1 , from K d,1 =0.1K p,1 Start by gradually increasing K d,1 Until the drone is firmly attached to the wire and the oscillation disappears, record the K at this time. d,1 .

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

[0143]

[0144] Among them, γ(t) represents the yaw angle of the UAV at time t, K p,2 , K d,2 Respectively represent the proportional gain and differential gain for adjusting the yaw angle;

[0145] The yaw angle indicates the angle at which the drone rotates around the vertical axis. Its function is to keep the drone's flight direction consistent with the direction of the wire extension path. This is manifested by controlling the drone's nose to turn left or right. When the drone's heading deviates from the wire direction, that is, α(t)≠0°, the yaw controls the drone to turn in the direction of the wire extension path. The larger α(t) is, the stronger the correction force is. Used to suppress oscillations caused by inertia or external disturbances, and to limit the rate of change of the yaw angle to avoid excessive oscillation amplitude. When K p,2 If K is too large, the drone will turn violently and may overshoot or oscillate. p,2 If K is too small, the drone will respond slowly and it will be difficult to quickly align the path. d,2 If the K d,2 If it is too small, the oscillation cannot be effectively suppressed, and the heading of the drone will swing repeatedly. Based on experimental parameter adjustment, K is determined. p,2 and K d,2 The variation of the yaw angle of the UAV with the direction deviation angle is shown in Table 1. p,2 =0.5, K d,2 =0.2.

[0146] Table 1. Yaw angle change table

[0147]

[0148]

[0149] The formula for calculating the throttle of a drone is:

[0150]

[0151] Among them, Y(t) represents the throttle of the UAV at time t, K p,3 , K d,3 They represent the proportional gain and differential gain of the throttle adjustment respectively.

[0152] The throttle is used to control the vertical rise and fall of the UAV, which directly affects the height of the UAV, that is, the position of the z-axis. In line flight, without considering obstacles, the goal of the throttle is to keep the UAV at a vertical distance from the wire during flight. The throttle output is calculated by the deviation between the height of the UAV's current position and the height of the reference point. p,3 ×[z u (t)-z' u(t)] represents the adjustment amplitude of the throttle based on the altitude deviation. If the UAV is below the reference point altitude, the throttle is increased to climb; if the UAV is above the reference point altitude, the throttle is reduced to descend. However, adjusting only through the proportional term will cause the UAV to oscillate repeatedly near the target altitude. The oscillation is suppressed by the rate of change of the altitude deviation. If the UAV approaches the target altitude quickly, the throttle is reduced to avoid overshoot. If the UAV moves away from the target, the throttle is increased. K is determined based on experimental parameter adjustment. p,3 and K d,3 , the throttle changes of the UAV are shown in Table 2, take K p,3 =0.5, K d,3 =0.1.

[0153] Table 2. Throttle change table

[0154]

[0155]

[0156] 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 the danger distance threshold. When the Euclidean distance is less than the danger distance threshold, adjust the UAV'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 UAV's attitude data.

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

[0158] The formula for calculating the coordinates of the closest obstacle and the Euclidean distance to the real-time position of the drone is:

[0159]

[0160] 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, and x o (t), y o (t), z o (t) represents the horizontal, vertical and vertical coordinates of the nearest obstacle respectively;

[0161] Calculate the azimuth of the closest obstacle relative to the drone:

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

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

[0164] The adjusted yaw angle is:

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

[0166] Among them, γ new (t) represents the yaw angle after adjustment at time t, M avoid represents the obstacle avoidance gain coefficient;

[0167] The adjusted throttle is:

[0168]

[0169] Among them, Y new (t) represents the throttle after adjustment at time t, M H represents the height gain coefficient, and D0 represents the danger distance threshold.

[0170] The yaw angle and throttle affect the horizontal and vertical directions of the drone respectively, so the two attitude data are mainly adjusted. The azimuth of the obstacle is calculated based on the relative position of the drone and the nearest obstacle. The yaw angle is adjusted according to the azimuth of the obstacle relative to the position of the drone. If the obstacle is on the right side of the drone, the yaw angle is increased, otherwise the yaw angle is reduced. The obstacle avoidance gain coefficient M avoid Determines the drone's sensitivity to the obstacle's azimuth. avoid The larger the value, the more sensitive the drone is to the adjustment of the azimuth angle, and the more obvious the change of the yaw angle. avoid =0.3, to balance the stability and flexibility of the drone; the throttle controls the lift of the drone and directly adjusts the altitude to avoid obstacles. The altitude is adjusted based on the relative position of the obstacle and the drone. If the obstacle is above, the throttle is reduced to lower the altitude. If the obstacle is below, the throttle is increased to raise the altitude. The adjustment amplitude decays smoothly as the distance decreases to avoid overshoot. The altitude gain coefficient M H It reflects the adjustment sensitivity of the UAV to the vertical direction during the obstacle avoidance process. H The larger the value, the faster the drone's obstacle avoidance response speed will be, but the adjustment process may be unstable. H The smaller it is, the slower the response may be. H ∈[0.2,0.5]; Adjustment of the roll angle will cause the drone to tilt and produce lateral displacement, but the power line conductor environment is usually narrow, and lateral displacement is likely to cause side collision, so no adjustment is made. The adjusted yaw angle is shown in Table 3, and the adjusted throttle is shown in Table 4.

[0171] Table 3. Yaw angle adjustment diagram

[0172]

[0173]

[0174] Table 4. Throttle adjustment diagram.

[0175]

[0176]

[0177] See also Figure 6 The present invention also provides a system for performing line-mimicking flight based on radar point cloud ranging, and the system is used to implement the above-mentioned method for performing line-mimicking flight based on radar point cloud ranging, specifically comprising:

[0178] A point cloud acquisition module is used to collect point cloud data of the drone's surrounding environment in real time, wherein the drone's surrounding environment includes wires and obstacles, and the point cloud data includes horizontal distance, vertical distance, scanning angle, and point cloud density;

[0179] The coordinate calculation module is used to set the point cloud density threshold, eliminate 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;

[0180] The attitude calculation module is used to divide the point cloud data converted into Cartesian coordinates into wire point cloud and obstacle point cloud based on Euclidean clustering. The extended trajectory of the wire point cloud is used as the target path for the UAV to imitate the line flight. The directional deviation angle from the UAV to the target path is calculated based on the UAV's real-time position. The UAV's roll angle, yaw angle, and throttle angle are calculated in real time based on the vertical distance and directional deviation.

[0181] The attitude adjustment module is used to 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 the danger distance threshold. When the Euclidean distance is less than the danger distance threshold, the yaw angle and throttle of the drone are adjusted 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.

[0182] The above formulas are all dimensionless and numerical calculations. The formulas are obtained by collecting a large amount of data and performing software simulation to obtain the most recent real situation. The preset parameters in the formulas are set by technicians in this field according to actual conditions.

[0183] The above embodiments can be implemented in whole or in part by software, hardware, firmware, or any other combination thereof. When implemented using software, the above embodiments can be implemented in whole or in part in the form of a computer program product. Those skilled in the art will appreciate that the units and algorithm steps of each example described in conjunction with the embodiments disclosed herein can be implemented by electronic hardware, or a combination of computer software and electronic hardware. Whether these functions are performed by hardware or software depends on the specific application and design constraints of the technical solution.

[0184] The units described as separate components may or may not be physically separate, and the components shown as units may or may not be physical units, and may be located in one place or distributed across multiple network units. Some or all of these units may be selected to achieve the purpose of this embodiment as needed.

[0185] The above is only a specific implementation method of the present application, but the scope of protection of the present application is not limited thereto. Any technician familiar with this technical field can easily think of changes or replacements within the technical scope disclosed in this application, which should be covered by the scope of protection of the present application.

Claims

1. A method for linear flight based on radar laser point cloud ranging, characterized in that: The specific steps include: Step 1: Real-time acquisition of point cloud data of the drone's surroundings, including 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 into Cartesian coordinates; Step 3: Based on the point cloud data converted into Cartesian coordinates, it is divided into wire point cloud and obstacle point cloud based on Euclidean clustering. The extended trajectory of the wire point cloud is used as the target path for the UAV to imitate the line. The directional deviation angle from the UAV to the target path is calculated based on the real-time position of the UAV. The roll angle, yaw angle, and throttle attitude data of the UAV are calculated in real time based on the vertical distance and directional deviation. 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 the danger distance threshold. When the Euclidean distance is less than the danger distance threshold, adjust the UAV'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 UAV's attitude data.

2. The method for performing line flight based on radar laser point cloud ranging according to claim 1, characterized in that: The polar coordinates of the midpoint in step 2 are: p i ′ =[d i (l),d i (h),θ i ] Among them, 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 area where the point cloud density is lower than the threshold, i∈[1,n], n represents the number of remaining points in the point cloud after removing the area where the point cloud density is lower than the threshold, d i (l) represents the horizontal distance from the i-th point to the UAV, d i (h) represents the vertical distance from the i-th point to the drone, θ i It represents the scanning angle of the i-th point when the UAV collects point cloud data; The roll axis, pitch axis and yaw axis of the drone are used as the horizontal, vertical and vertical axes of the Cartesian coordinate system respectively; The formula for converting the polar coordinates of the point cloud into Cartesian coordinates is: 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 ) Among them, x i 、y i 、z i Respectively represent the horizontal, vertical, and vertical coordinates of the i-th point in the Cartesian coordinate system, p i Represents the Cartesian coordinates of the i-th point.

3. The method for performing line flight based on radar laser point cloud ranging according to claim 2, characterized in that: The principle of distinguishing the point clouds of wires and obstacles based on Euclidean clustering in step 3 is: Construct a three-dimensional point set of all point clouds in the Cartesian coordinate system, P = {p1, p2, ..., p n }, where P represents the coordinate set of all points in the point cloud in the Cartesian coordinate system; The search radius of Euclidean clustering is set to 2 times the diameter of the wire. For each point p i , find all points in P whose distance is within the search radius: N ∈1 (p i )={q i ∈P,i∈[1,n]|||p i -q i ||≤∈} Among them, N ∈ (p i ) represents all the elements in P that are related to p i The point set whose distance is within the search radius, q i Indicates that in the set P, it is different from p i Any point, ∈ represents the search radius, ||p i -q|| means p i and q i The Euclidean distance between Represents point q i The horizontal, vertical and vertical coordinates of Statistical point set N ∈ (p i ) contains the number of points N(p i ), when N(p i )≥N0, then point p i is the core point; N0 represents the minimum neighborhood point threshold; For each core point, all points within its search radius are combined to generate a cluster; Perform PCA principal component analysis on each cluster to obtain the cluster distribution variance in the main direction and 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. when When , the clustered point cloud is judged to be a wire, otherwise it is an obstacle, where λ1 represents the distribution variance in the main direction and λ2 represents the distribution variance in the secondary direction.

4. The method for performing line flight based on radar laser point cloud ranging according to claim 1, characterized in that: The principle for calculating the directional deviation angle from the drone to the target path in step 3 is: The position of the drone at time t is u(t) = [x u (t),y u (t),z u (t)], the position of the UAV at time t-Δt is u(t-Δt)=[x u (t-Δt),y u (t-Δt),z u (t-Δt)], Δt represents the time interval between time t and time t-Δt; Among them, t represents the current time, u(t) represents the real-time coordinates of the drone at time t, and x u (t), y u (t), z u (t) represents the real-time horizontal, vertical, and vertical coordinates of the UAV at time t, u(t-Δt) represents the position of the UAV at time t-Δt, and x u (t-Δt), y u (t-Δt), z u (t-Δt) represents the real-time horizontal, vertical, and vertical coordinates of the UAV at time t-Δt, respectively; At time t, the point closest to u(t) on the extended trajectory of the wire point cloud 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)] Among them, u′(t) represents the coordinates of the reference point, x′ u (t), y′ u (t), z′ u (t) represent the horizontal, vertical and vertical coordinates of the reference point respectively; Obtain the coordinates of the reference point closest to u(t-Δt) on the extended trajectory of the wire point cloud and calculate the tangent vector between the reference points based on 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) Among them, u ′ (t-Δt) represents the coordinate of the reference point closest to u(t-Δt), x′ u (t-Δt), y′ u (t-Δt, zu′t-Δt represent the horizontal, vertical and vertical coordinates of u′t-Δt respectively, Tt 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 direction deviation angle is: Among them, α(t) represents the directional deviation angle of the UAV at time t, ||v u (t)|| and ||T(t)|| represent the modulus of the velocity vector and the tangent vector respectively.

5. The method for performing line flight based on radar laser point cloud ranging according to claim 4, characterized in that: The principle for calculating the three attitude data of the UAV, namely, the roll angle, the yaw angle and the throttle, in real time in step 3 is as follows: The formula for calculating the roll angle of the drone is: Among them, φ(t) represents the roll angle of the UAV at time t, K p,1 , K d,1 Represent the proportional gain and differential gain of the roll angle respectively; The formula for calculating the yaw angle of the drone is: Among them, γ(t) represents the yaw angle of the UAV at time t, K p,2 , K d,2 Respectively represent the proportional gain and differential gain for adjusting the yaw angle; The formula for calculating the throttle of a drone is: Among them, Y(t) represents the throttle of the UAV at time t, K p,3 , K d,3 They represent the proportional gain and differential gain of the throttle adjustment respectively.

6. The method for performing line flight based on radar laser point cloud ranging according to claim 5, characterized in that: The principle of adjusting the yaw angle and throttle of the drone based on the position of the closest obstacle in step 4 is: The formula for calculating the coordinates of the closest obstacle and the Euclidean distance to the real-time position of the drone is: 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, and x o (t), y o (t), z o (t) represents the horizontal, vertical and vertical coordinates of the nearest obstacle respectively; Calculate the azimuth of the closest 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)] Among them, γ new (t) represents the yaw angle after adjustment at time t, M avoid represents the obstacle avoidance gain coefficient; The adjusted throttle is: Among them, Y new (t) represents the throttle after adjustment at time t, M H represents the height gain coefficient, and D0 represents the danger distance threshold.

7. A system for linear flight based on radar laser point cloud ranging, characterized by: The system is used to implement the method for performing line-flight based on radar laser point cloud ranging as described in any one of claims 1 to 6, specifically comprising: A point cloud acquisition module is used to collect point cloud data of the drone's surrounding environment in real time, wherein the drone's surrounding environment includes wires and obstacles, and 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, eliminate 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 divide the point cloud data converted into Cartesian coordinates into wire point cloud and obstacle point cloud based on Euclidean clustering. The extended trajectory of the wire point cloud is used as the target path for the UAV to imitate the line flight. The directional deviation angle from the UAV to the target path is calculated based on the UAV's real-time position. The UAV's roll angle, yaw angle, and throttle angle are calculated in real time based on the vertical distance and directional deviation. The attitude adjustment module is used to 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 the danger distance threshold. When the Euclidean distance is less than the danger distance threshold, the yaw angle and throttle of the drone are adjusted 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.

Images