Heterogeneous high-real-time multi-axis robot cooperative motion control system and method

Abstract

The invention relates to the technical field of industrial automation control and robots, in particular to a heterogeneous high-real-time multi-axis robot cooperative motion control system and method.The system comprises an SoC main control layer, an FPGA driving layer and a hardware interconnection layer, the SoC main control layer adopts a heterogeneous SoC chip, an ARM processor is responsible for track planning and task scheduling, an FPGA logic deployment reconfigurable PID calculation engine is responsible for FPGA logic deployment, and the FPGA driving layer is responsible for FPGA logic deployment. A multi-axis position ring and speed ring algorithm is realized through the time division multiplexing processing unit; the FPGA driving layer comprises a plurality of FPGA coprocessors, and an FOC current loop IP core is instantiated to execute bottom layer vector control related operation; the hardware interconnection layer adopts an EPPI protocol supporting topology awareness, dynamic bandwidth allocation and a ping-pong buffering mechanism, and efficient transmission of a control instruction and feedback data is achieved. Through hierarchical control and hardware collaborative design, the problems that an existing multi-axis control system is large in calculation delay, insufficient in synchronization precision and low in hardware resource utilization rate are effectively solved.

Description

Technical Field

[0001] This invention relates to the field of industrial automation control and robotics, and in particular to a heterogeneous high real-time multi-axis robot cooperative motion control system and method. Background Technology

[0002] In multi-joint serial robots such as six-axis robotic arms, the control system needs to simultaneously control the motion of multiple servo motors. Modern high-performance servo drives typically employ Field-Oriented Control (FOC) technology. This technology decouples an AC motor into torque and excitation components through coordinate transformation, enabling independent control and achieving excellent speed regulation performance similar to that of a DC motor. An FOC system typically contains three closed-loop feedback loops: the innermost is the current loop (or torque loop), the middle is the velocity loop, and the outermost is the position loop. To ensure high dynamic response, the current loop update frequency typically needs to be on the order of 10kHz to 100kHz, while the position and velocity loops also need to be maintained on the order of 1kHz to 10kHz.

[0003] Existing multi-axis motion control system architectures are mainly divided into two categories: "centralized" and "distributed," but both have insurmountable technical bottlenecks. Traditional centralized control architectures based on general-purpose processors such as x86 CPUs or DSPs, while widely used in industry, reveal serious structural bottlenecks when facing the demands of multi-axis, high-frequency, and precision control. First, due to the serial instruction execution nature of the von Neumann architecture, the computational load becomes excessive as the number of controlled axes increases. This makes it difficult for the processor to sequentially complete the entire chain of calculations—from sensor reading and complex kinematics calculation to PID closed-loop control and PWM generation—within microsecond-level control cycles, easily leading to timing timeouts. Second, even under a real-time operating system, general-purpose computing platforms suffer from unavoidable interrupt response jitter due to factors such as cache misses, bus contention, and scheduling overhead. This microsecond-level timing uncertainty directly translates into motor torque fluctuations, thereby impairing machining accuracy and surface quality.

[0004] Mainstream distributed control architectures based on industrial Ethernet or fieldbuses decentralize sampling, control, and execution capabilities to individual nodes, which is beneficial for modular expansion and reducing the computational burden on single points in engineering. For high-performance motion control scenarios, system design typically requires end-to-end budgeting of communication link bandwidth usage, message overhead, and scheduling mechanisms. Therefore, in tasks sensitive to timing consistency, such as dual-arm collaboration or multi-axis linkage, systems often need to combine topology, switching and forwarding strategies, and synchronization parameter tuning to control the impact of propagation delay and jitter on trajectory consistency.

[0005] Based on the shortcomings of the existing technologies, there is an urgent need for a multi-axis robot cooperative motion control system and method that can solve the problems of large serial latency in computation, insufficient inter-axis synchronization accuracy, and low utilization of hardware resources. Summary of the Invention

[0006] The purpose of this invention is to overcome the shortcomings of the prior art and provide a heterogeneous, high real-time multi-axis robot cooperative motion control system and method, solving the problems of large computational serial delay, insufficient inter-axis synchronization accuracy, and low hardware resource utilization in existing multi-axis robot control systems. Through an innovative hardware acceleration engine, dedicated communication protocol, and synchronization network, the following objectives are achieved: 1. Real-time performance indicators: Achieve a deterministic control cycle of no more than 125 microseconds for the position / speed loop of the 9-axis motors in the entire system, and a current loop update frequency of 12.5 microseconds.

[0007] 2. Use a small number of high-performance PID processing units to poll and serve multi-axis control tasks, reducing the number of arithmetic units such as multiply-accumulators, reducing resources and costs, and improving scalability.

[0008] 3. Flexible scalability: Supports cascading expansion of the number of axes via the EPPI interface to adapt to robot configurations with different degrees of freedom.

