An unmanned aerial vehicle flight control system based on path constraints

Abstract

The application relates to the field of flight control technology and discloses a path-constraint-based unmanned aerial vehicle flight control system, which comprises a constraint analysis unit, a deviation calculation unit, a constraint association unit, a trajectory correction unit and a flight control unit. The scheme changes the open-loop control of the unmanned aerial vehicle from only tracking preset navigation points to vector attitude correction based on original space constraints, suppresses trajectory deviation caused by positioning drift and wind speed disturbance, realizes independent adjustment of an angle channel and a distance channel, improves the anti-interference capability of power line patrol flight control, and guarantees the effect of unmanned aerial vehicle flight control.

Description

Technical Field

[0001] This invention relates to the field of flight control technology, and more specifically to a path-constrained unmanned aerial vehicle (UAV) flight control system. Background Technology

[0002] In power line inspection using drones, flight control is typically achieved based on a preset waypoint sequence. Operators pre-mark the locations of inspection targets such as towers, insulator strings, and ground wires on a 3D point cloud view. The software then generates fixed waypoints based on these marks. During flight, the drone only tracks and executes the preset waypoints, thus achieving path constraints during flight control. However, the above control method still has the following defects: During the flight of the UAV, it may deviate from the preset waypoint due to external factors such as positioning drift and wind speed disturbance. The flight controller only returns the UAV to the preset waypoint and does not verify the relative position constraints of the inspection target. This means that once the trajectory deviates, it is impossible to complete the trajectory correction based on the original inspection target constraints, thus affecting the control effect of the UAV flight. Summary of the Invention

[0003] To address the shortcomings of existing technologies, this invention provides a path-constrained unmanned aerial vehicle (UAV) flight control system, which solves the aforementioned problems.

[0004] The above-mentioned technical objective of the present invention is achieved through the following technical solution: A path-constrained unmanned aerial vehicle (UAV) flight control system includes: The constraint analysis unit is used to acquire spatial constraint data of the inspection targets, analyze the spatial constraint data to obtain a first spatial vector and a second spatial vector, and construct and analyze a spatial inner product matrix based on the first spatial vector and the second spatial vector to obtain the spatial constraint scale representing the spatial constraints between each inspection target in the absence of interference. The deviation calculation unit is used to acquire the three-dimensional coordinates of the current position of the controlled object in real time. Based on the spatial constraint data, it analyzes the degree of angular deviation of the current position of the controlled object relative to the azimuth of the inspection target and obtains the azimuth deviation value. At the same time, it performs deviation analysis between the current position of the controlled object and the tower position and spatial constraint scale in the spatial constraint data to obtain the target relative deviation value representing the distance deviation of the tower position. The constraint association unit is used to analyze the positional deviation value and the target relative deviation value to obtain a column vector. The column vector is then subjected to exponential decay calculation with the spatial inner product matrix to obtain the constraint association coefficient, which represents the degree of constraint deviation between the current position of the controlled object and the tower position. The trajectory correction unit is used to determine the safety threshold based on the constraint correlation coefficient, and then compare and analyze the constraint correlation coefficient with the safety threshold to obtain the trajectory correction weight, which represents the priority of trajectory correction under the current deviation state. The flight control unit is used to calculate and analyze the error components in the horizontal and vertical planes based on the trajectory correction weights and spatial constraint scales, and generate flight control commands to drive the controlled object to move from its current deviation position to the constrained path of the inspection target.

[0005] Furthermore, spatial constraint data includes: The three-dimensional coordinates of the tower location, insulator string, and ground wire during the inspection.

[0006] Furthermore, the spatial constraint data is analyzed to obtain a first spatial vector and a second spatial vector. A spatial inner product matrix is ​​constructed based on the first and second spatial vectors and analyzed to obtain the spatial constraint scale representing the space constraints between each inspection target under interference-free conditions, including: Using the three-dimensional coordinates of the tower location in the spatial constraint data as the spatial reference point, the first and second spatial vectors of the three-dimensional coordinates of the insulator string and the three-dimensional coordinates of the ground wire relative to the three-dimensional coordinates of the tower location are calculated respectively. Calculate the magnitude of the first spatial vector and the magnitude of the second spatial vector respectively, and calculate the inner product of the first spatial vector and the second spatial vector to obtain the inner product value. Construct the inner product matrix based on the first spatial vector, the second spatial vector and the inner product value. By analyzing the spatial inner product matrix, the envelope radius and spatial envelope value of the spatial constraints are determined, and the spatial constraint scale is obtained.

[0007] Furthermore, the spatial inner product matrix is ​​analyzed to determine the envelope radius and spatial envelope value of the spatial constraints, thus obtaining the spatial constraint scale, including: Calculate the determinant of the spatial inner product matrix to obtain the spatial constraint envelope radius, calculate the cross product vector of the first spatial vector and the second spatial vector, and use the magnitude of the cross product vector as the spatial envelope value. Divide the spatial envelope value by the sum of the first spatial vector magnitude and the second spatial vector magnitude to obtain the spatial constraint height value. Calculate the first spatial vector magnitude, the second spatial vector magnitude, the spatial constraint envelope radius, and the spatial constraint height value to obtain the spatial constraint scale representing the space constraints between each inspection target in an interference-free state.

