A tethered unmanned aerial vehicle dynamic following control system and method based on cable force tactile feedback

Abstract

The application discloses a tethered unmanned aerial vehicle dynamic following control system and method based on cable force sense feedback and belongs to the field of tethered unmanned aerial vehicle control technology. The system comprises a force sense sensing unit, a vehicle-mounted motion state sensing unit and a cooperative controller. The force sense sensing unit is used for sensing multi-dimensional force information of a tethered cable in real time. The vehicle-mounted motion state sensing unit is used for acquiring the motion state of a vehicle-mounted platform. The cooperative controller is configured to solve a disturbance vector of the cable to the unmanned aerial vehicle based on the multi-dimensional force information, fuse the disturbance vector with the motion state, generate a target tracking instruction containing both position expectation and compliance expectation, and send the target tracking instruction to a flight control device for execution, so that the unmanned aerial vehicle actively conforms to the cable force disturbance while maintaining the relative position, and the cable tension is dynamically maintained within a safe range. The cooperative controller further comprises a signal conditioning module and a hardware safety comparison module, which realize high-precision real-time acquisition of the multi-dimensional force information and rapid response to whether the multi-dimensional force is out of limit. The application is suitable for vehicle-mounted mobile scenes such as highway inspection and can effectively prevent the cable from being excessively pulled and broken or from falling due to sagging.

Description

Technical Field

[0001] This invention relates to the field of tethered unmanned aerial vehicle (UAV) control technology, specifically to a dynamic following control system and method for tethered UAVs used in vehicle-mounted mobile scenarios such as highway inspection. Background Technology

[0002] Tethered drones obtain continuous power from a ground platform via cables, enabling them to hover for hours or even days, and have broad application prospects in highway inspection, emergency communication, and security monitoring. In highway inspection applications, tethered drones are typically mounted on inspection vehicles and need to achieve stable dynamic following flight while the vehicle is in motion.

[0003] However, existing tethered drone tracking control schemes primarily rely on position sensors such as GPS and vision, implementing feedback control by measuring the relative positional deviation between the drone and the ground platform. These schemes suffer from the following prominent problems:

[0004] 1. Software response lag leads to high risk of cable overload. In existing tethered drone follow-up control devices, cable tension monitoring and safety protection rely entirely on the main control module's software processing. From sensor acquisition, AD conversion, software judgment to actuator action, the entire link has inherent delays. When the vehicle platform suddenly accelerates or encounters sudden disturbances such as strong crosswinds, the software response has not yet been completed, and the cable tension has instantly exceeded its load-bearing limit, which can easily cause cable breakage or drone loss of control and crash. Existing solutions lack a fast hardware processing link independent of software processing.

[0005] 2. Single-axis force monitoring has a protection blind spot. Existing devices typically only collect the tensile force component of the tethered cable in one axis (such as the vertical direction) for threshold judgment, without performing vector sum calculations on the three-axis force information. However, the cable tension experienced by tethered drones in actual operation is a three-dimensional spatial vector, and lateral pulling or oblique dragging conditions are extremely common. Relying solely on single-axis component judgment may result in situations where the tensile force in one direction is severely exceeded, but the monitored axis still shows normal values, creating a significant safety protection blind spot.

[0006] 3. A trade-off between position holding and cable flexibility. Existing devices employ a rigid position holding control strategy, strictly requiring the UAV to maintain a fixed relative position with the vehicle platform. Under this control method, when the vehicle platform's motion changes abruptly, the UAV will perform a violent correction maneuver to maintain its preset position, causing the cable to bear enormous impact loads. Relaxing the position constraint to protect the cable, however, would prevent the UAV from effectively following the vehicle's movement. Existing devices lack a control scheme that can achieve a dynamic balance between position holding and cable flexibility.

[0007] Therefore, there is an urgent need for a hardware architecture and control method that can actively and quickly sense the three-dimensional force of cables to solve the problems existing in the above-mentioned technologies. Summary of the Invention

[0008] To address the aforementioned shortcomings of existing technologies, this invention provides a dynamic following control system and method for tethered drones based on cable force feedback, thereby solving the problems of response lag, cable force loss control, and inability to simultaneously maintain position and achieve compliance in existing position deviation-based following control schemes.

[0009] To achieve the above objectives, the present invention adopts the following technical solution:

[0010] A dynamic following control system for tethered unmanned aerial vehicles (UAVs) based on cable force feedback, comprising:

[0011] The force sensing unit employs a three-dimensional force sensor, which is mounted on the cable guide wheel of the take-up and release device; it is used to collect analog signals of three-dimensional force.

[0012] The vehicle-mounted motion state perception module uses a GNSS receiver and an inertial measurement unit to acquire latitude, longitude, speed, heading data, acceleration, and angular velocity data.

[0013] The collaborative controller includes a force feedback control circuit and a main control chip connected thereto. The force feedback control circuit is connected to a force sensing unit to collect digital force signals from the cable, implements threshold comparison through hardware circuitry to output an interrupt signal, and uploads the digital force signals and the interrupt signal to the main control chip. The main control chip uses the digital force signals as input to calculate a compliance correction vector based on preset virtual mass, virtual damping, and virtual stiffness parameters, and superimposes it onto the initial position expectation to generate the target tracking command Xcmd. The tethered UAV then follows the highway inspection vehicle according to the target position command Xcmd.

