A force-position coordinated control circuit with energy feedback

Abstract

The present invention relates to a force-position coordinated control circuit with energy feedback, and the circuit includes a CPU, dual FPGAs, a drive circuit, a positioning motor, a torque regulator, and an energy feedback circuit. The CPU runs a control algorithm and sends instructions to the dual FPGAs, and the two FPGAs independently generate PWM waveforms and complete high-speed sampling, significantly reducing control delays and sampling synchronization errors. The energy feedback circuit stores the regenerative electrical energy generated when the motor is reversely driven through a supercapacitor, and preferentially supplies power to the CPU and FPGA, reducing dependence on external power supplies and improving energy utilization. The present invention eliminates synchronous control delays, improves anti-interference performance through the collaborative design of force-position closed-loop decoupling and dynamic voltage stabilization of supercapacitors through a dual FPGA parallel architecture, thereby solving the technical problems of dynamic response mismatch and low energy feedback efficiency in force-position hybrid control. It is suitable for scenarios such as high-precision robot joints and aerospace actuators.

Description

Technical Field

[0001] The present invention belongs to the field of power electronics technology and precision electromechanical control, and specifically relates to a force-position coordinated control circuit with energy feedback, which is suitable for scenarios such as high-precision robot joints and aerospace actuators. Background Art

[0002] Traditional force-position hybrid control circuits generally use a single controller (such as an MCU) to simultaneously process two motor signals. This inherent flaw restricts system performance. A single controller must alternately process the two signals through time-sharing multiplexing, which results in a prolonged control cycle and significant dynamic response lag. For example, the cumulative computational delay of the dual current loop can exceed 200μs. Furthermore, the sampling of the two motor signals, due to shared controller resources, leads to timing contention. The synchronization error between the encoder and current signal acquisition often exceeds 20μs, severely impacting the stability of high-precision motion control. Furthermore, the system suffers from low energy efficiency, with motor regenerative energy often dissipated as heat through braking resistors. This not only wastes energy but also increases the burden on thermal design. Existing improvements attempt to reduce computational latency through multi-core CPUs or distributed controllers. However, multi-core task scheduling requires increased inter-core communication overhead (approximately 10-30μs), exponentially increasing system complexity and cost. While energy storage units such as lithium batteries can partially recover some energy, their low charge and discharge rates make them difficult to match the transient pulse energy generated by high-frequency reverse drive of the motor. The above contradictions are particularly prominent in high-dynamic scenarios such as robot joints and precision servo systems. There is an urgent need to build a new force-position collaborative control architecture that combines low-latency signal processing capabilities and high-efficiency energy management. Summary of the Invention

[0003] In order to solve the above technical problems, the present invention provides a force-position coordinated control circuit with energy feedback, which reduces the time delay of dual-motor synchronous control and sampling, and realizes dynamic decoupling of force-position hybrid control and efficient recovery of motor reverse drive regenerated electric energy and system energy supply optimization.

[0004] To achieve the above object, the technical solution adopted by the present invention is as follows:

[0005] A force-position coordinated control circuit with energy feedback includes a CPU, a first FPGA, a second FPGA, a motor A drive circuit, a motor B drive circuit, motor A, motor B, a current acquisition circuit A, a current acquisition circuit B, an encoder signal acquisition circuit A, an encoder signal acquisition circuit B, and an energy feedback circuit, wherein:

[0006] The CPU is bidirectionally connected to the first FPGA, the second FPGA, and the energy feedback circuit respectively;

[0007] The energy feedback circuit, current acquisition circuit A, encoder signal acquisition circuit A, and encoder signal acquisition circuit B are all unidirectionally connected to the first FPGA, inputting regenerated electric energy or acquisition information to the first FPGA, and the first FPGA outputs control information to the motor A drive circuit; the motor A drive circuit drives the motor A to run, and outputs current and position signals to the current acquisition circuit A and the encoder signal acquisition circuit A, respectively;