[0008] Furthermore, based on the spatial constraint data analysis, the degree of angular deviation of the current position of the controlled object relative to the azimuth of the inspection target is obtained, yielding the azimuth deviation value. Simultaneously, a deviation analysis is performed between the current position of the controlled object and the tower position and spatial constraint scale in the spatial constraint data to obtain the target relative deviation value representing the distance deviation from the tower position, including: Divide the cross product vector of the first spatial vector and the second spatial vector by its own magnitude to obtain a unit vector, wherein the cross product vector is perpendicular to both the first spatial vector and the second spatial vector. Using the unit vector as the reference direction for the inspection target's orientation, the difference between the three-dimensional coordinates of the current position of the controlled object and the three-dimensional coordinates of the tower position is calculated to obtain the third spatial vector. The third spatial vector is then analyzed with the reference direction to obtain the orientation deviation value, which represents the degree of angular deviation of the third spatial vector relative to the reference direction. The magnitude of the third spatial vector is used as the real-time distance between the current position of the control object and the tower location. The real-time distance and the spatial constraint scale are calculated to obtain the target relative deviation value representing the deviation of the tower location distance.

[0009] Furthermore, the positional deviation value and the target relative deviation value are analyzed to obtain a column vector. The column vector is then subjected to exponential decay calculation with the spatial inner product matrix to obtain the constraint correlation coefficient, representing the degree of constraint deviation between the current position of the controlled object and the tower position. This coefficient includes: Divide each element of the spatial inner product matrix by the square of the spatial constraint scale to obtain the spatial constraint matrix, and then solve for the inverse matrix of the spatial constraint matrix. The azimuth deviation value and the target relative deviation value are used to construct a two-dimensional deviation column vector. The exponential decay value is obtained by calculating the deviation column vector and the inverse matrix. The difference between 1 and the exponential decay value is used as the constraint correlation coefficient for the degree of constraint deviation between the current position of the controlled object and the tower position.

[0010] Furthermore, a safety threshold is determined based on the constraint correlation coefficient. Then, the constraint correlation coefficient is compared and analyzed with the safety threshold to obtain the trajectory correction weight, which represents the priority of trajectory correction under the current deviation state. This weight includes: A two-dimensional constraint column vector is constructed based on the constraint correlation coefficient. The constraint column vector contains two components, namely the first constraint component and the second constraint component. The constraint quadratic form value is obtained by calculating the transpose of the constraint column vector, the inverse matrix of the spatial constraint matrix, and the constraint column vector. The quadratic values ​​of the constraints are analyzed to determine the safety threshold, and the trajectory correction weight is calculated based on the correlation coefficient between the safety threshold and the constraints.

[0011] Furthermore, the quadratic values ​​of the constraints are analyzed to determine the safety threshold, and the trajectory correction weights are calculated based on the correlation coefficient between the safety threshold and the constraints, including: The square root of the constraint quadratic value is taken to obtain the distance modulus. The distance modulus is then calculated to obtain the safety threshold. The constraint correlation coefficient is compared with the safety threshold to obtain the trajectory correction weight, which represents the priority of trajectory correction under the current deviation state.

[0012] Furthermore, based on trajectory correction weights and spatial constraint scales, calculations are performed to analyze the error components in the horizontal and vertical planes, generating flight control commands to drive the controlled object from its current deviation position to the constrained path of the inspection target, including: The trajectory correction weight is multiplied by the spatial constraint scale to obtain the expected distance. Then, the unit vector of the third spatial vector is found, and the unit vector is calculated with the expected distance to obtain the expected position offset vector. Based on the expected position offset vector and the three-dimensional coordinates of the tower, the position error vector is determined. The position error vector is decomposed into horizontal plane error components and vertical plane error components, where the horizontal plane error components are the projections of the position error vector in the horizontal plane, and the vertical plane error components are the projections of the position error vector in the vertical direction. Based on the analysis of the horizontal and vertical error components, yaw control commands, pitch control commands, and roll control commands are obtained. The combination of yaw control commands, pitch control commands, and roll control commands yields the flight control commands.