[0009] The above objectives are achieved through the following technical solutions: A heterogeneous high real-time multi-axis robot cooperative motion control system is provided to solve the problems of large computational serial delay, insufficient inter-axis synchronization accuracy and low hardware resource utilization in existing multi-axis robot control systems. The system includes a SoC main control layer, an FPGA driver layer and a hardware interconnection layer. The hardware interconnection layer connects the SoC main control layer and the FPGA driver layer. The layers work together to achieve microsecond-level real-time closed-loop control. The SoC main control layer adopts a heterogeneous SoC chip containing an ARM processor and FPGA logic resources. The ARM processor runs a non-real-time operating system and is responsible for trajectory planning, human-computer interaction and task scheduling. The FPGA logic resources deploy a reconfigurable PID calculation engine based on register window reuse technology. By time-division multiplexing a limited number of high-performance PE processing units, the multi-axis position loop and velocity loop algorithms are polled and processed, reducing the hardware arithmetic unit occupation. The FPGA driver layer includes multiple FPGA coprocessors. Each FPGA coprocessor instantiates an independent FOC current loop IP core, which is responsible for the underlying current loop FOC algorithm, SVPWM generation, ADC sampling control and encoder signal processing. Each FPGA coprocessor can drive a multi-axis motor, and the number of axes can be extended by cascading the FPGA coprocessors through a dedicated interface. The hardware interconnect layer adopts the eight-line parallel peripheral interface (EPPI) protocol that supports topology awareness and ping-pong buffering mechanism to realize high-speed, deterministic transmission of control commands and feedback data between the SoC main control layer and each of the FPGA coprocessors, thereby reducing communication latency and jitter.

[0010] Preferably, the reconfigurable PID calculation engine allocates independent parameter storage space in RAM for all controlled axes; at the start of the calculation cycle, the hardware logic automatically maps the parameters of the current target axis to the operation register window of the PE processing unit. After the calculation is completed, the result is automatically written back, and the PE processing unit immediately switches to the context of the next axis to ensure 32-bit fixed-point high-precision operation.

[0011] Preferably, each of the PE processing units has a built-in intelligent state machine, which supports dynamic switching between three control modes: "position-velocity cascade", "independent position loop", and "independent velocity loop". In the "position-velocity cascade" mode, the position loop calculation output is directly used as the input of the velocity loop without the intervention of the ARM processor.

[0012] Preferably, the physical layer of the eight-line parallel peripheral interface (EPPI) protocol adopts an 8-bit bidirectional data bus (SDA[7:0]), in conjunction with a clock line (SCK), a chip select line (CS_N), and a handshake signal (VLD / RD_READY), to support half-duplex communication and achieve fast switching of bus direction through a tri-state buffer.

[0013] Preferably, the frame structure of the eight-wire parallel peripheral interface (EPPI) protocol includes a device ID field, and each FPGA coprocessor obtains its local physical ID through board-level hardwired connections or a plug-and-play mechanism; during the communication handshake phase, the FPGA coprocessor hardware compares the received frame ID with the local ID in real time, and only responds to bus operations when the IDs match; if an ID mismatch or chip select signal abnormality is detected, the hardware watchdog automatically resets the communication state machine.

[0014] Preferably, the ping-pong buffering mechanism is implemented through a double buffer queue. When the PID calculation engine processes the Nth frame of data, the eight-line parallel peripheral interface (EPPI) bus transmits the N+1th frame of data under the background DMA drive, realizing "zero-wait" pipeline processing.

[0015] Preferably, the implementation process of the FOC current loop IP core includes: acquiring three-phase current through an AD7606 parallel ADC, filtering it through a moving average filter, and then performing Clarke and Park transformations using a lookup table method to convert the current in the abc coordinate system to the current in the dq coordinate system; two parallel PI controllers control the d-axis and q-axis respectively, with the target value of I_d set to 0 and the target value of I_q coming from the EPPI interface; and an SVPWM generator implementing seven-segment modulation to generate six PWM waveforms with dead time.

[0016] Preferably, in the FPGA driver layer, one FPGA coprocessor is designated as the master synchronization node, and the rest are slave nodes; the master synchronization node generates a global synchronization pulse, which is distributed to each slave node and the SoC master control layer through a dedicated differential line to achieve an inter-axis synchronization accuracy of ≤5ns.

[0017] Preferably, the eight-line parallel peripheral interface (EPPI) protocol supports variable-length frame transmission. Bit[7] of the control frame is a read / write flag, Bit[6:2] is an adjustable data packet length of 1-32 bytes, and Bit[1:0] is the target motor / device ID. During the system initialization phase, the return data content of each FPGA coprocessor can be configured. During the operation phase, high-frequency current feedback is transmitted through short frames, and low-frequency status monitoring is transmitted through long frames to maximize the effective data throughput.

[0018] A heterogeneous, high real-time multi-axis robot cooperative motion control method, applied to the aforementioned control system, includes the following steps: S1: System initialization, complete the topology identification and ID binding between the SoC main control layer and each FPGA coprocessor through the eight-line parallel peripheral interface (EPPI) protocol, configure the number of PE processing units, control mode and FOC current loop parameters of the PID calculation engine, and set the motion loop control cycle to 125μs and the current loop update frequency to 80kHz. S2: The ARM processor of the SoC main control layer performs trajectory planning and task scheduling to generate multi-axis target trajectories; the reconfigurable PID calculation engine in the FPGA logic resources uses time-division multiplexing PE processing units to poll and load parameters of each axis, execute position loop and velocity loop algorithms, and output q-axis reference current (Iq_ref). S3: The FPGA coprocessor receives the q-axis reference current (Iq_ref) through the EPPI protocol, combines the local ADC sampling data with the encoder electrical angle, executes the FOC algorithm, and sequentially completes current filtering, coordinate transformation, PI adjustment and SVPWM generation to drive the motor. S4: The FPGA coprocessor collects motor position and speed feedback data and transmits it back to the SoC main control layer through the ping-pong buffering mechanism of the eight-line parallel peripheral interface (EPPI) protocol. S5: Repeat steps S2-S4 to ensure inter-axis synchronization through global synchronization pulses, thereby achieving microsecond-level real-time closed-loop collaborative control of the multi-axis robot.