[0008] The energy feedback circuit, current acquisition circuit B, encoder signal acquisition circuit B, and current acquisition circuit A are all unidirectionally connected to the second FPGA, and regenerated electric energy or acquisition information is input into the second FPGA. The second FPGA outputs control information to the motor B drive circuit. The motor B drive circuit drives the motor B to run, and outputs current and position signals to the current acquisition circuit B and the encoder signal acquisition circuit B respectively. The encoder signal acquisition circuit B sends a correction signal to the first FPGA; wherein, the regenerated electric energy of the motor B driven in reverse is sent to the energy feedback circuit.

[0009] The beneficial effects of the present invention are:

[0010] The present invention significantly improves the comprehensive performance of the force-position hybrid control system through the integration of the CPU and dual FPGA collaborative architecture, the force-position cross-feedback mechanism and the dynamic power supply of supercapacitors: the total control delay of the dual FPGA parallel processing system is significantly reduced, the PWM waveform is synchronized at the sub-microsecond level, and the real-time cross-feedback of the position and torque loops is combined (the position motor current sampling value is injected into the torque loop feedforward, and the torque motor encoder signal corrects the position loop), which effectively suppresses nonlinear friction and load mutation interference; the supercapacitor module recovers the motor reverse drive energy, and converts it into a low-voltage stable power supply through a multi-stage voltage regulator chip, which gives priority to powering the CPU and FPGA, thereby improving the overall energy efficiency of the system and reducing dependence on external power supply; in terms of hardware architecture, the single driver board integrates dual FPGA control cores, reduces communication interfaces and auxiliary circuits, and reduces manufacturing costs, providing a high-precision, low-latency, and high-energy-efficiency integrated control solution for high-end equipment such as robot joints and aerospace actuators. BRIEF DESCRIPTION OF THE DRAWINGS

[0011] Figure 1 This is a schematic diagram of a force-position hybrid control circuit with energy feedback according to the present invention;

[0012] Figure 2 This is the structure diagram of the energy feedback circuit;

[0013] Figure 3 This is a structural diagram of a force-position hybrid control circuit with energy feedback according to the present invention. DETAILED DESCRIPTION

[0014] In order to make the above-mentioned objects, features and advantages of the present invention more obvious and easy to understand, the present invention is further described below with reference to the accompanying drawings.

[0015] One of the purposes of the present invention is to provide a force-position coordinated drive circuit and controller to reduce the time delay of dual-motor synchronous control and sampling, and to achieve dynamic decoupling of force-position hybrid control; the second purpose of the present invention is to achieve efficient recovery of regenerated electric energy from motor reverse drive and system energy supply optimization.

[0016] One of the objectives of the present invention is achieved in the following manner: based on the CPU running the upper-level motion planning algorithm, dual FPGAs independently and in parallel process motor control tasks - the first FPGA is dedicated to the current loop calculation, high-precision current sampling and PWM waveform generation of the position mode motor, and the second FPGA is dedicated to the real-time decoding of the encoder signal, torque loop control and PWM output of the torque mode motor. The dual FPGAs interact with the CPU through a hardware-level parallel bus, eliminating the cumulative control cycle delay caused by the time-sharing multiplexing of a single controller and achieving sub-microsecond synchronization of the dual PWM waveforms; at the same time, a force-position cross-feedback mechanism is introduced into the control loop, and the current sampling value of the position mode motor (reflecting the output torque) is injected into the feedforward compensation link of the torque mode motor in real time, and the encoder position signal of the torque mode motor is fed back to the position loop for dynamic correction, effectively suppressing the mechanical coupling interference between the positioning motor and the torque regulator.

[0017] The second object of the present invention is achieved by integrating a high-power density supercapacitor on the DC bus side to capture the transient regenerative electric energy generated when the motor is reverse driven, and converting the wide range voltage output by the supercapacitor into stable DC power through a multi-stage voltage regulator chip. At the same time, a priority power supply circuit is designed to preferentially supply the stabilized supercapacitor power to the CPU and FPGA core modules.

[0018] Please refer to Figure 1 , which is a schematic diagram of the force-position hybrid control circuit with energy feedback of the present invention, including a CPU, a first FPGA, a second FPGA, a motor A drive circuit, a motor B drive circuit, motor A, motor B, a current acquisition circuit A, a current acquisition circuit B, an encoder signal acquisition circuit A, an encoder signal acquisition circuit B, a power supply circuit, and an energy feedback circuit.