[0013] Furthermore, based on the analysis of the horizontal and vertical error components, yaw control commands, pitch control commands, and roll control commands are obtained. The combination of these commands yields the flight control commands, including: The direction of the direction from the three-dimensional coordinates of the tower to the current position is projected in the horizontal plane as the horizontal projection direction of the connecting line. When the horizontal projection direction of the connecting line is a 0 vector, a control command to not adjust the yaw direction and a control command to not adjust the roll direction are generated. Otherwise, the horizontal plane error component and the horizontal projection direction of the connecting line are respectively used as three-dimensional vectors with a z component of 0 to perform cross product operation to obtain the yaw cross product vector. Based on the vertical component of the yaw cross product vector, the yaw direction control command is generated. The pitch control command is generated based on the vertical plane error component. The horizontal plane error component is decomposed into a component along the horizontal projection direction of the connecting line and a component perpendicular to the connecting line. The component perpendicular to the connecting line is taken as the roll error component. The horizontal projection direction of the connecting line and the roll error component are respectively extended into three-dimensional vectors with a z-component of 0, and then cross product operation is performed to obtain the roll cross product vector. The roll control command is generated based on the vertical component of the roll cross product vector. The yaw control command, pitch control command, and roll control command are combined into a flight control command used to drive the controlled object to move from its current deviation position to the constrained path of the inspected target.

[0014] In summary, the present invention has the following main beneficial effects: The constraint analysis unit calculates the difference between the three-dimensional coordinates of the insulator string and the ground wire to obtain the first and second spatial vectors. After constructing a spatial inner product matrix, analysis is performed to obtain the spatial constraint scale of the geometric relationship of each inspection target under interference-free conditions. This avoids the reference drift problem caused by simply relying on the absolute coordinates of preset waypoints. The deviation calculation unit calculates the azimuth deviation and target relative deviation, quantitatively decomposing the trajectory deviation into two independent dimensions: angle and distance. This allows the controller to distinguish the type of deviation. The constraint correlation coefficient calculated by the constraint correlation unit effectively prevents control overshoot and oscillation. The trajectory correction unit obtains the trajectory correction weight, and the trajectory... The correction weights remain small and only fine-tune when there is a slight deviation, and monotonically increase to approach 1 but never saturate when there is a severe deviation. This provides the controller with continuous and reasonable guidance on correction priority and intensity. The yaw control command, pitch control command, and roll control command generated by the flight control unit are combined into flight control commands. This scheme transforms the UAV from open-loop control that only tracks preset waypoints to vector attitude correction based on the original spatial constraints. It suppresses trajectory deviation caused by positioning drift and wind speed disturbances, realizes independent adjustment of angle and distance channels, improves the anti-interference capability of power line inspection flight control, and ensures the effectiveness of UAV flight control. Attached Figure Description

[0015] Figure 1 This is a block diagram of the path-constrained unmanned aerial vehicle (UAV) flight control system of the present invention; Figure 2 This is a flowchart of the spatial constraint scale calculation of the present invention; Figure 3 This is a flowchart for calculating the target relative deviation value of the present invention. Detailed Implementation

[0016] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0017] refer to Figure 1 , Figure 2 and Figure 3 A path-constrained unmanned aerial vehicle (UAV) flight control system includes: The constraint analysis unit is used to acquire spatial constraint data of the inspection targets, analyze the spatial constraint data to obtain a first spatial vector and a second spatial vector, and construct and analyze a spatial inner product matrix based on the first spatial vector and the second spatial vector to obtain the spatial constraint scale representing the spatial constraints between each inspection target in an interference-free state. The inspection target is the target that needs to be inspected.

[0018] The deviation calculation unit is used to acquire the three-dimensional coordinates of the current position of the controlled object in real time. Based on the spatial constraint data, it analyzes the degree of angular deviation of the current position of the controlled object relative to the azimuth of the inspection target and obtains the azimuth deviation value. At the same time, it performs deviation analysis between the current position of the controlled object and the tower position and spatial constraint scale in the spatial constraint data to obtain the target relative deviation value representing the distance deviation of the tower position. The controlled object is a drone; The constraint association unit is used to analyze the positional deviation value and the target relative deviation value to obtain a column vector. The column vector is then subjected to exponential decay calculation with the spatial inner product matrix to obtain the constraint association coefficient, which represents the degree of constraint deviation between the current position of the controlled object and the tower position. The trajectory correction unit is used to determine the safety threshold based on the constraint correlation coefficient, and then compare and analyze the constraint correlation coefficient with the safety threshold to obtain the trajectory correction weight, which represents the priority of trajectory correction under the current deviation state. The flight control unit is used to calculate and analyze the error components in the horizontal and vertical planes based on the trajectory correction weights and spatial constraint scales, and generate flight control commands to drive the controlled object to move from its current deviation position to the constrained path of the inspection target.