[0014] The ground power supply and communication module is used to provide power to the tethered cable retraction device, the collaborative controller, and the vehicle motion status sensing module, as well as to enable the collaborative controller to communicate with the outside world.

[0015] The force feedback control circuit includes a force sensing unit signal conditioning circuit and a safety threshold hardware comparison circuit connected in sequence.

[0016] The force sensing unit signal conditioning circuit includes a reference voltage circuit, a three-way Wheatstone bridge and amplifier circuit, and a multi-channel analog-to-digital acquisition circuit to acquire three-dimensional force signals.

[0017] The reference voltage circuit includes a precision reference source chip U1 and its peripheral capacitors C2, C3, and C4, which outputs a +5V_REF precision voltage to provide a reference power supply for the subsequent Wheatstone bridge and analog-to-digital converter.

[0018] The three-way Wheatstone bridge and amplifier circuit includes three identical signal conditioning branches that acquire analog force signals V_Fx, V_Fy, and V_Fz from the UAV cable in the X, Y, and Z dimensions. Each connection includes: a strain gauge RG1 from the three-dimensional force sensor and voltage divider resistors R1-R3 forming a full-bridge structure to convert the force signal into a differential voltage signal. This differential voltage signal is then connected to the input of operational amplifier U2 via current-limiting resistors R4 and R5. Operational amplifier U2 is powered by a single +5V_REF supply and an external gain resistor R6 amplifies the signal output from the bridge. The output of the operational amplifier is connected to an RC filter network to output the analog force signal.

[0019] The multi-channel analog-to-digital acquisition circuit uses an analog-to-digital converter U3; the analog-to-digital converter U3 is connected to a +5V_REF reference voltage and a crystal oscillator U6; the three analog force signals V_Fx, V_Fy, and V_Fz are connected to the input pins AIN0, AIN2, and AIN4 of the analog-to-digital converter U3 via current-limiting resistors R20, R21, and R26, respectively; the output pins SCLK, DIN, DOUT, and CS are connected to the SPI interface to output the converted three digital signals V_x, V_y, and V_z to the main control chip.

[0020] The security threshold hardware comparison circuit includes a square operation circuit, an inverse summation circuit, a square root operation circuit, and a threshold comparison circuit connected in sequence.

[0021] The squaring circuit uses a three-way multiplier. The two input terminals of each multiplier are connected to the same analog force signal. The three multipliers perform squaring operations on the three analog force signals V_Fx, V_Fy, and V_Fz respectively, and the output terminal of each multiplier is connected to the summing node through resistors R1, R2, and R3 respectively.

[0022] The inverting summation circuit uses an operational amplifier U4 as an inverting adder to receive three squared signals and perform summation operations, outputting the inverted value of the square of the three-dimensional force vector.

[0023] The square root operation circuit is a closed-loop feedback square root circuit composed of operational amplifier U5 and multiplier U6. The output signal of operational amplifier U4 is buffered by operational amplifier U5, multiplied by itself by multiplier U6, and then fed back to the inverting input terminal of operational amplifier U5 to form a closed loop, and finally outputs the modulus signal of the three-dimensional force vector sum.

[0024] The threshold comparison circuit uses voltage comparator U7.1 and voltage comparator U7.2.

[0025] The modulus signal of the sum of the three-dimensional force vectors is connected to the non-inverting inputs of two comparators, and the inverting inputs of the two comparators are connected to the preset safety thresholds Vref1 and Vref2 respectively; the outputs of the comparators are connected to the +5V power supply through pull-up resistors R7 and R8, and output signals Vcom1 and Vcom2 to the main control chip.

[0026] When the magnitude of the sum of the three-dimensional force vectors exceeds any threshold, the corresponding comparator flips and outputs a hardware trigger signal, achieving microsecond-level response for safety protection.

[0027] The main control chip includes the following program modules:

[0028] Force information processing module: Receives three-dimensional force digital signals V_x, V_y, and V_z from the force feedback control circuit, and calculates the current disturbance vector generated by the tethered cable on the UAV after signal conditioning, that is, the three-dimensional tension vector Fcable of the cable;

[0029] The motion state fusion module receives data from the vehicle motion state perception unit, performs spatiotemporal alignment and fusion, and outputs the initial expected relative position Xdes of the vehicle platform. Xdes includes estimates of pose, velocity and acceleration.

[0030] The admittance control module takes the cable tension vector Fcable as input and outputs a compliance correction position vector through a predefined virtual mass-spring-damper model. X;

[0031] The instruction synthesis module converts the compliance correction vector output by the admittance control module into a variable. X is superimposed on the initial expected relative position Xdes calculated by the motion state fusion module to generate the final UAV target position command Xcmd=Xdes+ΔX; it is also used to adjust the command strategy based on the interrupt signals Vcom1 and Vcom2 output by the safety threshold hardware comparison circuit.

[0032] The safety protection module sets a first safety threshold Vref1 and a second safety threshold Vref2 and sends them to the safety threshold hardware comparison circuit; the first safety threshold corresponds to 75% of the rated tensile force of the cable, and the second safety threshold corresponds to the lower limit of the tensile force of the cable under its own weight sag safety limit;

[0033] The interrupt signals Vcom1 and Vcom2 output by the hardware comparison circuit for acquiring the safety threshold are collected.