[0019] The CPU is bidirectionally connected to the first FPGA, the second FPGA, and the energy feedback circuit respectively;

[0020] The energy feedback circuit, current acquisition circuit A, encoder signal acquisition circuit A, and encoder signal acquisition circuit B are all unidirectionally connected to the first FPGA, inputting regenerated electric energy or acquisition information to the first FPGA, and the first FPGA outputs control information to the motor A drive circuit; the motor A drive circuit drives the motor A to run, and outputs current and position signals to the current acquisition circuit A and the encoder signal acquisition circuit A, respectively;

[0021] The energy feedback circuit, current acquisition circuit B, encoder signal acquisition circuit B, and current acquisition circuit A are all unidirectionally connected to the second FPGA, inputting regenerated electric energy or acquisition information into the second FPGA. The second FPGA outputs control information to the motor B drive circuit. The motor B drive circuit drives motor B to operate, outputting current and position signals to the current acquisition circuit B and the encoder signal acquisition circuit B, respectively. The encoder signal acquisition circuit B sends a correction signal to the first FPGA. The regenerated electric energy generated by the reverse drive of motor B is sent to the energy feedback circuit.

[0022] The power supply circuit supplies power to the energy feedback circuit, the motor A drive circuit, and the motor B drive circuit.

[0023] The CPU is used to perform upper-level motion planning, such as calculating feedforward terms to compensate for the nonlinear friction of the motor and the flexible deformation of the gear train, and running the position loop and speed loop algorithms of the motor.

[0024] The first and second FPGAs are used to run the current loop algorithms for motors A and B, respectively, generating PWM waveforms in parallel and synchronously acquiring the current signals and encoder signals for motors A and B. When a single CPU processes the current loop algorithms for two motors, due to serial computational constraints, the CPU must time-share the two current loop algorithms, resulting in delayed dynamic response. When the CPU outputs waveforms via a software timer or PWM peripheral, it must undergo processes such as interrupt response and register configuration. This creates a delay between the completion of the algorithm calculation and the actual implementation of the PWM update, and the accuracy of dual-motor PWM synchronization is affected by task scheduling jitter. Encoder and current signals are typically transmitted to the CPU via peripheral interfaces (such as SPI and CAN). Due to bus polling mechanisms, multi-channel sampling must be performed sequentially, resulting in insufficient sampling synchronization. The sampled data must be read and processed uniformly by the CPU. The time difference between the current loop feedback value and the actual physical quantity can exceed one control cycle, exacerbating control loop phase delay. In contrast, the FPGA of the present invention uses hardware logic to run two current loop algorithms in parallel, reducing the computation time per channel to 1 / 5 to 1 / 10 of that of a CPU solution. The dual-motor computations are fully synchronized, eliminating time-sharing and cumulative delays. PWM waveform generation is directly driven by hardware counters, reducing the latency from algorithm results to PWM duty cycle updates to less than 1 / 10 of the original. Encoder and current signals are acquired in real time via dedicated hardware interfaces within the FPGA (such as the SSI decoding module and delta-sigma ADC interface), with dual-channel sampling trigger intervals controllable to nanoseconds. Sampled data is directly embedded in the current loop calculation process, reducing feedback latency to within a single control cycle. Furthermore, the random jitter (such as interrupt response delay and task switching overhead) caused by multitasking in the CPU solution is completely eliminated by hardware parallelization in the FPGA solution. This CPU and dual-FPGA architecture significantly reduces system synchronization control and signal acquisition latency.

[0025] The motor A driving circuit and the motor B driving circuit are used to receive the PWM waveforms generated by the first FPGA and the second FPGA respectively, and convert them into their own switching states to drive the operation of the motor A and the motor B.

[0026] Motor A is a positioning motor used for position tracking and position control via a closed-loop position loop. Position feedback from motor B is also introduced, i.e., the first FPGA collects the position feedback signal from encoder signal acquisition circuit B to dynamically correct the target position.

[0027] The current acquisition circuit A and the encoder signal acquisition circuit A are used to measure the current and position signal of the motor A respectively.

[0028] Motor B is a torque regulator, used to adjust output torque and control torque through a closed-loop current loop. Torque feedback from Motor A is also introduced. Specifically, the second FPGA collects the torque feedback signal from the current acquisition circuit A and performs feedforward compensation for the target torque.