[0019] In one embodiment, the spatial constraint data includes: During the inspection, the three-dimensional coordinates of the tower location, insulator string, and ground wire are respectively included. In one embodiment, spatial constraint data is analyzed to obtain a first spatial vector and a second spatial vector. A spatial inner product matrix is ​​constructed based on the first and second spatial vectors and analyzed to obtain a spatial constraint scale representing the spatial constraints between inspection targets under interference-free conditions, including: Using the three-dimensional coordinates of the tower location in the spatial constraint data as the spatial reference point, the first spatial vector and the second spatial vector of the three-dimensional coordinates of the insulator string and the three-dimensional coordinates of the ground wire relative to the three-dimensional coordinates of the tower location are calculated respectively, and the first spatial vector and the second spatial vector are linearly independent. The first spatial vector is the difference between the three-dimensional coordinates of the insulator string and the three-dimensional coordinates of the tower, resulting in the difference components in three directions. The second spatial vector is the difference between the three-dimensional coordinates of the ground wire and the three-dimensional coordinates of the tower, resulting in the difference components in three directions. Calculate the magnitudes of the first and second spatial vectors respectively, and calculate the inner product of the first and second spatial vectors to obtain the inner product value. Construct a spatial inner product matrix based on the first and second spatial vectors and the inner product value. The spatial inner product matrix is ​​2 rows and 2 columns. The main diagonal elements are the squares of the magnitudes of the first and second spatial vectors respectively, and the secondary diagonal elements are all the inner product values. By analyzing the spatial inner product matrix, the envelope radius and spatial envelope value of the spatial constraints are determined, and the spatial constraint scale is obtained.

[0020] In one embodiment, the spatial inner product matrix is ​​analyzed to determine the envelope radius and spatial envelope value of the spatial constraint, thereby obtaining the spatial constraint scale, including: The determinant of the spatial inner product matrix is ​​calculated to obtain the spatial constraint envelope radius. Specifically, this involves subtracting the square of the spatial inner product value from the product of the square of the magnitude of the first spatial vector and the square of the magnitude of the second spatial vector to obtain the determinant of the spatial inner product matrix. Then, the fourth root of the determinant value is taken to obtain the spatial constraint envelope radius. Calculate the cross product vector of the first spatial vector and the second spatial vector, and use the magnitude of the cross product vector as the spatial envelope value. Specifically, this includes: multiplying the second component of the first spatial vector by the third component of the second spatial vector and subtracting the multiplication of the third component of the first spatial vector by the second component of the second spatial vector to obtain the first component. Multiply the third component of the first spatial vector by the first component of the second spatial vector, and then subtract the first component of the first spatial vector multiplied by the third component of the second spatial vector to obtain the second component. Multiply the first component of the first spatial vector by the second component of the second spatial vector, and then subtract the multiplication of the second component of the first spatial vector by the first component of the second spatial vector to obtain the third component. The first component, the second component, and the third component constitute the cross product vector; Divide the spatial envelope value by the sum of the first spatial vector magnitude and the second spatial vector magnitude to obtain the spatial constraint height value. Perform a square root operation on the first spatial vector magnitude, the second spatial vector magnitude, the spatial constraint envelope radius, and the spatial constraint height value to obtain the spatial constraint scale representing the space between each inspection target in an interference-free state.

[0021] By subtracting the three-dimensional coordinates of the insulator string and the ground wire from the three-dimensional coordinates of the tower position using the same length unit, linearly independent first and second spatial vectors are obtained. Then, a spatial inner product matrix is ​​constructed, and the spatial constraint envelope radius and spatial envelope value are calculated. Finally, the spatial constraint scale representing the spatial constraint between each inspection target under interference-free conditions is calculated. The spatial constraint scale does not only depend on the absolute position of the preset waypoint, but also extracts relatively invariant constraint features from the inherent relationship of the tower. The flight controller can use this to verify the spatial constraint relationship between the current pose and the inspection target in real time, effectively resisting external interference such as positioning drift and wind speed disturbance, realizing trajectory correction based on the spatial constraint scale, and improving the flight control of UAV power line inspection in complex environments.

[0022] In one embodiment, the angular deviation of the current position of the controlled object relative to the azimuth of the inspection target is analyzed based on spatial constraint data to obtain an azimuth deviation value. Simultaneously, a deviation analysis is performed between the current position of the controlled object and the tower position and spatial constraint scale in the spatial constraint data to obtain a target relative deviation value representing the distance deviation from the tower position, including: Divide the cross product vector of the first spatial vector and the second spatial vector by its own magnitude to obtain a unit vector with a magnitude of 1, wherein the cross product vector is perpendicular to both the first spatial vector and the second spatial vector. Using the unit vector as the reference direction for the inspection target's orientation, the difference between the three-dimensional coordinates of the current position of the controlled object and the three-dimensional coordinates of the tower position is calculated to obtain the third spatial vector. The cross product of the third spatial vector and the reference direction is then taken, and the modulus is divided by the modulus of the third spatial vector to obtain the orientation deviation value, which represents the degree of angular deviation of the third spatial vector relative to the reference direction. The magnitude of the third spatial vector is used as the real-time distance between the current position of the control object and the tower location. The difference between the real-time distance and the spatial constraint scale is divided by the spatial constraint scale to obtain the target relative deviation value representing the distance deviation from the tower location.