[0034] The instruction adjustment strategy based on the interrupt signals Vcom1 and Vcom2 output by the security threshold hardware comparison circuit includes:

[0035] When the first safety threshold is exceeded, the instruction synthesis module is triggered to scale the weights of Xdes, thus adjusting the compliance correction vector. X takes precedence, and the drone prioritizes moving in the direction that reduces pull;

[0036] When the speed drops below the second safety threshold, a speed feedforward component is injected into the command synthesis module, causing the drone to actively accelerate to "catch up" with the vehicle platform, thereby tightening the slack cable.

[0037] A dynamic following control method for tethered drones based on cable force feedback includes the following steps:

[0038] Three-dimensional force digital signals are acquired through a force feedback control circuit; the three-dimensional tension vector Fcable of the tethered cable on the UAV is calculated after signal conditioning.

[0039] The vehicle-mounted motion state perception unit collects data, performs spatiotemporal alignment and fusion, and outputs the initial expected relative position Xdes of the vehicle platform, which includes estimates of pose, velocity and acceleration.

[0040] Using the cable tension vector Fcable as input, and passing through a predefined virtual mass-spring-damper system, the output is a compliance correction position vector. X;

[0041] Compliance Modification Vector X is superimposed with the initial expected relative position Xdes to generate the final UAV target position command Xcmd=Xdes+ΔX; and the command adjustment strategy is performed according to the interrupt signals Vcom1 and Vcom2 output by the safety threshold hardware comparison circuit.

[0042] The instruction adjustment strategy based on the interrupt signals Vcom1 and Vcom2 output by the security threshold hardware comparison circuit includes:

[0043] Set a first safety threshold Vref1 and a second safety threshold Vref2; the first safety threshold corresponds to 60%~85% of the rated tensile force of the cable, and the second safety threshold corresponds to the lower limit of the tensile force of the cable under its own weight sag safety limit.

[0044] The interrupt signals Vcom1 and Vcom2 output by the hardware comparison circuit for acquiring the safety threshold are collected.

[0045] When the first safety threshold is exceeded, the weights of Xdes are scaled to make the compliance correction vector... X takes precedence, and the drone prioritizes moving in the direction that reduces pull;

[0046] When the speed drops below the second safety threshold, a velocity feedforward component is injected to cause the tethered drone to actively accelerate in order to "catch up" with the vehicle platform, thereby tightening the slack cable.

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

[0048] 1. Hardware-level rapid response. This invention, by setting up a hardware safety comparison circuit, directly performs threshold comparison and logic interlocking of the cable force information in the analog domain. Without the need for software processing, it can trigger an emergency stop signal within microseconds, fundamentally eliminating the risk of cable overload caused by software response lag.

[0049] 2. Precise vector force sensing. The signal conditioning module synchronously conditions and acquires the three-axis signals from the three-dimensional force sensor, and obtains the actual tensile force vector modulus through vector summation circuitry. This ensures that the threshold comparison is based on the actual cable stress state, rather than a single axial component, thus avoiding protection blind spots.

[0050] 3. Achieving a dynamic balance between position holding and compliance. By using a compliance correction vector calculated online through an admittance control model, this invention achieves the basic goal of maintaining the relative position of the UAV while simultaneously exhibiting a moderate "compliance" with cable tension disturbances, thus keeping the cable tension within a safe range, preventing both overstretching and breakage, and sag and fall. Attached Figure Description

[0051] Figure 1 This is a schematic diagram of the overall architecture of the system of the present invention.

[0052] Figure 2 This is an internal block diagram of the collaborative controller in this invention.

[0053] Figure 3 This is a schematic diagram of the signal conditioning circuit for the force sensing unit.

[0054] Figure 4 This is a schematic diagram of a hardware comparison circuit for a safety threshold.

[0055] Figure 5(a) is a simulation comparison of the cable tension response using the method of the present invention and the conventional position feedback method.

[0056] Figure 5(b) is a comparison of the position error when applying the method of the present invention and the conventional position feedback method.

[0057] Figure 5(c) is a comparison of the speed of a UAV using the method of the present invention and the conventional position feedback method.

[0058] In the diagram, 10 is the tethered drone, 20 is the vehicle-mounted platform, 30 is the tether cable, 21 is the tether cable retraction device, 22 is the ground power supply and communication unit, 23 is the collaborative controller, 231 is the force feedback control circuit, 232 is the main control chip, 24 is the vehicle-mounted motion state sensing unit, and 25 is the force sensor unit. Detailed Implementation

[0059] To make the above-mentioned objects, features, and advantages of the present invention more apparent and understandable, the specific embodiments of the present invention will be described in detail below with reference to the accompanying drawings. Many specific details are set forth in the following description to provide a thorough understanding of the present invention. However, the present invention can be implemented in many other ways different from those described herein, and those skilled in the art can make similar modifications without departing from the spirit of the invention. Therefore, the present invention is not limited to the specific embodiments disclosed below.

[0060] Example 1: Application Scenarios

[0061] Figure 1 This illustrates an application scenario for this embodiment: dynamic following control of a tethered drone based on cable force feedback. It mainly includes:

[0062] The tethered drone 10 is a multi-rotor drone equipped with an onboard power module, flight control system, communication module, and mission payloads (such as a high-definition camera and an infrared thermal imager). The onboard power module provides +12V power, the flight control system is the tethered drone 10's own control system, and the communication module uses wireless communication and the MAVLink V2.0 communication protocol.