[0019] This invention provides a heterogeneous, high-real-time multi-axis robot cooperative motion control system and method. It achieves efficient reuse of multi-axis operations through a heterogeneous SoC FPGA architecture and a reconfigurable PID calculation engine; low-latency data transmission through the topology-aware EPPI protocol and ping-pong buffering mechanism; and high-precision inter-axis coordination through global synchronization pulses and a distributed FOC current loop. This significantly improves computational performance, reduces communication latency and hardware footprint, and offers flexible scalability. Specific beneficial effects include: 1. Significantly Improved Computational Performance: Experiments have shown that the PID calculation engine of this invention achieves a 7.72-fold increase in computational speed compared to the RISC-V soft-core-based implementation. A single dual-loop PID iteration requires only 91 clock cycles, providing ample time margin for complex control algorithms.

[0020] 2. Significantly reduced communication latency: The EPPI protocol, combined with the ping-pong mechanism, reduces the end-to-end transmission and processing latency of 9-axis control data to less than 60μs, which is better than traditional SPI or I2C solutions.

[0021] 3. Extremely high dynamic accuracy: Thanks to the inter-axis synchronization accuracy within 5ns and the high-frequency position update of 125μs, the system significantly improves the dynamic performance of the robot when driving a multi-joint robotic arm. Attached Figure Description

[0022] Figure 1 This is a structural block diagram of a heterogeneous high real-time multi-axis robot cooperative motion control system according to the present invention; Figure 2 This is a hardware architecture diagram of the PID calculation engine in a heterogeneous high real-time multi-axis robot cooperative motion control system described in this invention. Figure 3 This is a structural block diagram of the EPPI interface in a heterogeneous high real-time multi-axis robot cooperative motion control system described in this invention; Figure 4 This is a block diagram of the FOC three-loop system in the heterogeneous high real-time multi-axis robot cooperative motion control system described in this invention; Figure 5 This is a schematic diagram of the ping-pong operation in a heterogeneous high real-time multi-axis robot cooperative motion control system described in this invention; Figure 6 This is the pid_pe state transition diagram in the heterogeneous high real-time multi-axis robot cooperative motion control system described in this invention; Figure 7 This is a block diagram of the PID algorithm in the heterogeneous high real-time multi-axis robot cooperative motion control method described in this invention. Detailed Implementation

[0023] The present invention will now be described in further detail with reference to the accompanying drawings and embodiments. The described embodiments are merely some, not all, of the embodiments of the present invention. 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.

[0024] This solution provides a heterogeneous, high real-time multi-axis robot cooperative motion control system. The system's hierarchical, heterogeneous, and hardware-software co-operational control architecture is as follows: Figure 1 As shown, the architecture includes a SoC main control layer, an FPGA driver layer, and a hardware interconnect layer. Each layer has a clear division of labor and works collaboratively, including: 1. SoC Main Control Layer: A heterogeneous SoC chip containing an ARM processor and FPGA logic resources is adopted to form a collaborative mode of "upper-layer task scheduling + lower-layer hardware acceleration". The ARM processor runs a non-real-time operating system (such as Linux) and is mainly responsible for trajectory planning, human-computer interaction, and task scheduling, handling non-computationally intensive upper-layer tasks such as host computer communication, robot kinematics forward and inverse kinematics, and human-computer interface responses, avoiding latency caused by complex calculations in general-purpose processors. Simultaneously, a dedicated reconfigurable PID calculation engine is deployed using the FPGA logic resources within the SoC. This engine is specifically responsible for handling computationally intensive multi-axis position and velocity loop algorithms, reducing hardware resource consumption through time-sharing multiplexing mechanisms and solving the problems of computational serial latency and low resource utilization.

[0025] 2. FPGA Driver Layer: Multiple low-cost FPGAs are used as coprocessors to form a distributed driver node. Each FPGA coprocessor is specifically responsible for the underlying current loop FOC algorithm, SVPWM generation, ADC sampling control, and encoder signal processing. Each FPGA can drive multi-axis motors (e.g., each FPGA drives 3 axes in this embodiment). FPGAs are cascaded through a dedicated interface to facilitate axis expansion. The parallel processing characteristics of the FPGA coprocessors can meet the requirements of 80kHz high-frequency updates in the current loop, ensuring the real-time performance of the underlying control. At the same time, the distributed architecture avoids single-point computing power bottlenecks.

[0026] 3. Hardware Interconnect Layer: A dedicated eight-wire parallel peripheral interface (EPPI) protocol is designed as a communication bridge between the SoC main control layer and each FPGA driver layer node, enabling high-speed, deterministic transmission of control commands and feedback data. This interface protocol solves the arbitration latency and topology identification difficulties of traditional buses through topology-aware ID verification, ping-pong buffering mechanism, and dynamic bandwidth allocation, providing low-jitter, high-reliability data transmission guarantees for collaborative work between layers and improving inter-axis synchronization accuracy.