[0023] By analyzing the cross product vector of the first and second spatial vectors, the reference direction of the inspection target's azimuth is obtained. Then, the difference between the three-dimensional coordinates of the current position of the controlled object and the three-dimensional coordinates of the tower position is used to obtain the third spatial vector. The azimuth deviation value and the target relative deviation value are calculated respectively. Thus, the trajectory error of the UAV deviating from the preset waypoint is quantitatively decomposed into two orthogonal dimensions: angle deviation and tower position distance deviation. This decomposition method enables the flight controller to distinguish whether the UAV is azimuthally deflected or radially displaced, and to implement differentiated compensation control for the angle channel and distance channel respectively, avoiding oscillation or overshoot caused by a single callback strategy. When positioning drift or wind speed disturbance causes trajectory deviation, the flight controller can correct the heading and position in real time based on these two deviation values, ensuring that the UAV always flies safely within the spatial constraint envelope of the inspection target, thus improving the control of UAVs during power line inspection.

[0024] In one embodiment, the orientation deviation value and the target relative deviation value are analyzed to obtain a column vector. The column vector is then subjected to exponential decay calculation with the spatial inner product matrix to obtain a constraint correlation coefficient representing the degree of constraint deviation between the current position of the controlled object and the tower position, including: Divide each element of the spatial inner product matrix by the square of the spatial constraint scale to obtain the spatial constraint matrix, and then solve for the inverse matrix of the spatial constraint matrix. The azimuth deviation value and the target relative deviation value are used to form a two-dimensional deviation column vector. The transpose, inverse matrix and the deviation column vector are multiplied together to obtain the quadratic deviation value. Then, the negative of the quadratic deviation value is exponentially operated with the natural constant e as the base to obtain the exponential decay value. The difference between 1 and the exponential decay value is used as the constraint correlation coefficient for the degree of constraint deviation between the current position of the controlled object and the tower position; This calculation allows the constraint correlation coefficient to be limited to the 0-1 range. When the azimuth deviation and the target relative deviation are both 0, the deviation column vector is a 0 vector, the quadratic value of the deviation is 0, the exponential decay value is 1, and the constraint correlation coefficient is 0. When both types of deviations increase simultaneously, the quadratic value of the deviation increases, the exponential decay value approaches 0, and the constraint correlation coefficient monotonically increases and approaches 1 but is not equal to 1, forming a bounded saturation characteristic.

[0025] By dividing each element of the spatial inner product matrix by the square of the spatial constraint scale, the spatial constraint matrix is ​​obtained and its inverse is solved. Then, a two-dimensional deviation column vector is constructed using the azimuth deviation value and the target relative deviation value, and the constraint correlation coefficient is calculated. The constraint correlation coefficient integrates the deviation information of the two dimensions of angular deviation and distance deviation, and reflects the degree of constraint deviation between the current position of the controlled object and the tower position in real time, thereby improving the UAV's ability to autonomously correct its trajectory and its flight stability under positioning drift and wind speed disturbance.

[0026] In one embodiment, a safety threshold is determined based on the constraint correlation coefficient. Then, the constraint correlation coefficient is compared and analyzed with the safety threshold to obtain a trajectory correction weight representing the priority of trajectory correction under the current deviation state. This weight includes: A two-dimensional constraint column vector is constructed based on the constraint correlation coefficient. The constraint column vector contains two components, namely the first constraint component and the second constraint component. The first constraint component is the constraint correlation coefficient, and the second constraint component is the constraint correlation coefficient divided by (the difference between 1 and the constraint correlation coefficient). By multiplying the transpose of the constraint column vector, the inverse of the spatial constraint matrix, and the constraint column vector in sequence, we obtain the value of the constraint quadratic form. The quadratic values ​​of the constraints are analyzed to determine the safety threshold, and the trajectory correction weight is calculated based on the correlation coefficient between the safety threshold and the constraints.

[0027] In one embodiment, the constraint quadratic value is analyzed to determine a safety threshold, and the trajectory correction weight is calculated based on the safety threshold and the constraint correlation coefficient, including: Perform the square root operation on the constraint quadratic value to obtain the distance modulus, and then divide 1 by (the square of the sum of 1 and the distance modulus) to obtain the safety threshold. Among them, as the constraint correlation coefficient increases, the distance modulus increases monotonically, and the safety threshold decreases monotonically and tends to 0, thus realizing an adaptive dynamic threshold.

[0028] Calculate the sum of the constraint correlation coefficient and the safety threshold, and then divide the constraint correlation coefficient by the sum to obtain the trajectory correction weight, which represents the priority of trajectory correction required under the current deviation state. The larger the trajectory correction weight, the more serious the deviation of the current trajectory from the constraint, the higher the priority of trajectory correction and the stronger the correction force. The smaller the trajectory correction weight, the more slight the deviation, the lower the correction priority and the weaker the correction force.

[0029] By constructing a two-dimensional constraint column vector based on the constraint correlation coefficient, calculating the constraint quadratic form value, and analyzing the constraint quadratic form value, a dynamic safety threshold is obtained. The constraint correlation coefficient is divided by the sum of the constraint correlation coefficient and the safety threshold to obtain the trajectory correction weight. The calculated trajectory correction weight can automatically adapt to the differences in the geometric configuration of different tower locations, avoiding misjudgment or omission under varying working conditions with a fixed threshold. The flight controller dynamically allocates control based on the trajectory correction weight, maintaining stable flight when there is a slight deviation, and prioritizing strong correction when there is a serious deviation. This achieves a shift from passively returning to the preset waypoint to actively controlling and enhancing the flight stability of the UAV power line inspection in scenarios with superimposed wind speed disturbances and positioning drift.