[0029] The current acquisition circuit B and the encoder signal acquisition circuit B are used to measure the current and position signal of the motor B respectively.

[0030] The power supply circuit is used to provide power to the system.

[0031] The energy feedback circuit is used to store regenerative energy generated by the reverse drive of motor B (i.e., the torque regulator) and preferentially uses this regenerative energy to power the CPU, first FPGA, and second FPGA. The power supply circuit is connected to an external power source and generates a regulated DC voltage via a step-down module. When the energy feedback circuit is not storing energy, the external power source powers the system. Figure 2 VCC5V and VCC3.3V are the DC stable voltages obtained after the external power supply is stepped down by the power supply circuit.

[0032] like Figure 2 Figure 2 shows an energy feedback circuit structure diagram, illustrating the circuit principles for storing regenerative energy generated by reverse-driven motor B (torque regulator) and prioritizing power for the CPU and FPGA according to a preferred embodiment of the present invention. The energy feedback circuit includes several NMOS and PMOS transistors, a voltage regulator chip U1, and a CPU-controlled relay U3. PMOS transistor Q7 includes a supercapacitor C8 connected to the DC bus. Supercapacitor C8 is used to store regenerative energy generated by reverse-driven motor B. One end of relay U3 is connected to PMOS transistor Q7 to switch the supercapacitor's charge and discharge states. Voltage regulator chip U1 is connected to the other end of relay U3 to convert the wide-range voltage output by supercapacitor C8 into a stable DC voltage.

[0033] Furthermore, when the motor is driven forward, PMOS transistor Q7 is turned off, relay U3 is turned on, and supercapacitor C8 is discharged. When the motor is driven reverse, PMOS transistor Q7 is turned on, relay U3 is turned off, and supercapacitor C8 is charged. When supercapacitor C8 is discharged, the CPU, first FPGA, and second FPGA are preferentially powered by supercapacitor C8. When supercapacitor C8 is charged or out of power, the CPU, first FPGA, and second FPGA are powered by the power supply circuit.

[0034] For details, see Figure 2The S-pole of PMOS transistor Q7 is connected to the DC bus, the D-pole is connected to supercapacitor C8, and the G-pole is connected to current-limiting resistor R8. One end of supercapacitor C8 is connected to the D-pole of PMOS transistor Q2, and the other end is grounded. Pull-down resistor R9 is connected between current-limiting resistor R8 and ground. Diode D3 is connected to the DC bus, with its cathode connected to the anode of diode D1. The cathode of diode D1 is connected to the cathode of Zener diode D2, and the anode of Zener diode D2 is connected to the DC bus. The load terminal of relay U3 is connected between the D-pole of PMOS transistor Q2 and the input terminal VIN of voltage regulator chip U1. The positive input terminal is connected to the CPU, and the negative input terminal is grounded.

[0035] When the DC bus voltage reaches 24V, PMOS transistor Q2 turns off, controlling relay U3's load terminal to conduct, and supercapacitor C8 discharges. When motor B is driven in reverse, due to the unidirectional conduction characteristics of diode D3, the S-pole voltage of PMOS transistor Q2 exceeds 24V, causing Q2 to conduct. This simultaneously turns off the load terminal of control relay U3, and supercapacitor C8 begins charging. To prevent transient voltages from exceeding the supercapacitor's withstand voltage, diode D1 and Zener diode D2 are connected in series and then in parallel across diode D3. When the D-pole voltage of PMOS transistor Q2 exceeds the safety threshold, D2 turns on.

[0036] The voltage regulator chip U1 is composed of, for example, XL4015, and can convert an input voltage of 8 to 36V into an output voltage of 1.25 to 32V.

[0037] (1)

[0038] Pick is 3.3k, =10k, then =5V.