[0027] like Figure 2As shown, to address the characteristics of high computational complexity and repetitiveness in PID control, this invention designs a hardware acceleration engine based on register window multiplexing technology, which is a core technical means to solve the problems of "low hardware resource utilization" and "large computational serial latency". The core design idea of ​​this engine is to avoid allocating fixed computational logic to each axis. Instead, it instantiates a limited number of high-performance Processing Elements (PEs), improving hardware resource utilization through time-sharing multiplexing and leveraging the parallel capabilities of the FPGA to shorten computation latency. The specific working mechanism is as follows: Resource reuse and context switching: The system allocates independent parameter storage space in RAM for all controlled axes, storing the control parameters (position loop PID parameters, velocity loop PID parameters) and state variables (target position, feedback position, target velocity, feedback velocity, integral state, etc.) for each axis. At the beginning of each calculation cycle (125μs), the hardware logic automatically "maps" the parameters of the current target axis to the operation register window of the PE. The PE completes the PID calculation task for that axis within a very short time slice. After the calculation is completed, the result is automatically written back to the corresponding storage space in RAM, and the PE immediately switches to the context of the next axis to start loading and calculating a new set of parameters. This mechanism, while ensuring 32-bit fixed-point high-precision operation, greatly reduces the number of adders and multipliers (e.g., in this embodiment, 3 PEs can handle 9-axis tasks), reduces hardware resource consumption and cost, and solves the problem of hardware resource redundancy in traditional solutions.

[0028] Multi-mode support and pipelined architecture: Each PE has a built-in intelligent state machine (FSM) that supports dynamic switching between three control modes: "position-velocity cascade," "independent position loop," and "independent velocity loop." The state machine automatically plans the data path according to the system configuration. For example, in the "position-velocity cascade" mode, the position loop calculation is executed first, and its output is directly fed into the velocity loop calculation logic as an intermediate variable without CPU intervention, reducing data transmission latency and improving computational efficiency. In the "independent position loop" or "independent velocity loop" modes, the state machine only plans the calculation path for the corresponding single loop, adapting to different control requirements. Meanwhile, the PID processing unit adopts a highly efficient three-stage pipeline microarchitecture design to maximize computational throughput: the first stage is the instruction fetch and data loading stage, which quickly extracts deviations and control parameters from RAM based on state machine instructions; the second stage is the arithmetic operation stage, which uses the underlying hard core to execute the PID algorithm and speed loop feedforward compensation calculation in parallel; the third stage is the limiting and write-back stage, which performs anti-integral saturation processing on the output results and automatically writes the updated integral state back to RAM, thereby realizing hardware-level full-process acceleration from parameter acquisition, core calculation to state update, and further shortening the computational latency.

[0029] The operation of PE is controlled by a finite state machine, and its state transitions are as follows: Figure 6 As shown, the specific states include: IDLE: Waiting for the Sync_Pos trigger signal, in standby mode; LOAD_CTX: Loads the context parameters of the current axis, including control parameters, deviation data, etc. CALC_POS: Performs position loop calculation, and performs PID calculation based on the deviation between the target position and the feedback position; CALC_SPD: Performs speed loop calculation. In cascade mode, it directly receives the output of the position loop as the target speed and performs speed loop PID calculation. UPDATE: Updates the state variables, writing the calculation results and integration state back to RAM; NEXT_AXIS: Determines whether the calculation task for all motors in the group has been completed. If not, it jumps to the LOAD_CTX state, loads the context for the next axis, and starts a new round of calculation.

[0030] like Figure 3 As shown, in order to solve the arbitration delay and topology identification difficulties in multi-node communication of traditional buses, this invention proposes an EPPI communication protocol that supports topology awareness, which is a core technical means to ensure "inter-axis synchronization accuracy": 1. Physical Layer Definition: An 8-bit bidirectional data bus (SDA[7:0]) is used in conjunction with a clock line (SCK), a chip select line (CS_N), and handshake signals (VLD / RD_READY) to support half-duplex communication. A tri-state buffer is used to achieve rapid switching of the bus direction, reducing the latency caused by communication direction switching and ensuring high data transmission efficiency. Compared with traditional SPI or I2C interfaces, the parallel transmission mode significantly improves the data transmission rate.

[0031] 2. Topology Awareness and ID Verification: The protocol frame structure includes a device ID field. Each slave FPGA obtains its local physical ID through board-level hardwired connections or plug-and-play mechanisms, achieving hardware-level binding between physical topology and logical address. During communication, when the SoC master control layer initiates communication, it encapsulates the logical ID (SDA[1:0]) of the target device in the first control byte. All connected FPGAs simultaneously listen to the bus. When the chip select signal (CS_N) is detected to be low, the FPGA parses the logical ID in the control byte and compares it with the local hardware ID in real time. If the IDs match, the FPGA takes over the data bus and prepares to receive subsequent data or send feedback data. If the IDs mismatch or the chip select signal is abnormal, the hardware watchdog will automatically reset the communication state machine to prevent the execution of erroneous instructions, ensuring that even in the case of bus multiplexing, data can be accurately routed to the specified physical node, achieving topology awareness and avoiding the delay caused by traditional bus arbitration.