[0030] In one embodiment, based on trajectory correction weights and spatial constraint scales, error components in the horizontal and vertical planes are calculated and analyzed to generate flight control commands for driving the controlled object to move from its current deviation position to the constrained path of the inspection target, including: The trajectory correction weight is multiplied by the spatial constraint scale to obtain the desired distance. Then, the unit vector of the third spatial vector is found, and the unit vector is calculated with the desired distance to obtain the desired position offset vector. Specifically, the process includes: dividing the third spatial vector by its magnitude to obtain the unit vector in the direction of the third spatial vector; taking the reference direction as the tangential reference direction; when the magnitude of the cross product of the third spatial vector and the unit vector represented by the reference direction is >0, taking the unit vector in the direction of the cross product as the azimuth correction direction; otherwise, setting the azimuth correction direction to the 0 vector; then multiplying the desired distance by the unit vector in the direction of the third spatial vector to obtain the first offset component; multiplying the desired distance by the azimuth deviation value and then by the azimuth correction direction to obtain the second offset component; and adding the first offset component and the second offset component to obtain the desired position offset vector. Based on the desired position offset vector and the three-dimensional coordinates of the tower, the position error vector is determined. Specifically, this includes: adding the desired position offset vector to the three-dimensional coordinates of the tower to obtain the desired position, and subtracting the current position obtained by adding the three-dimensional coordinates of the tower to the third spatial vector from the desired position to obtain the position error vector. The position error vector is decomposed into horizontal plane error components and vertical plane error components, where the horizontal plane error components are the projections of the position error vector in the horizontal plane, and the vertical plane error components are the projections of the position error vector in the vertical direction. Based on the analysis of the horizontal and vertical error components, yaw control commands, pitch control commands, and roll control commands are obtained. The combination of yaw control commands, pitch control commands, and roll control commands yields the flight control commands.

[0031] In one embodiment, based on the analysis of the horizontal and vertical plane error components, yaw control commands, pitch control commands, and roll control commands are obtained. The combination of these commands yields flight control commands, including: The direction projected from the three-dimensional coordinates of the tower position to the current position in the horizontal plane is used as the horizontal projection direction of the connecting line. When the horizontal projection direction of the connecting line is a 0 vector, a control command to not adjust the yaw direction and a control command to not adjust the roll direction are generated. Otherwise, the horizontal plane error component and the horizontal projection direction of the connecting line are used as three-dimensional vectors with a z-component of 0, and a cross product operation is performed to obtain the yaw cross product vector. Based on the vertical component of the yaw cross product vector, a yaw direction control command is generated: when the component is positive, the yaw direction control command is to yaw to the right; when the component is negative, the yaw direction control command is to yaw to the left; when the component is 0, the yaw direction control command is not adjusted. Pitch control commands are generated based on the vertical plane error components. When the vertical plane error components are positive, the pitch control command is to tilt up; when the vertical plane error components are negative, the pitch control command is to tilt down; and when the vertical plane error components are 0, the pitch control command is not adjusted. The horizontal plane error component is decomposed along the horizontal projection direction of the connecting line into a component along that direction and a component perpendicular to that direction. The component perpendicular to that direction is taken as the roll error component. The horizontal projection direction of the connecting line and the roll error component are respectively extended into three-dimensional vectors with a z-component of 0, and then cross-product operation is performed to obtain the roll cross-product vector. The roll direction control command is generated based on the vertical component of the roll cross-product vector: when the component is positive, the roll direction control command is to roll to the right; when the component is negative, the roll direction control command is to roll to the left; when the component is 0, the roll direction control command is not adjusted. The yaw, pitch, and roll control commands are combined into a flight control command to drive the controlled object from its current deviation position toward the constrained path of the inspection target. The flight control command is used to instruct the controlled object to adjust the direction of its three-axis attitude angles.

[0032] By calculating the trajectory correction weights and spatial constraint scales, the desired distance is obtained. The position error vector is then determined by combining the third spatial vector. This error vector is then independently decomposed into horizontal and vertical error components, generating yaw, pitch, and roll control commands respectively. These three control commands are then combined into a complete flight control command. This scheme transforms spatial constraint deviation into differentiated adjustment signals for the three-axis attitude angles, enabling the flight controller to make corrections based on the original inspection target constraints, rather than simply reverting to preset waypoints. This effectively avoids control coupling and interference between channels, ensuring that the UAV can still smoothly approach along the constraint path even under positioning drift and wind speed disturbances. At the same time, the yaw, pitch, and roll commands are independently decided based on the cross product direction, ensuring that the correction actions conform to spatial geometry and adapt to changes in deviation status in real time, thus guaranteeing the control effect of the UAV.