[0063] The vehicle-mounted platform 20 is a highway patrol vehicle, equipped with a tethered cable retraction device 21, a ground power supply and communication unit 22, a coordination controller 23, and an onboard motion state sensing unit 24. A three-dimensional force sensor is integrated on the cable guide wheel of the retraction device 21 as a force sensing unit 25. The tethered cable retraction device 21 uses a torque-controlled winch driven directly by a servo motor. The drive motor of this winch is a DC servo motor, controlled by the coordination controller.

[0064] The tether cable 30 connects the tether cable retraction device 21 of the vehicle platform 20 to the tethered drone 10. It integrates power supply wires and fiber optic communication cables for transmitting electrical energy and data.

[0065] The vehicle-mounted motion state sensing unit 24 includes at least one of a GNSS receiver (acquiring latitude, longitude, speed, and heading), an inertial measurement unit (acquiring acceleration and angular velocity), and a vehicle-mounted CAN bus data interface.

[0066] Example 2: Working principle of the cooperative controller

[0067] Figure 2A functional block diagram of the collaborative controller 23 is shown. The collaborative controller 23 operates within the vehicle controller of the vehicle platform 20. The collaborative controller 23 includes a force feedback control circuit 231 and a main control chip 232 connected thereto. The force feedback control circuit 231 is connected to the force sensing unit 25 to collect the three-dimensional tension signal of the cable, implements threshold comparison through hardware circuitry to output an interrupt signal, and uploads the three-dimensional tension signal and the interrupt signal of the cable to the main control chip 232.

[0068] Example 3: Signal Conditioning Circuit for Force Sensing Unit

[0069] The signal conditioning module includes: a Wheatstone bridge, composed of strain gauges built into the three-dimensional force sensor, used to convert the tension on each axis into differential voltage; an operational amplifier, connected to the output of the Wheatstone bridge, used to amplify the differential voltage; and a multi-channel analog-to-digital acquisition unit, connected to the operational amplifier, used to convert the filtered voltage into the digital force signal.

[0070] Figure 3 The specific connection relationship of the force sensing unit signal conditioning circuit of the present invention is shown. The circuit mainly consists of three parts: a reference voltage circuit, a three-way Wheatstone bridge and amplifier circuit, and a multi-channel analog-to-digital acquisition circuit, to realize high-precision acquisition of three-dimensional force signals.

[0071] Reference voltage circuit: Composed of precision reference source chip U1 (REF5050) and its peripheral components C2, C3, and C4. This circuit stably outputs a +5V_REF precision voltage, providing a reference power supply for the subsequent Wheatstone bridge and analog-to-digital converter, eliminating the influence of power supply fluctuations on force signal measurement.

[0072] The specific circuit connections are as follows: the +12V power supply is connected to the power supply pin VIN of the precision reference source chip U1, capacitors C2 and C3 are connected in parallel between pin VIN and pin GND, pin GND is grounded, pin TRIM / NR is grounded through capacitor C4, and pin VOUT outputs a +5V_REF precision voltage.

[0073] Three-way Wheatstone bridge and amplifier circuit: The circuit contains three signal conditioning channels with the same structure, corresponding to force signal acquisition in the X, Y and Z dimensions respectively.

[0074] Each channel consists of a strain gauge (RG1 / RG2 / RG3, nominally 120Ω) and voltage divider resistors (R1-R3, R7-R9, R13-R15) forming a full-bridge structure to convert the force signal into a differential voltage signal. The differential signal is connected to the input of operational amplifiers U2 / U4 / U5 (AD623) via current-limiting resistors (R4 / R5, R10 / R11, R16 / R17). The AD623 is powered by a single +5V_REF and amplifies the mV-level signal output by the bridge by 100 times through external gain resistors (R6 / R12 / R18, 1.01kΩ). An RC filter network (C5 / C6 / C22, C8 / C9 / C23, C11 / C12 / C24) is connected to the output of the operational amplifier to filter out high-frequency noise, resulting in three conditioned force signals V_Fx, V_Fy, and V_Fz.

[0075] Specifically, the connection relationships between each Wheatstone bridge and the amplifier circuit are as follows: +5V_REF is grounded to GND through capacitor C1; resistor R1 is connected in series with strain gauge RG1 and then in parallel with capacitor C1; resistors R2 and R3 are connected in series and then in parallel with capacitor C1; the lead between resistor R1 and strain gauge RG1 is connected to pin 2 of the inverting input of operational amplifier U2 through current-limiting resistor R4; the lead between resistors R2 and R3 is connected to pin 3 of the non-inverting input of operational amplifier U2 through current-limiting resistor R5; an external gain resistor R6 is connected between pins 1 and 8 of operational amplifier U2; pin 7 is connected to the +5V_REF precision voltage and is also grounded through capacitor C5; pin 5 is grounded to GND; pin 6 of the output terminal is also connected to GND through capacitors C6 and C22 in parallel; pin 6 of the output terminal outputs the conditioned force signal V_Fx.

[0076] Multi-channel analog-to-digital (ADC) acquisition circuit: Centered on a 24-bit high-precision ADC U3 (ADS1256). Three analog force signals, V_Fx, V_Fy, and V_Fz, are connected to channels AIN0, AIN2, and AIN4 respectively via current-limiting resistors (R20 / R21 / R26). The circuit uses +5V_REF provided by REF5050 as the reference voltage (VREFP) and is equipped with a crystal oscillator U6 (7.68MHz) to provide the clock for the ADC. The ADS1256 outputs the converted digital signal to the co-controller via the SPI interface (SCLK, DIN, DOUT, CS), achieving high-precision acquisition of the three-dimensional force signal.