[0032] 3. Ping-Pong Buffering Mechanism: The EPPI interface employs a dual-buffered queue (Ping-PongBuffer) on both the SoC and FPGA sides. This queue is constructed from dual-port RAM and contains two buffers (BufferA and BufferB) with a depth of 256 bytes. Its operation diagram is shown below. Figure 5 As shown. The specific workflow is as follows: State 0: The SoC's PID engine reads the feedback data from the previous cycle in BufferA for calculation; at the same time, the EPPI bus controller writes the new data received from the FPGA in the current cycle into BufferB; State 1: After the SoC completes the calculation, the buffer pointers are swapped, the PID engine starts reading the new feedback data in BufferB, and the EPPI bus controller writes the new data for the next cycle into BufferA. Switching Trigger: Buffer switching is strictly triggered by the falling edge of the global synchronization pulse to ensure the synchronization of data reading and writing, realize "zero wait" pipeline processing, greatly improve the parallelism of data transmission and processing, and reduce the impact of communication delay on the control cycle.

[0033] 4. Dynamic bandwidth allocation and frame structure: The EPPI protocol supports variable-length frame transmission to adapt to the needs of different industrial control stages. The control frame is defined as follows: Bit[7] is the read / write flag (1: write, 0: read); Bit[6:2] is the data packet length, which can be dynamically adjusted from 1 to 32 bytes; Bit[1:0] is the target motor / device ID. The dynamic bandwidth allocation mechanism is as follows: During the system initialization stage, the SoC can send a configuration frame to set the data content returned by each FPGA (position only, speed only, or full status); During the operation stage, high-frequency current feedback can be configured as a short frame (e.g., 16 bytes) to reduce the amount of data transmission and ensure the transmission speed; low-frequency status monitoring can be configured as a long frame (e.g., 24 bytes) to transmit more comprehensive status information, thereby maximizing the effective data throughput within a limited 125μs period and avoiding delays caused by bandwidth waste.

[0034] like Figure 4 As shown and as Figure 7 As shown, one of the core functions of the FPGA driver layer in this solution is to execute the current loop FOC algorithm, which is crucial to ensuring the "current loop update frequency of 12.5μs". The specific steps are as follows: 1. ADC Sampling and Filtering: The FPGA controls the AD7606 parallel ADC to simultaneously acquire the current signals (Ia, Ib, Ic) of the three-phase motor. The sampling frequency is consistent with the current loop update frequency (80kHz). The acquired raw data enters a moving average filter to filter out high-frequency noise, improve the stability and accuracy of the current signal, and provide a reliable data foundation for subsequent coordinate transformation.

[0035] 2. Clarke and Park Transforms: Trigonometric function calculations are performed using a lookup table method, reducing computational complexity and increasing speed (compared to hardware calculations, the lookup table method reduces clock cycles by more than 50%). First, the Clarke transform converts the current signals (Ia, Ib, Ic) in a three-phase stationary coordinate system (abc) into current signals in a two-phase stationary coordinate system (αβ). Then, combined with the motor rotor electrical angle θ fed back from the magnetic encoder (such as AS5600), the Park transform converts the current signals in the two-phase stationary coordinate system (αβ) into current signals (Id, Iq) in a two-phase rotating coordinate system (dq), achieving decoupling of the motor torque component and excitation component, laying the foundation for independent control.

[0036] 3. PI Regulation: Includes two parallel PI controllers, controlling the d-axis (excitation) and q-axis (torque) respectively. The d-axis current target value (Id_aim) is set to 0 to achieve maximum torque-to-current ratio control, improving motor efficiency. The q-axis current target value (Iq_aim) comes from the EPPI interface and is output by the PID calculation engine of the SoC main control layer. The PI controllers calculate based on the deviation between the target current and the feedback current, outputting voltage commands (Vd, Vq) for the d-axis and q-axis. Furthermore, the PI parameters can be dynamically configured by the SoC via the EPPI interface to adapt to different motor characteristics.

[0037] 4. SVPWM Generator: Implements seven-segment space vector pulse width modulation. Based on the Vd and Vq voltage commands, it calculates the amplitude and angle of the voltage space vector, determines its sector, and then calculates the duration (T0, T1, T2) of adjacent basic space vectors. A comparator generates six PWM waveforms with dead time (e.g., 2μs) to directly drive the inverter MOSFETs, avoiding bridge arm shoot-through risk while precisely controlling the motor's torque and speed.

[0038] As a specific embodiment of this solution, the details are as follows: (I) System Hardware Composition The heterogeneous high real-time multi-axis robot cooperative motion control system in this embodiment consists of a high-performance SoC chip and three slave FPGA chips in its core hardware, with the following specific configuration: 1. SoC Main Control End: Utilizing a Xilinx Zynq UltraScale+ series SoC chip, which integrates an ARM Cortex-A53 multi-core processor and FPGA programmable logic. The processing system (PS) runs a Linux operating system, handling tasks related to host computer communication, robot kinematics forward and inverse kinematics, trajectory planning, and human-machine interface. The programmable logic (PL) end deploys the core PID calculation engine IP core of this invention. This IP core interacts with the PS end via the AXI-Lite bus, receiving target position commands and configuring control parameters, while also communicating with the slave FPGA through the EPPI master controller interface.