[0033] Although embodiments of the invention have been shown and described, it will be understood by those skilled in the art that various changes, modifications, substitutions and alterations can be made to these embodiments without departing from the principles and spirit of the invention, the scope of which is defined by the appended claims and their equivalents.

Claims

1. A path-constrained unmanned aerial vehicle (UAV) flight control system, characterized in that, include: The constraint analysis unit is used to acquire spatial constraint data of the inspection targets, analyze the spatial constraint data to obtain a first spatial vector and a second spatial vector, and construct and analyze a spatial inner product matrix based on the first spatial vector and the second spatial vector to obtain the spatial constraint scale representing the spatial constraints between each inspection target in the absence of interference. The deviation calculation unit is used to acquire the three-dimensional coordinates of the current position of the controlled object in real time. Based on the spatial constraint data, it analyzes the degree of angular deviation of the current position of the controlled object relative to the azimuth of the inspection target and obtains the azimuth deviation value. At the same time, it performs deviation analysis between the current position of the controlled object and the tower position and spatial constraint scale in the spatial constraint data to obtain the target relative deviation value representing the distance deviation of the tower position. The constraint association unit is used to analyze the positional deviation value and the target relative deviation value to obtain a column vector. The column vector is then subjected to exponential decay calculation with the spatial inner product matrix to obtain the constraint association coefficient, which represents the degree of constraint deviation between the current position of the controlled object and the tower position. The trajectory correction unit is used to determine the safety threshold based on the constraint correlation coefficient, and then compare and analyze the constraint correlation coefficient with the safety threshold to obtain the trajectory correction weight, which represents the priority of trajectory correction under the current deviation state. The flight control unit is used to calculate and analyze the error components in the horizontal and vertical planes based on the trajectory correction weights and spatial constraint scales, and generate flight control commands to drive the controlled object to move from its current deviation position to the constrained path of the inspection target.

2. The path-constrained unmanned aerial vehicle (UAV) flight control system according to claim 1, characterized in that, Spatial constraint data, including: The three-dimensional coordinates of the tower location, insulator string, and ground wire during the inspection.

3. The path-constrained unmanned aerial vehicle (UAV) flight control system according to claim 1, characterized in that, The spatial constraint data is analyzed to obtain a first spatial vector and a second spatial vector. A spatial inner product matrix is ​​constructed based on the first and second spatial vectors and analyzed to obtain the spatial constraint scale representing the spatial constraints between each inspection target under interference-free conditions, including: Using the three-dimensional coordinates of the tower location in the spatial constraint data as the spatial reference point, the first and second spatial vectors of the three-dimensional coordinates of the insulator string and the three-dimensional coordinates of the ground wire relative to the three-dimensional coordinates of the tower location are calculated respectively. Calculate the magnitude of the first spatial vector and the magnitude of the second spatial vector respectively, and calculate the inner product of the first spatial vector and the second spatial vector to obtain the inner product value. Construct the inner product matrix based on the first spatial vector, the second spatial vector and the inner product value. By analyzing the spatial inner product matrix, the envelope radius and spatial envelope value of the spatial constraints are determined, and the spatial constraint scale is obtained.

4. The path-constrained unmanned aerial vehicle (UAV) flight control system according to claim 3, characterized in that, Analyzing the spatial inner product matrix determines the envelope radius and spatial envelope value of the spatial constraints, yielding the spatial constraint scale, including: Calculate the determinant of the spatial inner product matrix to obtain the spatial constraint envelope radius, calculate the cross product vector of the first spatial vector and the second spatial vector, and use the magnitude of the cross product vector as the spatial envelope value. Divide the spatial envelope value by the sum of the first spatial vector magnitude and the second spatial vector magnitude to obtain the spatial constraint height value. Calculate the first spatial vector magnitude, the second spatial vector magnitude, the spatial constraint envelope radius, and the spatial constraint height value to obtain the spatial constraint scale representing the space constraints between each inspection target in an interference-free state.

5. A path-constrained unmanned aerial vehicle (UAV) flight control system according to claim 4, characterized in that, Based on spatial constraint data analysis, the angular deviation of the current position of the controlled object relative to the azimuth of the inspection target is analyzed to obtain the azimuth deviation value. Simultaneously, deviation analysis is performed between the current position of the controlled object and the tower position and spatial constraint scale in the spatial constraint data to obtain the target relative deviation value representing the distance deviation from the tower position, including: Divide the cross product vector of the first spatial vector and the second spatial vector by its own magnitude to obtain a unit vector, wherein the cross product vector is perpendicular to both the first spatial vector and the second spatial vector. Using the unit vector as the reference direction for the inspection target's orientation, the difference between the three-dimensional coordinates of the current position of the controlled object and the three-dimensional coordinates of the tower position is calculated to obtain the third spatial vector. The third spatial vector is then analyzed with the reference direction to obtain the orientation deviation value, which represents the degree of angular deviation of the third spatial vector relative to the reference direction. The magnitude of the third spatial vector is used as the real-time distance between the current position of the control object and the tower location. The real-time distance and the spatial constraint scale are calculated to obtain the target relative deviation value representing the deviation of the tower location distance.