[0077] The specific circuit connections are as follows: the three conditioned force signals V_Fx, V_Fy, and V_Fz are connected to the AIN0, AIN2, and AIN4 pins of the analog-to-digital converter U3 via current-limiting resistors R20, R21, and R26, respectively; the AIN1, AIN3, and AIN5 pins are grounded (GND); the AINCOM pin is grounded; the reference voltage +5V_REF is connected to the VREFP pin via resistor R24; the VREFN pin is grounded via resistor R25; capacitors C13, C14, and C15 are connected in parallel between the VREFP and VREFN pins; and the AGND and DGND pins are both grounded. Pin AVDD is connected to a +5V power supply. Pin AVDD is connected to pin AGND after being connected in parallel with capacitors C18 and C19. Pins RESET, SYNCPDWIN, and DVDD are all connected to +3.3V. Pin DVDD is also connected to ground GND after being connected in parallel with capacitors C21 and C20. Pin XTAL1CLKIN and pin XTAL2 are connected to crystal oscillator U6. Pins XTAL1CLKIN and XTAL2 are also connected to ground GND after being connected in parallel with capacitors C16 and C17 respectively. Pins SCLK, DIN, DOUT, and CS are connected to the SPI interface to output the converted digital signal.

[0078] Example 4: Hardware Comparison Circuit for Safety Threshold

[0079] The hardware safety comparison module includes: a vector sum calculation circuit for receiving analog voltage signals from the three axes of the three-dimensional force sensor and outputting a composite voltage representing the tensile force vector modulus; a first comparator, whose non-inverting input is connected to a first reference voltage, whose inverting input is connected to the composite voltage, and whose output output is the first interrupt signal; and a second comparator, whose non-inverting input is connected to the composite voltage, whose inverting input is connected to a second reference voltage, and whose output output is the second interrupt signal; the first reference voltage corresponds to the first safety threshold, and the second reference voltage corresponds to the second safety threshold. Further, the outputs of the first and second comparators are open-collector outputs, and their wired-AND connection forms an emergency stop signal, which is directly connected to the enable terminal of the motor driver of the tethered cable retraction device. Preferably, the first safety threshold is set to 65%~80% of the rated tensile force of the tethered cable, and the second safety threshold is set to the lower limit of the tensile force required for the tethered cable to achieve a safe sag under its own weight. Preferably, the vector sum calculation circuit includes: three analog multipliers forming a square operation circuit for squaring the analog voltage signals of the three axes respectively; an inverting adder for summing the three squared signals; and a square root circuit for performing a square root operation on the summation result.

[0080] Figure 4The specific circuit structure of the hardware comparison circuit for the safety threshold of the present invention is shown. This circuit is a purely analog circuit, which can realize the calculation of the three-dimensional force vector and modulus and its rapid comparison with the safety threshold without the intervention of the main control unit. The circuit mainly includes four parts: a square operation circuit, an inverting summation circuit, a square root operation circuit, and a threshold comparison circuit.

[0081] The squaring circuit uses multiplier chips U1, U2, and U3 (AD633JR) to perform squaring operations on the three force signals V_Fx, V_Fy, and V_Fz, respectively. Each multiplier has two inputs connected to the same force signal to achieve the squaring function, and its output is connected to the summing node via resistors (R1 / R2 / R3, 10kΩ).

[0082] Specifically, taking one of the paths as an example, the force signal V_Fx is connected to pins Y1 and X1 of the multiplier chip U1. Pins Y2, Z, and X2 are grounded to GND. Pin -VS is connected to the negative terminal of the 15V power supply. Pin Z is also connected to the negative terminal of the 15V power supply through capacitor C1. Pin +VS is connected to the positive terminal of the 15V power supply. Pin +VS is also connected to GND through capacitor C2. Pin W is connected to the output terminal of the summing node after passing through resistor R1.

[0083] Inverting summation circuit: The inverting adder is composed of operational amplifier U4 (TL081) and resistors R1, R2 and R3. It receives three square signals and performs summation operation, and outputs the inverted value of the square of the three-dimensional force vector.

[0084] The IN- pin of operational amplifier U4 is connected as the input terminal to the summation node output terminal of the three-way square operation circuit. The VCC- pin is connected to the negative terminal of the 15V power supply. The IN+ pin is grounded through resistor R5. The VCC+ pin is connected to the positive terminal of the 15V power supply. The OUT pin is used as the output terminal to output the inverted value of the square of the three-dimensional force vector.

[0085] Square root operation circuit: Centered on operational amplifier U5 (AD711) and multiplier U6 (AD633JN), this circuit forms a closed-loop feedback square root operation. The output signal of TL081 is buffered by AD711, multiplied by itself by multiplier U6, and then fed back to the inverting input of AD711, forming a closed loop. The final output is the magnitude of the three-dimensional force vector sum. Diode D1 (1N4148) is used to prevent negative signals from affecting the circuit.