[0039] 2. FPGA Slave Terminal: This consists of three identical FPGA driver boards (FPGA0, FPGA1, and FPGA2). Each FPGA driver board is responsible for driving three BLDC motors, and the three FPGAs work together to achieve the drive control of the 9-axis motors. FPGA0 is configured as the master synchronization node, while FPGA1 and FPGA2 are slave nodes. FPGA0 generates a global synchronization pulse, which is distributed to FPGA1, FPGA2, and the SoC master control terminal through a dedicated differential line to ensure timing synchronization of each node. Each FPGA internally instantiates three independent FOC current loop IP cores for parallel processing of the vector control tasks of the corresponding three motors.

[0040] 3. EPPI interface configuration: such as Figure 3 As shown, the EPPI interface signal lines include chip select signals CS2 and CS3, an 8-bit bidirectional data bus (SDA[7:0]), a clock line (SCK), and handshake signals (VLD / RD_READY). The interface protocol supports half-duplex communication and utilizes a tri-state buffer to achieve fast switching of the bus direction, providing hardware support for the transmission of control commands and feedback data between the SoC master and the FPGA slave. The EPPI interface consists of the following signal lines: Signal name Function data[7:0] Data cable vld A high signal indicates that communication is possible. rd_ready Fixed-period communication enable signal cs1 FPGA1 chip select signal cs2 FPGA2 chip select signal cs3 FPGA3 chip select signal .

[0041] (II) System Control Strategy like Figure 1 As shown, this embodiment adopts a hierarchical control strategy, dividing the control loop into a slow loop (motion loop) and a fast loop (current loop), which are executed by the SoC master and the FPGA slave respectively, as detailed below: 1. Slow Loop (Motion Loop): The control frequency is set to 8kHz, corresponding to a control cycle of 125μs, and is executed by the PID calculation engine on the SoC. Its core task is to read the encoder position and speed feedback data of all 9-axis motors, compare the feedback data with the target trajectory planned by the PS, and after PID calculations in the position and speed loops, output the q-axis reference current (Iq_ref) of the 9-axis motors and send it to the corresponding FPGA slave terminal through the EPPI interface.

[0042] 2. Fast Loop (Current Loop): The control frequency is set to 80kHz, corresponding to a control period of 12.5μs, and is executed by the FOCIP core on the FPGA side. For example... Figure 4 As shown, after the FPGA slave receives the Iq_ref sent by the SoC master, it combines the three-phase current data obtained by local ADC sampling and the motor electrical angle fed back by the encoder to execute a vector control algorithm and output a PWM waveform to drive the BLDC motor. This hierarchical control strategy fully utilizes the complex algorithm processing capabilities of the SoC and the high parallelism and low latency characteristics of the FPGA, avoiding the computing power bottleneck of a single-processor architecture.

[0043] (III) Specific Implementation of the EPPI Communication Protocol In this embodiment, the EPPI protocol achieves efficient data transmission between the SoC and FPGA through the following mechanism: 1. Topology Awareness and ID Binding Mechanism: To achieve topology identification without manual hard coding, the EPPI protocol designs a "physical location and logical ID binding" mechanism. When the SoC initiates communication, it encapsulates the logical ID of the target device (SDA[1:0]) in the first control byte, and all connected FPGAs simultaneously listen to the bus. When the chip select signal cs_n is detected to be low, the FPGA parses the logical ID in the control byte and compares it with the local hardware ID. If the IDs match, the FPGA takes over the data bus, ready to receive subsequent data or send feedback data; if the IDs do not match, the FPGA maintains a high impedance state, ignores subsequent clock signals, and ensures that the data is accurately routed to the specified physical node.

[0044] 2. Dynamic bandwidth allocation and frame structure: The EPPI protocol supports variable-length frame transmission to adapt to the needs of different industrial control stages. The control frame is defined as follows: Bit[7] is the read / write flag (1 indicates write, 0 indicates read); Bit[6:2] is the data packet length, which can be dynamically adjusted from 1 to 32 bytes; Bit[1:0] is the target motor / device ID. The dynamic bandwidth allocation mechanism is specifically manifested as follows: During the system initialization stage, the SoC can send a configuration frame to set the data content returned by each FPGA (position only, speed only, or full status); During the operation stage, high-frequency current feedback is configured for short frame transmission, and low-frequency status monitoring is configured for long frame transmission, thereby maximizing the effective data throughput within a limited 125μs period.

[0045] 3. Ping-pong double-buffered data stream mechanism: such as Figure 5 As shown, to achieve "zero-wait" pipelined processing, the EPPI controller integrates a Ping-Pong Buffer built from dual-port RAM, containing two buffers (BufferA and BufferB) with a depth of 256 bytes. The workflow consists of two states: In state 0, the SoC's PID engine reads the feedback data from the previous cycle in BufferA for calculation, while the EPPI bus controller writes the new data received from the FPGA in the current cycle into BufferB; in state 1, after the SoC completes the calculation, the buffer pointers are swapped, the PID engine reads the new data from BufferB, and the EPPI bus controller writes the new data for the next cycle into BufferA. Buffer switching is strictly triggered by the falling edge of the global synchronization pulse, ensuring the synchronization of data read and write operations.