6. A path-constrained unmanned aerial vehicle (UAV) flight control system according to claim 5, characterized in that, Analyzing the positional deviation and the relative deviation from the target, a column vector is obtained. The column vector is then subjected to exponential decay calculation with the spatial inner product matrix to obtain the constraint correlation coefficients, representing the degree of constraint deviation between the current position of the controlled object and the tower position. These coefficients include: Divide each element of the spatial inner product matrix by the square of the spatial constraint scale to obtain the spatial constraint matrix, and then solve for the inverse matrix of the spatial constraint matrix. The azimuth deviation value and the target relative deviation value are used to construct a two-dimensional deviation column vector. The exponential decay value is obtained by calculating the deviation column vector and the inverse matrix. The difference between 1 and the exponential decay value is used as the constraint correlation coefficient for the degree of constraint deviation between the current position of the controlled object and the tower position.

7. A path-constrained unmanned aerial vehicle (UAV) flight control system according to claim 6, characterized in that, The safety threshold is determined based on the constraint correlation coefficient. Then, the constraint correlation coefficient and the safety threshold are compared and analyzed to obtain the trajectory correction weight, which represents the priority of trajectory correction under the current deviation state. This weight includes: A two-dimensional constraint column vector is constructed based on the constraint correlation coefficient. The constraint column vector contains two components, namely the first constraint component and the second constraint component. The constraint quadratic form value is obtained by calculating the transpose of the constraint column vector, the inverse matrix of the spatial constraint matrix, and the constraint column vector. The quadratic values ​​of the constraints are analyzed to determine the safety threshold, and the trajectory correction weight is calculated based on the correlation coefficient between the safety threshold and the constraints.

8. A path-constrained unmanned aerial vehicle (UAV) flight control system according to claim 7, characterized in that, The quadratic form values ​​of the constraints are analyzed to determine the safety threshold, and the trajectory correction weights are calculated based on the correlation coefficient between the safety threshold and the constraints, including: The square root of the constraint quadratic value is taken to obtain the distance modulus. The distance modulus is then calculated to obtain the safety threshold. The constraint correlation coefficient is compared with the safety threshold to obtain the trajectory correction weight, which represents the priority of trajectory correction under the current deviation state.

9. A path-constrained unmanned aerial vehicle (UAV) flight control system according to claim 8, characterized in that, Based on trajectory correction weights and spatial constraint scales, calculations are performed to analyze the error components in the horizontal and vertical planes, generating flight control commands to drive the controlled object from its current deviation position to the constrained path of the inspected target. These commands include: The trajectory correction weight is multiplied by the spatial constraint scale to obtain the expected distance. Then, the unit vector of the third spatial vector is found, and the unit vector is calculated with the expected distance to obtain the expected position offset vector. Based on the expected position offset vector and the three-dimensional coordinates of the tower, the position error vector is determined. The position error vector is decomposed into horizontal plane error components and vertical plane error components, where the horizontal plane error components are the projections of the position error vector in the horizontal plane, and the vertical plane error components are the projections of the position error vector in the vertical direction. Based on the analysis of the horizontal and vertical error components, yaw control commands, pitch control commands, and roll control commands are obtained. The combination of yaw control commands, pitch control commands, and roll control commands yields the flight control commands.

10. A path-constrained unmanned aerial vehicle (UAV) flight control system according to claim 9, characterized in that, Based on the analysis of horizontal and vertical error components, yaw, pitch, and roll control commands are obtained. The combination of these commands yields the flight control commands, including: The direction of the direction from the three-dimensional coordinates of the tower to the current position is projected in the horizontal plane as the horizontal projection direction of the connecting line. When the horizontal projection direction of the connecting line is a 0 vector, a control command to not adjust the yaw direction and a control command to not adjust the roll direction are generated. Otherwise, the horizontal plane error component and the horizontal projection direction of the connecting line are respectively used as three-dimensional vectors with a z component of 0 to perform cross product operation to obtain the yaw cross product vector. Based on the vertical component of the yaw cross product vector, the yaw direction control command is generated. The pitch control command is generated based on the vertical plane error component. The horizontal plane error component is decomposed into a component along the horizontal projection direction of the connecting line and a component perpendicular to the connecting line. The component perpendicular to the connecting line is taken as the roll error component. The horizontal projection direction of the connecting line and the roll error component are respectively extended into three-dimensional vectors with a z-component of 0, and then cross product operation is performed to obtain the roll cross product vector. The roll control command is generated based on the vertical component of the roll cross product vector. The yaw control command, pitch control command, and roll control command are combined into a flight control command used to drive the controlled object to move from its current deviation position to the constrained path of the inspected target.

Images