[0086] The inverting input (pin 2) of operational amplifier U5 is connected to an inverting summator circuit to acquire the inverted value of the square of the three-dimensional force vector sum. The non-inverting input (pin 3) of operational amplifier U5 is grounded to GND. Pin 4 of operational amplifier U5 is connected to the negative terminal of the 15V power supply and is also grounded to GND through capacitor C7. Pin 7 of operational amplifier U5 is connected to the positive terminal of the 15V power supply and is also grounded to GND through capacitor C8. Pin 6 of operational amplifier U5 serves as the output terminal and is connected to pins 1 and 3 of multiplier U6 through capacitor C9 and diode D1, respectively. Pins 2 and 4 of multiplier U6 are grounded to GND. Pin 8 of multiplier U6 is connected to the positive terminal of the 15V power supply and is also grounded to GND through capacitor C11. Pin 5 of multiplier U6 is connected to the negative terminal of the 15V power supply and is also grounded to GND through capacitor C10. Pin 7 of multiplier U6 is also fed back to the inverting input of operational amplifier U5 through resistor R6. Finally, the magnitude of the three-dimensional force vector sum is output to the threshold comparison circuit.

[0087] Threshold comparison circuit: Implemented by voltage comparators U7.1 and U7.2 (LM193). The magnitude of the three-dimensional force vector sum is connected to the non-inverting input of the comparator, and the inverting input is connected to the preset safety thresholds Vref1 and Vref2, respectively. The comparator output is connected to a +5V power supply through pull-up resistors (R7 / R8, 10kΩ), outputting Vcom1 and Vcom2 signals to the safety protection module of the co-controller. When the magnitude of the three-dimensional force vector sum exceeds either threshold (first safety threshold, second safety threshold), the corresponding comparator flips, outputting a hardware trigger signal to achieve microsecond-level response safety protection.

[0088] The non-inverting inputs of voltage comparators U7.1 and U7.2 acquire the modulus signal of the sum of three-dimensional force vectors. The inverting inputs of voltage comparators U7.1 and U7.2 are connected to preset safety thresholds Vref1 and Vref2. The outputs of voltage comparators U7.1 and U7.2 are also connected to a +5V power supply through pull-up resistors R7 and R8, respectively. The outputs of voltage comparators U7.1 and U7.2 output signals Vcom1 and Vcom2.

[0089] Example 5: Main Control Chip

[0090] The main control chip 232 mainly includes a force information processing module, a motion state fusion module, an admittance control module, an instruction synthesis module, and a safety protection module;

[0091] Force information processing module: Receives three-dimensional force digital signals V_x, V_y, and V_z from force feedback control circuit 231, and calculates the current disturbance vector generated by the tethered cable on the UAV after signal conditioning, i.e., the three-dimensional tension vector of the cable. F cable .

[0092] Motion State Fusion Module: Receives data from the vehicle-mounted motion state perception unit 24, performs spatiotemporal alignment and fusion, and outputs the initial expected relative position of the vehicle platform. X des , X des This includes pose, velocity, and acceleration estimation.

[0093] Admittance control module: Vectors cable tension. F cable As input, a compliance correction position vector is output through a predefined virtual mass-spring-damper system. X .

[0094] Virtual parameters M , B , K The settings determine the degree to which a drone obeys external forces:

[0095] Virtual quality M When the force is large, the drone's acceleration response to external forces becomes slower;

[0096] Virtual Damping B When the speed is large, the drone's speed response to external forces is suppressed, which is beneficial for overshoot suppression;

[0097] Virtual stiffness K It determines the proportional relationship between external force and displacement. K The smaller the drone, the more "compliant" it is, and the more sensitive it is to external forces.

[0098] In a preferred embodiment M , B , K It is not a fixed value, but rather depends on the current vehicle speed on the onboard platform. v Adaptive adjustment: The higher the vehicle speed, the greater the virtual damping. B The corresponding increase is made to enhance system stability and prevent cable sag and oscillation caused by overshoot.

[0099] Instruction synthesis module: This module processes the compliance correction vector output by the admittance control module. X The initial expected relative position calculated based on the pose of the vehicle platform X des The data is overlaid to generate the final UAV target position command. X cmd = X des +Δ X .

[0100] Safety protection module: A first safety threshold is set corresponding to 75% of the cable's rated tensile force, and a second safety threshold corresponds to the lower limit of the cable's tensile force under its own weight and sag safety limit. These values ​​are output to a threshold comparison circuit, which, after real-time monitoring and comparison, outputs interrupt signals Vcom1 and Vcom2. When the first safety threshold is exceeded, the safety protection module intervenes, instructing the command synthesis module to scale. X des The weights of the compliance correction vector X Taking the lead, the drone is forced to move in the direction that reduces tension. When the speed drops below the second safety threshold, the safety protection module injects a velocity feedforward component into the command synthesis module, causing the drone to actively accelerate to "catch up" with the vehicle platform, thereby tightening the slack cable.

[0101] Example 6: Performance Verification

[0102] Figures 5(a) to 5(c) show a simulation comparison of cable tension using the method of this invention (solid line) and the conventional position PID feedback method (dashed line) under a simulated sudden acceleration condition on a vehicle platform. The simulation parameters are shown in Table 1 below.

[0103] Table 1

[0104]

[0105] The vehicle platform is set to start moving at 2m / s at 1 second. 2 The acceleration increased from 5 m / s² to 15 m / s² over 5 seconds. A comparison was made between traditional PID position feedback control and the force admittance control of this invention; both have the same underlying position tracking PID parameters.