[0046] (iv) Specific implementation of the PID calculation engine like Figure 2 As shown, in this embodiment, the PID calculation engine achieves efficient control of the 9-axis motor through the following design: 1. Hardware Architecture Configuration: To achieve 9-axis parallel control with limited FPGA resources, this embodiment adopts a CPU-like "context switching" hardware architecture, instantiating only 3 physical PID processing units (PE0, PE1, PE2). PE0 is responsible for the calculation tasks of motors 1-3, PE1 for motors 4-6, and PE2 for motors 7-9. The system defines a unified parameter storage array to store 9 sets of control parameters and state variables, allocating independent parameter storage space for each controlled axis.

[0047] 2. Time-slice rotation mechanism: Since the FPGA's clock frequency (200MHz) is much higher than the control frequency (8kHz), a time-slice rotation mechanism is used to achieve "parallel" processing of the 9 axes. At time T0, the hardware scheduler loads the parameters of motor 1 from the storage array into the working register window of PE0, and PE0 performs a single-cycle multiply-accumulate operation; at time T1, PE0 writes the calculation result back to the parameter storage array and immediately loads the parameters of motor 2 into the same group of working register windows, completing the calculation tasks of all motors in the group in sequence.

[0048] 3. Three-stage pipelined microarchitecture and state machine control: The PID processing unit adopts a highly efficient three-stage pipelined microarchitecture design to maximize computational throughput. The first stage is the instruction fetch and data loading stage, which quickly extracts deviations and control parameters from RAM based on state machine instructions; the second stage is the arithmetic operation stage, which uses the underlying hard core to execute the PID algorithm and speed loop feedforward compensation calculation in parallel; the third stage is the limiting and write-back stage, which performs anti-integral saturation processing on the output results and automatically writes the updated integral state back to RAM, realizing hardware-level full-process acceleration from parameter acquisition, core calculation to state update.

[0049] like Figure 6 As shown, the operation of PE is controlled by a finite state machine, which includes the following states: IDLE (waiting for the Sync_Pos trigger signal), LOAD_CTX (loading the current axis context), CALC_POS (performing position loop calculation), CALC_SPD (performing speed loop calculation; in cascade mode, the output of CALC_POS is directly connected to the input of CALC_SPD), UPDATE (updating state variables), and NEXT_AXIS (determining whether the calculation of all motors in the group is complete; if not, jumping to LOAD_CTX to load the next axis).

[0050] (V) Specific Implementation of FPGA Current Loop FOC Algorithm like Figure 7 As shown, the FOC current loop IP core on the FPGA side performs vector control according to the following steps: 1. ADC Sampling and Filtering: The FPGA controls the AD7606 parallel ADC to simultaneously acquire three-phase current data. The acquired data enters the moving average filter to filter out high-frequency noise and provide a stable current signal for subsequent coordinate transformation.

[0051] 2. Clarke and Park Transformation: Using a lookup table method, trigonometric function calculations are performed to transform the current in a three-phase stationary coordinate system (abc) into the current in a two-phase rotating coordinate system (dq). During the transformation, the motor rotor electrical angle θ fed back from the magnetic encoder is used for real-time rotational transformation, achieving decoupling of the motor torque component and the excitation component.

[0052] 3. PI Regulation: The FOCIP core contains two parallel PI controllers, controlling the d-axis (excitation) and q-axis (torque) respectively. The d-axis current target value (Id_aim) is set to 0 to achieve maximum torque-to-current ratio control; the q-axis current target value (Iq_aim) comes from Iq_ref received via the EPPI interface. The PI controllers calculate the deviation between the target current and the feedback current, outputting voltage commands (Vd, Vq) for the d-axis and q-axis.

[0053] 4. SVPWM Generation: Implements seven-segment space vector pulse width modulation. Based on the Vd and Vq voltage commands, it determines the sector to which the voltage space vector belongs, calculates the duration (T0, T1, T2) of adjacent basic vectors, and generates 6 PWM waveforms with dead time through a comparator to directly drive the inverter MOSFET and control the operation of the BLDC motor.

[0054] The above description is merely illustrative of the embodiments of the present invention and is not intended to limit the present invention. For those skilled in the art, any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the protection scope of the present invention.

Claims

1. A heterogeneous, high real-time multi-axis robot cooperative motion control system, characterized in that, It includes a SoC main control layer, an FPGA driver layer and a hardware interconnection layer. The hardware interconnection layer connects the SoC main control layer and the FPGA driver layer. The layers work together to achieve microsecond-level real-time closed-loop control. The SoC main control layer adopts a heterogeneous SoC chip containing an ARM processor and FPGA logic resources. The ARM processor runs a non-real-time operating system and is responsible for trajectory planning, human-computer interaction and task scheduling. The FPGA logic resources deploy a reconfigurable PID calculation engine based on register window reuse technology. By time-division multiplexing a limited number of high-performance PE processing units, the multi-axis position loop and velocity loop algorithms are polled and processed, reducing the hardware arithmetic unit occupation. The FPGA driver layer includes multiple FPGA coprocessors. Each FPGA coprocessor instantiates an independent FOC current loop IP core, which is responsible for the underlying current loop FOC algorithm, SVPWM generation, ADC sampling control and encoder signal processing. Each FPGA coprocessor can drive a multi-axis motor, and the number of axes can be extended by cascading the FPGA coprocessors through a dedicated interface. The hardware interconnect layer adopts an eight-line parallel peripheral interface protocol that supports topology awareness and ping-pong buffering mechanism to realize high-speed, deterministic transmission of control commands and feedback data between the SoC main control layer and each of the FPGA coprocessors, reducing communication latency and jitter.