[0039] The load terminal of relay U2 is connected between the output terminal VOUT of voltage regulator chip U1 and the G terminal of NMOS transistor Q4. The positive input terminal is connected to the CPU, and the negative input terminal is grounded. The S terminal of NMOS transistor Q4 is grounded, and the G terminal is also connected to the D terminal of PMOS transistor Q3, which in turn is connected to the G terminal of PMOS transistor Q3. Pull-up resistor R5 is connected between the G and S terminals of PMOS transistor Q3. The D terminal of PMOS transistor Q3 is also connected to the G terminal of PMOS transistor Q1, and the S terminal is also connected to the S terminal of PMOS transistor Q1, and is also connected to the power supply input of the CPU and FPGA. Pull-down resistor R3 has one end connected to the G terminal of PMOS transistor Q1 and the other end connected to ground. The D terminal of PMOS transistor Q1 is connected to the 5V output port of the power supply circuit.

[0040] When the power supply circuit is powered off, the load end of the control relay U2 is powered off. At this time, VOUT1=0V and the circuit does not work.

[0041] When the power supply circuit is energized, the load end of the control relay U2 is turned on.

[0042] If the output voltage of voltage regulator chip U1 is 5V, NMOS transistor Q4 turns on, and the G-pole voltage of PMOS transistor Q3 is 0. Because the conduction voltage drop of the parasitic diode in PMOS transistor Q3 is only a few tens of millivolts, Q3 turns on at this time, and the S-pole voltage of Q3 is 5V. The G-pole and S-pole voltages of Q1 are both 5V, and Q1 turns off. VOUT1 = 5V, provided by voltage regulator chip U1.

[0043] If the output voltage of voltage regulator chip U1 is 0, NMOS transistor Q4 turns off, and the G terminal voltage of PMOS transistor Q1 is 0. Because the conduction voltage drop of the parasitic diode in PMOS transistor Q1 is only a few tens of millivolts, Q1 turns on, and the S terminal voltage of Q1 is 5V. The G terminal of PMOS transistor Q3 is pulled up through R5, and Q3 turns off. VOUT1 = 5V, provided by the power supply circuit.

[0044] The voltage regulator chip U5 is composed of, for example, 1117, and can convert an input voltage of 5V into an output voltage of 3.3V.

[0045] Similarly, when the power supply circuit is powered off, the load end of the control relay U4 is turned off. At this time, VOUT2 = 0V and the circuit does not work.

[0046] When the power supply circuit is energized, the load end of the control relay U4 is turned on;

[0047] If the output voltage of voltage regulator chip U5 is 3.3V, NMOS transistor Q6 turns on, and the G-pole voltage of PMOS transistor Q5 is 0. Because the conduction voltage drop of the parasitic diode in PMOS transistor Q5 is only a few tens of millivolts, Q5 turns on at this time, and the S-pole voltage of Q5 is 3.3V. The G-pole and S-pole voltages of Q2 are both 3.3V, and Q2 turns off. VOUT2 = 3.3V, provided by voltage regulator chip U5.

[0048] If the output voltage of voltage regulator chip U5 is 0, NMOS transistor Q6 turns off, and the G terminal voltage of PMOS transistor Q2 is 0. Because the conduction voltage drop of the parasitic diode in PMOS transistor Q2 is only a few tens of millivolts, Q2 turns on at this time, and the S terminal voltage of Q2 is 3.3V. The G terminal of PMOS transistor Q5 is pulled up through R6, and Q5 turns off. VOUT2 = 3.3V, provided by the power supply circuit.

[0049] In this way, when the supercapacitor C8 is discharged, the CPU and FPGA are preferentially powered by the supercapacitor C8.

[0050] Figure 3A schematic diagram of the entire circuit according to the preferred embodiment of the present invention is shown below. Since those skilled in the art can easily understand the present invention based on the circuit diagram, the schematic diagrams of the motor A drive circuit, motor B drive circuit, current acquisition circuit A, current acquisition circuit B, encoder signal acquisition circuit A, encoder signal acquisition circuit B, and power supply circuit have been shown in the figure. Figure 3 The specific circuit connections are disclosed and will not be described in detail here.

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

[0052] The specific embodiments described above further illustrate the objectives, technical solutions and beneficial effects of the present invention in detail. It should be understood that the above are only specific embodiments of the present invention and are not intended to limit the present invention. Any modifications, equivalent substitutions, improvements, etc. made within the spirit and principles of the present invention should be included in the scope of protection of the present invention.

Claims