[0106] Figure 5(a) shows the time response curves of the horizontal tension of the tethered cable under the two schemes. The traditional PID scheme has a peak tension of 37.4N after acceleration, while in the scheme of this invention, the force admittance module converts the cable tension vector into compliance correction in real time, so that the UAV moves in the direction of the tension to release the impact energy. The peak tension is only 28.5N, and the transition is smooth, always far away from the danger threshold.

[0107] Figures 5(b) and 5(c) show a comparison of following position error and UAV speed, respectively. It can be seen that this invention, while maintaining good following accuracy, significantly reduces the risk of mechanical shock and cable overload, fully demonstrating the advantages of force-feedback compliant control.

[0108] Those skilled in the art will understand that embodiments of the present invention can be provided as methods, systems, or computer program products. Therefore, the present invention can take the form of a completely hardware embodiment, a completely software embodiment, or an embodiment combining software and hardware aspects. Furthermore, the present invention can take the form of a computer program product embodied on one or more computer-usable storage media containing computer-usable program code.

[0109] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention and not to limit its scope of protection. Although the present invention has been described in detail with reference to the above embodiments, those skilled in the art should understand that after reading the present invention, they can still make various changes, modifications or equivalent substitutions to the specific implementation of the application, but these changes, modifications or equivalent substitutions are all within the scope of protection of the claims pending approval.

Claims

1. A dynamic following control system for tethered unmanned aerial vehicles based on cable force feedback, characterized in that, include: The force sensing unit (25) adopts a three-dimensional force sensor, which is located on the cable guide wheel of the take-up and release device (21); Used to acquire analog signals of three-dimensional forces; The vehicle-mounted motion state perception module (24) uses a GNSS receiver and an inertial measurement unit to acquire latitude, longitude, speed, heading data and acceleration and angular velocity data; The collaborative controller (23) includes a force feedback control circuit (231) and a main control chip (232) connected thereto. The force feedback control circuit (231) is connected to the force sensing unit (25) to collect the digital force signal of the cable, and implements a threshold comparison output interrupt signal through hardware circuit. The digital force signal of the cable and the interrupt signal are uploaded to the main control chip (232). The main control chip (232) is used to calculate the compliance correction vector based on the preset virtual mass, virtual damping and virtual stiffness parameters using the digital force signal as input, and superimpose it on the initial position expectation to generate the target tracking command Xcmd. The tethered UAV (10) follows the highway inspection vehicle according to the target position command Xcmd. The ground power supply and communication module (22) is used to provide power to the tethered cable retraction device (21), the cooperative controller (23), and the vehicle motion state sensing module (24), and to enable the cooperative controller (23) to communicate with the outside world.

2. The tethered UAV dynamic following control system based on cable force feedback according to claim 1, characterized in that, The force feedback control circuit (231) includes a force sensing unit signal conditioning circuit and a safety threshold hardware comparison circuit connected in sequence.

3. The tethered UAV dynamic following control system based on cable force feedback according to claim 2, characterized in that, The force sensing unit signal conditioning circuit includes a reference voltage circuit, a three-way Wheatstone bridge and amplifier circuit, and a multi-channel analog-to-digital acquisition circuit to acquire three-dimensional force signals.

4. A tethered UAV dynamic following control system based on cable force feedback according to claim 3, characterized in that, The reference voltage circuit includes a precision reference source chip U1 and its peripheral capacitors C2, C3, and C4, which outputs a +5V_REF precision voltage to provide a reference power supply for the subsequent Wheatstone bridge and analog-to-digital converter. The three-way Wheatstone bridge and amplifier circuit includes three identical signal conditioning branches that acquire analog force signals V_Fx, V_Fy, and V_Fz from the UAV cable in the X, Y, and Z dimensions. Each connection includes: a strain gauge RG1 from the three-dimensional force sensor and voltage divider resistors R1-R3 forming a full-bridge structure to convert the force signal into a differential voltage signal. This differential voltage signal is then connected to the input of operational amplifier U2 via current-limiting resistors R4 and R5. Operational amplifier U2 is powered by a single +5V_REF supply and an external gain resistor R6 amplifies the signal output from the bridge. The output of the operational amplifier is connected to an RC filter network to output the analog force signal. The multi-channel analog-to-digital acquisition circuit uses an analog-to-digital converter U3; the analog-to-digital converter U3 is connected to a +5V_REF reference voltage and a crystal oscillator U6; the three analog force signals V_Fx, V_Fy, and V_Fz are connected to the input pins AIN0, AIN2, and AIN4 of the analog-to-digital converter U3 via current-limiting resistors R20, R21, and R26, respectively; the output pins SCLK, DIN, DOUT, and CS are connected to the SPI interface to output the converted three digital signals V_x, V_y, and V_z to the main control chip (232).

5. A tethered UAV dynamic following control system based on cable force feedback according to claim 3, characterized in that, The security threshold hardware comparison circuit includes a square operation circuit, an inverse summation circuit, a square root operation circuit, and a threshold comparison circuit connected in sequence.