2. The heterogeneous high real-time multi-axis robot cooperative motion control system according to claim 1, characterized in that, The reconfigurable PID calculation engine allocates independent parameter storage space in RAM for all controlled axes. At the start of the calculation cycle, the hardware logic automatically maps the parameters of the current target axis to the operation register window of the PE processing unit. After the calculation is completed, the result is automatically written back, and the PE processing unit immediately switches to the context of the next axis to ensure 32-bit fixed-point high-precision operation.

3. The heterogeneous high real-time multi-axis robot cooperative motion control system according to claim 2, characterized in that, Each of the PE processing units has a built-in intelligent state machine that supports dynamic switching between three control modes: "position-velocity cascade", "independent position loop", and "independent velocity loop". In the "position-velocity cascade" mode, the position loop calculation output is directly used as the input of the velocity loop without the intervention of the ARM processor.

4. The heterogeneous high real-time multi-axis robot cooperative motion control system according to claim 1, characterized in that, The physical layer of the eight-line parallel peripheral interface protocol uses an 8-bit bidirectional data bus, along with a clock line, chip select line, and handshake signal, to support half-duplex communication and achieve fast switching of bus direction through a tri-state buffer.

5. A heterogeneous high real-time multi-axis robot cooperative motion control system according to claim 4, characterized in that, The frame structure of the eight-line parallel peripheral interface protocol includes a device ID field. Each FPGA coprocessor obtains its local physical ID through board-level hardwired connections or a plug-and-play mechanism. During the communication handshake phase, the FPGA coprocessor hardware compares the received frame ID with the local ID in real time, and only responds to bus operations when the IDs match. If an ID mismatch or chip select signal abnormality is detected, the hardware watchdog automatically resets the communication state machine.

6. A heterogeneous high real-time multi-axis robot cooperative motion control system according to claim 1, characterized in that, The ping-pong buffering mechanism is implemented through a double buffer queue. When the PID calculation engine processes the Nth frame of data, the eight-line parallel peripheral interface bus transmits the N+1th frame of data under the background DMA drive, realizing "zero-wait" pipeline processing.

7. A heterogeneous high real-time multi-axis robot cooperative motion control system according to claim 1, characterized in that, The implementation process of the FOC current loop IP core includes: acquiring three-phase current through an AD7606 parallel ADC, filtering it through a moving average filter, and then using a lookup table method to perform Clarke and Park transformations to convert the current in the abc coordinate system to the current in the dq coordinate system; two parallel PI controllers control the d-axis and q-axis respectively, with the target value of I_d set to 0 and the target value of I_q coming from the EPPI interface; and an SVPWM generator to implement seven-segment modulation and generate six PWM waveforms with dead time.

8. A heterogeneous high real-time multi-axis robot cooperative motion control system according to claim 1, characterized in that, In the FPGA driver layer, one FPGA coprocessor is designated as the master synchronization node, and the rest are slave nodes; the master synchronization node generates a global synchronization pulse, which is distributed to each slave node and the SoC master control layer through a dedicated differential line.

9. A heterogeneous high real-time multi-axis robot cooperative motion control system according to claim 1, characterized in that, The eight-line parallel peripheral interface protocol supports variable-length frame transmission. Bit[7] of the control frame is a read / write flag, Bit[6:2] is the adjustable data packet length of 1-32 bytes, and Bit[1:0] is the target motor / device ID. During the system initialization phase, the return data content of each FPGA coprocessor can be configured. During the operation phase, high-frequency current feedback is transmitted through short frames, and low-frequency status monitoring is transmitted through long frames to maximize the effective data throughput.

10. A heterogeneous, high real-time multi-axis robot cooperative motion control method, applied to the control system described in any one of claims 1-9, characterized in that, Includes the following steps: S1: System initialization, completes topology identification and ID binding between the SoC main control layer and each FPGA coprocessor through the eight-line parallel peripheral interface protocol, and configures the number of PE processing units, control mode and FOC current loop parameters of the PID calculation engine; S2: The ARM processor of the SoC main control layer performs trajectory planning and task scheduling to generate multi-axis target trajectories; the reconfigurable PID calculation engine in the FPGA logic resources uses the time-division multiplexing PE processing unit to poll and load parameters of each axis, execute position loop and velocity loop algorithms, and output q-axis reference current. S3: The FPGA coprocessor receives the q-axis reference current through the EPPI protocol, combines the local ADC sampling data with the encoder electrical angle, executes the FOC algorithm, and sequentially completes current filtering, coordinate transformation, PI adjustment and SVPWM generation to drive the motor. S4: The FPGA coprocessor collects motor position and speed feedback data and transmits it back to the SoC main control layer through the ping-pong buffer mechanism of the eight-line parallel peripheral interface protocol. S5: Repeat steps S2-S4 to ensure inter-axis synchronization through global synchronization pulses, thereby achieving microsecond-level real-time closed-loop collaborative control of the multi-axis robot.

Images