1. A force-position coordinated control circuit with energy feedback, characterized in that: It includes a CPU, a first FPGA, a second FPGA, a motor A drive circuit, a motor B drive circuit, a motor A, a motor B, a current acquisition circuit A, a current acquisition circuit B, an encoder signal acquisition circuit A, an encoder signal acquisition circuit B, and an energy feedback circuit, wherein: The CPU is bidirectionally connected to the first FPGA, the second FPGA, and the energy feedback circuit respectively; The energy feedback circuit, current acquisition circuit A, encoder signal acquisition circuit A, and encoder signal acquisition circuit B are all unidirectionally connected to the first FPGA, inputting regenerated electric energy or acquisition information to the first FPGA, and the first FPGA outputs control information to the motor A drive circuit; the motor A drive circuit drives the motor A to run, and outputs current and position signals to the current acquisition circuit A and the encoder signal acquisition circuit A, respectively; The energy feedback circuit, current acquisition circuit B, encoder signal acquisition circuit B, and current acquisition circuit A are all unidirectionally connected to the second FPGA, inputting regenerated electric energy or acquisition information into the second FPGA. The second FPGA outputs control information to the motor B drive circuit. The motor B drive circuit drives motor B to operate, outputting current and position signals to the current acquisition circuit B and the encoder signal acquisition circuit B, respectively. The encoder signal acquisition circuit B sends a correction signal to the first FPGA. The regenerated electric energy generated by the reverse drive of motor B is sent to the energy feedback circuit. The first FPGA and the second FPGA are used to respectively run the current loop algorithms of the motor A and the motor B, generate PWM waveforms in parallel, and synchronously collect the current signals and encoder signals of the motor A and the motor B; The motor A is a positioning motor used for position tracking, and controls the position through a closed-loop position loop. At the same time, the position feedback signal of the encoder signal acquisition circuit B acquired by the first FPGA is introduced to dynamically correct the target position. The motor B is a torque regulator for adjusting the output torque and controlling the torque through a current closed-loop control. At the same time, the torque feedback signal of the current acquisition circuit A acquired by the second FPGA is introduced to make feedforward compensation for the target torque.

2. A force-position coordinated control circuit with energy feedback according to claim 1, characterized in that: It also includes a power supply circuit, which supplies power to the energy feedback circuit, the motor A drive circuit, and the motor B drive circuit.

3. The force-position coordinated control circuit with energy feedback according to claim 1, characterized in that: The energy feedback circuit is used to store the regenerative electric energy generated by the reverse driving of the motor B, and preferentially uses the regenerative electric energy to power the CPU, the first FPGA, and the second FPGA.

4. The force-position coordinated control circuit with energy feedback according to claim 1, characterized in that: The energy feedback circuit includes a number of NMOS transistors, PMOS transistors, a voltage regulator chip U1, and a relay U3 controlled by a CPU.

5. The force-position coordinated control circuit with energy feedback according to claim 4, characterized in that: The PMOS tube Q7 includes a supercapacitor C8 connected to the DC bus, and the supercapacitor C8 is used to store the regenerative electric energy generated when the motor is reverse driven; one end of the relay U3 is connected to the PMOS tube Q7 and is used to switch the charge and discharge state of the supercapacitor; the voltage regulator chip U1 is connected to the other end of the relay U3 and is used to convert the wide range voltage output by the supercapacitor C8 into a stable DC voltage.

6. The force-position coordinated control circuit with energy feedback according to claim 5, characterized in that: When the motor is driven forward, the PMOS tube Q7 is turned off, the relay U3 is turned on, and the supercapacitor C8 is discharged; when the motor is driven reverse, the PMOS tube Q7 is turned on, the relay U3 is turned off, and the supercapacitor C8 is charged.

7. A force-position coordinated control circuit with energy feedback according to claim 6, characterized in that: The energy feedback circuit is a priority power supply circuit with two inputs: the DC voltage of the supercapacitor C8 after voltage regulation and the DC voltage generated by the power supply circuit. When the supercapacitor C8 is discharged, the CPU, the first FPGA, and the second FPGA are powered by the supercapacitor C8 first. When the supercapacitor C8 is charged or has no power, the CPU, the first FPGA, and the second FPGA are powered by the power supply circuit.

Images