6. A tethered unmanned aerial vehicle (UAV) dynamic following control system based on cable force feedback according to claim 5, characterized in that, The squaring circuit uses a three-way multiplier. The two input terminals of each multiplier are connected to the same analog force signal. The three multipliers perform squaring operations on the three analog force signals V_Fx, V_Fy, and V_Fz respectively, and the output terminal of each multiplier is connected to the summing node through resistors R1, R2, and R3 respectively. The inverting summation circuit uses an operational amplifier U4 as an inverting adder to receive three squared signals and perform summation operations, outputting the inverted value of the square of the three-dimensional force vector. The square root operation circuit is a closed-loop feedback square root circuit composed of operational amplifier U5 and multiplier U6. The output signal of operational amplifier U4 is buffered by operational amplifier U5, multiplied by itself by multiplier U6, and then fed back to the inverting input terminal of operational amplifier U5 to form a closed loop, and finally outputs the modulus signal of the three-dimensional force vector sum. The threshold comparison circuit uses voltage comparator U7.1 and voltage comparator U7.

2. The modulus signal of the sum of the three-dimensional force vectors is connected to the non-inverting input of two comparators, and the inverting input of the two comparators is connected to the preset safety thresholds Vref1 and Vref2 respectively; the output of the comparators is connected to the +5V power supply through pull-up resistors R7 and R8, and outputs Vcom1 and Vcom2 signals to the main control chip (232). When the magnitude of the sum of the three-dimensional force vectors exceeds any threshold, the corresponding comparator flips and outputs a hardware trigger signal, achieving microsecond-level response for safety protection.

7. A tethered unmanned aerial vehicle (UAV) dynamic following control system based on cable force feedback according to claim 1, characterized in that, The main control chip (232) includes the following program modules: Force information processing module: receives three-dimensional force digital signals V_x, V_y, and V_z from force feedback control circuit (231), and calculates the current disturbance vector generated by the tethered cable on the UAV after signal conditioning, that is, the three-dimensional tension vector Fcable of the cable; The motion state fusion module receives data from the vehicle motion state perception unit (24), performs spatiotemporal alignment and fusion, and outputs the initial expected relative position Xdes of the vehicle platform. Xdes includes estimates of pose, velocity and acceleration. The admittance control module takes the cable tension vector Fcable as input and outputs a compliance correction position vector through a predefined virtual mass-spring-damper model. X; The instruction synthesis module converts the compliance correction vector output by the admittance control module into a variable. X is superimposed on the initial expected relative position Xdes calculated by the motion state fusion module to generate the final UAV target position command Xcmd=Xdes+ΔX; it is also used to adjust the command strategy based on the interrupt signals Vcom1 and Vcom2 output by the safety threshold hardware comparison circuit. The security protection module sets a first security threshold Vref1 and a second security threshold Vref2 and sends them to the security threshold hardware comparison circuit. The first safety threshold corresponds to 75% of the cable's rated tensile force, and the second safety threshold corresponds to the lower limit of the cable's tensile force under its own weight sag safety limit. The interrupt signals Vcom1 and Vcom2 output by the hardware comparison circuit for acquiring the safety threshold are collected.

8. A tethered UAV dynamic following control system based on cable force feedback according to claim 7, characterized in that, The instruction adjustment strategy based on the interrupt signals Vcom1 and Vcom2 output by the security threshold hardware comparison circuit includes: When the first safety threshold is exceeded, the instruction synthesis module is triggered to scale the weights of Xdes, thus adjusting the compliance correction vector. X takes precedence, and the drone prioritizes moving in the direction that reduces pull; When the speed drops below the second safety threshold, a speed feedforward component is injected into the command synthesis module, causing the drone to actively accelerate to "catch up" with the vehicle platform, thereby tightening the slack cable.

9. A dynamic following control method for tethered unmanned aerial vehicles based on cable force feedback, characterized in that, Includes the following steps: The three-dimensional force digital signal is acquired by the force feedback control circuit (231); the three-dimensional tension vector Fcable of the tether cable on the UAV is calculated after signal conditioning; The vehicle motion state perception unit (24) collects data, performs spatiotemporal alignment and fusion, and outputs the initial expected relative position Xdes of the vehicle platform, which includes estimates of pose, velocity and acceleration. Using the cable tension vector Fcable as input, and passing through a predefined virtual mass-spring-damper system, the output is a compliance correction position vector. X; Compliance Modification Vector X is superimposed with the initial expected relative position Xdes to generate the final UAV target position command Xcmd=Xdes+ΔX; and the command adjustment strategy is performed according to the interrupt signals Vcom1 and Vcom2 output by the safety threshold hardware comparison circuit.

10. A dynamic following control method for tethered unmanned aerial vehicles based on cable force feedback according to claim 9, characterized in that, The instruction adjustment strategy based on the interrupt signals Vcom1 and Vcom2 output by the security threshold hardware comparison circuit includes: Set a first safety threshold Vref1 and a second safety threshold Vref2; the first safety threshold corresponds to 60%~85% of the rated tensile force of the cable, and the second safety threshold corresponds to the lower limit of the tensile force of the cable under its own weight sag safety limit. The interrupt signals Vcom1 and Vcom2 output by the hardware comparison circuit for acquiring the safety threshold are collected. When the first safety threshold is exceeded, the weights of Xdes are scaled to make the compliance correction vector... X takes precedence, and the drone prioritizes moving in the direction that reduces pull; When the speed drops below the second safety threshold, a speed feedforward component is injected to cause the tethered drone (10) to actively accelerate in order to "catch up" with the vehicle platform, thereby tightening the slack cable.

Images