Design Method and Control System of a Six-Degree-of-Freedom Parallel Platform Controller Based on Zynq and HLS

Abstract

The present invention discloses a design method and control system for a six-degree-of-freedom parallel platform controller based on Zynq and HLS. In the Zynq platform, the inverse kinematics algorithm for the hydraulic rod lengths of the six-degree-of-freedom parallel platform is designed using the HLS language. The logic resources of the PL side in Zynq are utilized to solve the problems of large computational workload and high parallelization requirements for the inverse kinematics algorithm and control module of the six-degree-of-freedom parallel platform. A TCP data frame and command frame transmission scheme based on an Ethernet interface is implemented using the PS side of Zynq, integrating parameter configuration and data transmission on the same Zynq chip, improving the integration degree of the controller and its stability in complex working environments, and solving the problems of high power consumption and large volume in traditional control schemes. The ap_fixed data type of HLS is used to reduce the time overhead in inverse kinematics calculations, improve the running speed of the inverse kinematics algorithm, and reduce the solution delay.

Description

Technical Field

[0001] The present invention belongs to the field of motion controllers, and particularly relates to a six-degree-of-freedom parallel platform controller based on Zynq and HLS. Background Art

[0002] With the advent of the intelligent era, six-degree-of-freedom parallel platforms are applied in more and more scenarios. In industrial scenarios, due to their high degrees of freedom of motion and large load capacity, they are widely used for simulating sea waves or bumpy roads. They are also often used in the aerospace industry to simulate various driving situations encountered by pilots or astronauts. They also have extensive applications in the civilian field, such as being linked with VR devices to achieve multi-dimensional virtual reality perception and immersive experience.

[0003] The core algorithm of the six-degree-of-freedom parallel platform is the inverse kinematics algorithm. A stable and fast inverse kinematics algorithm is the key to the smooth and efficient operation of the six-degree-of-freedom parallel platform. The inverse kinematics algorithm calculates the elongation or shortening amounts of six hydraulic rods by giving the displacements and deflection angles of the three axes of the moving platform, which involves coordinate transformation and matrix multiplication operations. The process is relatively complex and the six axes need to be solved and controlled synchronously, requiring high computing power of the computing device and efficient and diverse communication interfaces with peripheral devices. Existing inverse kinematics algorithms often run on embedded devices or industrial computer platforms.

[0004] Traditional six-degree-of-freedom parallel platforms usually use the method of industrial computers to collect and generate control signals, requiring a relatively large device volume and high power consumption. The solution algorithm running on the industrial computer platform needs to communicate with the controller through Ethernet, CAN bus, etc. to collect the data of the hydraulic rod length sensors and transmit the calculation results. The working environment of the six-degree-of-freedom parallel platform is complex and changeable, especially in the wild or relatively harsh environments, requiring the controller to be small in size and high in integration.

[0005] As the core algorithm of the six-degree-of-freedom parallel platform, the inverse kinematics algorithm has high requirements for the real-time performance of calculations. Using an industrial computer as the computing device requires the support of complex external devices, resulting in a decrease in the integrity of the entire system and lower stability when running in complex environments. Moreover, it requires relatively high power consumption and has certain requirements for the operating environment. Therefore, it is necessary to design a six-degree-of-freedom parallel platform controller with high integration and rich external interfaces. Summary of the Invention

[0006] The purpose of the present invention is to propose a design method and control system of a six-degree-of-freedom parallel platform controller based on Zynq and HLS, which can quantify the solution accuracy, in view of the shortcomings of the current six-degree-of-freedom parallel platform motion industrial computer controller, such as large volume, high power consumption and low integration. The present invention designs a six-degree-of-freedom platform hydraulic rod length inverse solution algorithm on the Zynq xc7z100ffg900-2 platform by using the HLS language, and solves the problems of large computational complexity and high parallelization requirements of the six-degree-of-freedom parallel platform inverse solution algorithm and control module by using the logic resources of the PL end in Zynq; a TCP data frame and command frame transmission scheme based on the Ethernet interface is realized by using the Zynq PS end, and parameter configuration and data transmission are integrated in the same Zynq chip, thereby improving the integration of the controller and the stability in a complex working environment, and solving the problems of high power consumption and large volume of the traditional control scheme; and the ap_fixed data type of HLS is used to reduce the time overhead in the inverse solution calculation, improve the running speed of the inverse solution algorithm, and reduce the solution delay.

[0007] The objective of the present invention is achieved through the following technical solutions:

[0008] According to a first aspect of the present invention, a method for designing a six-degree-of-freedom parallel platform controller based on Zynq and HLS is provided, the method comprising the following steps:

[0009] Step 1: HLS implements the inverse algorithm IP of the six-degree-of-freedom parallel platform; based on the spatial coordinate transformation of the tabletop center of mass coordinate system and the base center of mass coordinate system, the lengths of the six hydraulic rods of the six-degree-of-freedom parallel platform are solved according to the expected posture of the tabletop and the platform structure parameters; based on the optimization method of HLS, the data throughput of IP calculation is improved by inserting pipeline design to reduce the solution delay; fixed-point numbers with different bit widths are used to reduce the solution complexity and improve the solution speed while ensuring the solution accuracy;

[0010] Step 2: Ethernet data transmission module design, including the following sub-steps:

[0011] 2.1 TCP connection status detection timer is initialized. The timer periodically monitors whether the TCP connection exists. If the connection is detected to be disconnected, the connection is reestablished.

[0012] 2.2 Development of LwIP library, design of callback functions for sending and receiving data, design of transmission function for transmitting the six hydraulic rod length data sampled by the sensor to the upper computer, and configuration of the target address port;

[0013] 2.3 Transmission frame format design, by giving different meanings to different bits of the transmission frame, the host computer and controller can distinguish between command frames and data frames;

[0014] 2.4 Design of the data frame length, design the bit width of the sensor sampling points and the number of sampling points included in each data frame, and reduce the transmission waiting time by means of ping-pong operation on the transmission buffer;

[0015] Step 3, Design of the AXI bus data transmission module between the PS and the PL, including the following sub-steps:

[0016] 3.1 Design the HLS AXI-Lite interface at the PL end, and receive the parameters of the incremental PID module transmitted from the PS end through the registers in the AXI-Lite IP;

[0017] 3.2 Design and configure the AXI-Stream IP related to the sensor data transmission from the PL end to the PS end, batch transfer the sensor sampling data back to the PS end from the FIFO and generate the PS end interrupt signal;

[0018] Step 4, Design of the hydraulic rod length control module, including the following sub-steps;

[0019] 4.1 Design the drive of the external SSI length sensor and the DAC servo valve control module;

[0020] 4.2 Sensor data format conversion and fixed-point to floating-point conversion;

[0021] 4.3 Verilog implementation of the incremental PID module.

[0022] Furthermore, in the said Step 1, the HLS realizes the IP design of the inverse solution algorithm of the six-degree-of-freedom parallel platform, and the data transmission and external drive include: TCP data transmission module design, AXI-Lite interface and AXI-Stream data transmission interface design, incremental PID control module design, SSI length sensor drive module design, DAC drive module design.

[0023] Furthermore, in the said Step 1, the design process of the IP of the inverse solution algorithm of the six-degree-of-freedom parallel platform is as follows:

[0024] a) The inverse solution algorithm IP calculates the corresponding sine and cosine values according to the incoming angle information of the moving tabletop and then substitutes them into the pose transformation matrix The functional relationships between the angles in the x, y, and z directions and the three pose transformation matrices are as follows:

[0025]

[0026]

[0027]

[0028] b) The angular pose change matrix tfBP between the tabletop coordinate system and the base coordinate system is obtained by multiplying the pose transformation matrices in the x, y, and z directions.

[0029] tf BP = rot z (orints[3]) * rot y (orints[2]) * rot x (orints[1])

[0030] Among them, orints[1], orints[2], and orints[3] are the rotation angles of the moving tabletop around the x, y, and z axes respectively.

[0031] c) Through the angular pose transformation matrix tf BP formed by the pose information of the moving tabletop, the displacements trans in the x, y, and z directions of the moving tabletop, the vector height from the origin of the base coordinate system to the origin of the tabletop coordinate system in the initial case, the coordinates joint plat [i] of the connection point between the hydraulic rod and the tabletop, and the coordinates joint base [i] of the connection point between the hydraulic rod and the base, calculate the coordinate values of the connection points between the moving tabletop and the hydraulic rods in the base coordinate system, and then calculate the spatial vectors leg[i] of the six hydraulic rods in the base coordinate system, where i ∈ [1, 6] represents the six hydraulic rods.

[0032] leg[i] = trans + height + tf BP * joint plat [i] - joint base [i]

[0033] After taking the modulus of the leg[i] vector, the length of the hydraulic rod |lrg[i]| is obtained.

[0034] Furthermore, in step b), the matrix multiplication operation requires multiple parallel accesses to the data in the pose transformation matrix. The matrix variables are added with the ARRAY_PARTITION data partitioning implementation method in HLS, and the left and right matrices to be multiplied are respectively block-stored by rows and columns to achieve parallel calculation of the multiplication operation, thereby optimizing the parallelism of data access.

[0035] Furthermore, in step one, considering the speed requirement of the solution, HLS supports customizing the total bit width and the fractional bit width of the fixed-point number. The total bit width of the fixed-point number is reduced to 16 bits - 32 bits through the HLS ap_fixed custom data type, and the fractional bit width is set to half of the total bit width to improve the solution speed while ensuring the accuracy.

[0036] Further, in the second step, the design of the Ethernet data transmission module is specifically as follows:

[0037] a) After the controller is powered on, initialization operations are performed, including initialization of the TCP connection status detection timer, GPIO initialization, initialization of LwIP-related parameters, and initialization of the DMA interrupt function;

[0038] b) The controller listens for the IP address and port of the host computer. The host computer transmits a command frame to the PS side of the controller through a TCP connection. When the host computer instruction is received, the callback function for receiving data is called, and then the host computer instruction is further judged;

[0039] c) The attribute of the current command frame is judged through the first byte of the command frame; if it is 0x01, the current command frame is to reset the platform structure parameters; if it is 0x00, the current command frame is a pose frame for transmitting the desired pose of the tabletop, that is, the pose frame includes the desired displacements of the moving tabletop in the x, y, and z directions and the rotations of the moving tabletop around the x, y, and z axes, roll, pitch, and yaw; for offline operations without the host computer, the stored desired trajectory pose data is read from the EMMC storage module;

[0040] d) After confirming that the current command frame is a pose frame, the PS side passes the desired pose vector into the inverse kinematics algorithm IP through the AXI-Lite protocol by calling Xil_Out32, and then starts the inverse kinematics calculation by calling the start function XSteback_Start;

[0041] e) For the part of the inverse kinematics result returned to the host computer, it is detected whether the inverse kinematics calculation is completed through the status detection function XSteback_IsDone; after the inverse kinematics calculation is completed, the desired hydraulic rod length is obtained, and the desired hydraulic rod length is stored in the first 24 bytes of the LwIP TCP transmission area;

[0042] f) The actual hydraulic rod length data of 256 bytes, a total of 64 sampling points, sampled by the SSI length sensor is passed into the LwIP TCP transmission area after the 24 bytes of the desired hydraulic rod length in the PS side through AXI-DMA;

[0043] g) When both the desired hydraulic rod length and the actual hydraulic rod length sequence are written into the LwIP TCP transmission area, it is confirmed through the designed transmission function that the length of the data to be sent is less than the remaining available length of the send buffer, and TCP transmission is started. The current transmission sampling number and the bit width of each sampling point are written into the header of the TCP data frame. When a batch of data is sent to the host computer, the callback function for sending data is called to count the amount of data sent.

[0044] Further, the specific content of step 3.1 is as follows:

[0045] a) Customize the AXI-Lite IP, define the AXI-Lite bit width as 32 bits, and there are a total of 4 registers used to configure the incremental PID parameters. Initialize and define the AXI_M interface connected to the PS side, and assign corresponding addresses to the AXI-Lite IP;

[0046] b) The 4 registers store the P parameter, I parameter, D parameter, and calculation step of the PID respectively. The PS side writes these parameters into the instantiated BRAM through the AXI-Lite IP. The incremental PID module calculates the output of the incremental PID by reading these parameters and combining the expected hydraulic rod length and the actual hydraulic rod length.

[0047] Further, the specific step 3.2 is as follows:

[0048] a) The PS side enables the AXI-HP interface, declares the DMA_DEVICE_ID, and initializes the DMA interrupt;

[0049] b) After widening the real-time hydraulic rod length value sampled by the SSI length sensor to 32 bits, write it into the FIFO with a depth of 64, and start the DMA transfer when the FIFO full signal is detected;

[0050] c) After the PS side receives the interrupt of the DMA transfer completion, perform sliding filtering on the data of the SSI length sensor received by the PS side and store it in the TCP send buffer.

[0051] Further, the specific step four is as follows:

[0052] a) The actual hydraulic rod length sensor acquisition part is connected to the SSI length sensor on the hydraulic rod through the SSI interface, and the serial data sampled by the SSI interface is converted from serial to parallel and then sliding filtered to obtain the current rod length value;

[0053] b) The control part adopts the incremental PID method. The expected lengths of the six hydraulic rods are the values obtained by the previous inverse kinematics algorithm IP. Substitute the error value calculated by the real-time sampled rod length value, the sampling data obtained in the previous two sampling periods, and the expected value into the incremental PID formula to obtain the output control amount;

[0054] c) Through the state machine model, convert the control amount obtained in the previous step into SPI interface signals and output them to the DAC. The DAC converts the control signal into an analog electrical signal of the hydraulic servo valve to control the opening of the hydraulic servo valve, and further control the elongation or shortening length of the hydraulic rod.

[0055] According to the second aspect of the present invention, a six-degree-of-freedom parallel platform control system based on Zynq and HLS is provided. This system includes the following modules:

[0056] Inverse kinematics algorithm module for 6-DOF parallel platform: Implement the inverse kinematics IP of the 6-DOF parallel platform by HLS; Based on the spatial coordinate transformation between the tabletop centroid coordinate system and the base centroid coordinate system, solve the lengths of the six hydraulic rods of the 6-DOF parallel platform according to the desired pose of the tabletop and the platform structure parameters; Based on the optimization method of HLS, insert pipelines and ARRAY_PARTITION to improve the data throughput of the inverse kinematics algorithm operation and reduce the solution delay; Use fixed-point numbers with different bit widths to reduce the solution complexity and improve the solution speed while ensuring the solution accuracy.

[0057] Ethernet data transmission module: Include the initialization of the TCP connection status detection timer; Design of the LwIP TCP send data and receive data callback functions, design of the TCP transmission function, and configuration of the target address port; Design of the transmission frame format, distinguish the command frame and the data frame between the host computer and the controller by assigning different meanings to different bits of the transmission frame; Design of the data frame length.

[0058] Zynq PS PL data transmission and external device drive module, including the AXI bus data transmission module and the hydraulic rod length control module between PS and PL;

[0059] AXI bus data transmission module between PS and PL: Design the HLS AXI-Lite interface at the PL end to receive the parameters of the incremental PID module; Design and configure the AXI-Stream IP related to sensor data transmission from the PL end to the PS end;

[0060] Hydraulic rod length control module: Design of the hydraulic rod incremental PID control module, design of the drive module for the external SSI length sensor; Sensor data format conversion and fixed-point floating-point conversion; Verilog implementation of the incremental PID algorithm; Design of the drive module for the external DAC.

[0061] The beneficial effects of the present invention are as follows:

[0062] 1. The controller designed in the present invention is integrated on a single PCB and has the complete functions of data acquisition and control signal generation. After saving the desired motion trajectory in the EMMC storage module of the controller, it can work independently from the host computer platform. It is more suitable for the working environment of the 6-DOF parallel platform.

[0063] 2. The present invention uses the Zynq ARM+FPGA processor chip, and the control algorithms all run on the FPGA part using digital logic resources. Compared with the serial operation of traditional embedded devices, the parallel operation advantage of the Zynq PL part is more suitable for the multi-axis parallel characteristics of the 6-DOF parallel platform.

[0064] 3. Through the parallel operation of FPGA and the pipelined processing logic, the running speed of the inverse solution algorithm can be increased by about 10 times, shortening the control cycle and reducing the control delay.

[0065] 4. Through the design methods of quantization accuracy, parameter configuration registers and TCP control frames, the present invention realizes that the PID parameters, platform mechanical structure parameters and solution accuracy can all be reconfigured, thus having good universality for six-degree-of-freedom parallel platforms with different mechanical structures. BRIEF DESCRIPTION OF THE DRAWINGS

[0066] Figure 1 It is the software part flow block diagram provided by the embodiment of the present invention.

[0067] Figure 2 It is the hardware part connection interface block diagram provided by the embodiment of the present invention.

[0068] Figure 3 It is the solution accuracy under different fixed-point number precisions provided by the embodiment of the present invention.

[0069] Figure 4 It is the actual path tracking test result provided by the embodiment of the present invention. DETAILED DESCRIPTION OF THE EMBODIMENTS

[0070] To make the above objects, features and advantages of the present invention more obvious and understandable, the following detailed description of the specific embodiments of the present invention is made in conjunction with the drawings.

[0071] In the following description, many specific details are set forth in order to fully understand the present invention. However, the present invention can also be implemented in other ways different from those described herein. Those skilled in the art can make similar promotions without departing from the connotation of the present invention. Therefore, the present invention is not limited by the specific embodiments disclosed below.

[0072] The present invention provides a design method for a six-degree-of-freedom parallel platform controller based on Zynq and HLS, including the following steps:

[0073] Step 1, the inverse solution algorithm IP of the six-degree-of-freedom parallel platform is implemented by HLS; based on the spatial coordinate transformation between the tabletop centroid coordinate system and the base centroid coordinate system, according to the desired pose of the tabletop and the platform structure parameters, the lengths of the six hydraulic rods of the six-degree-of-freedom parallel platform are solved; based on the optimization method of HLS, the data throughput of the IP operation is improved by inserting a pipeline design, and the solution delay is reduced; different bit-width fixed-point numbers are used to reduce the solution complexity and improve the solution speed while ensuring the solution accuracy.

[0074] Step 2, the design of the Ethernet data transmission module, including the following sub-steps:

[0075] 2.1 Initialize the TCP connection status detection timer, periodically monitor whether the TCP connection exists through the timer, and re - establish the connection if the connection is detected to be disconnected;

[0076] 2.2 Develop the LwIP library, design the relevant callback functions for sending and receiving data, design the transmission function for transmitting the six hydraulic rod lengths sampled by the sensor to the host computer, and configure the target address port;

[0077] 2.3 Design the transmission frame format, distinguish the command frame and data frame between the host computer and the controller by assigning different meanings to different bits of the transmission frame;

[0078] 2.4 Design the data frame length, design the bit width of the sensor sampling point and the number of sampling points included in each data frame, and reduce the transmission waiting time through the ping - pong operation of the transmission buffer.

[0079] Step three, design the AXI bus data transmission module between PS and PL, including the following sub - steps:

[0080] 3.1 Design the PL - side HLS AXI - Lite interface, and receive the parameters of the incremental PID module transmitted from the PS - side through the registers in the AXI - Lite IP;

[0081] 3.2 Design and configure the AXI - Stream IP related to sensor data transmission from the PL - side to the PS - side, batch transfer the sensor sampling data back to the PS - side from the FIFO and generate the PS - side interrupt signal.

[0082] Step four, design the hydraulic rod length control module, including the following sub - steps;

[0083] 4.1 Design the drive of the external SSI length sensor and the DAC servo valve control module;

[0084] 4.2 Sensor data format conversion and fixed - point to floating - point conversion;

[0085] 4.3 Verilog implementation of the incremental PID module.

[0086] In one embodiment, the design process of the inverse kinematics algorithm IP of the six - degree - of - freedom parallel platform is as follows:

[0087] a) The inverse kinematics algorithm IP calculates the corresponding sine and cosine values based on the incoming angle information of the moving platform surface and substitutes them into the pose transformation matrix The functional relationships between the angles in the x, y, and z directions and the three pose transformation matrices are as follows:

[0088]

[0089]

[0090]

[0091] b) The angular pose change matrix tf between the tabletop coordinate system and the base coordinate system is obtained by multiplying the pose transformation matrices in the x, y, and z directions. BP ;

[0092] tf BP = rot z (orints[3]) * rot y (orints[2]) * rot x (orints[1])

[0093] where orints[1], orints[2], and orints[3] are the rotation angles of the moving tabletop around the x, y, and z axes respectively;

[0094] c) Through the angular pose transformation matrix tf formed by the pose information of the moving tabletop BP and the displacements trans in the x, y, and z directions of the moving tabletop, the vector height from the origin of the base coordinate system to the origin of the tabletop coordinate system in the initial state, the coordinates joint plat [i] of the connection point between the hydraulic rod and the tabletop, and the coordinates joint base [i] of the connection point between the hydraulic rod and the base, calculate the coordinate values of the connection points between the moving tabletop and the hydraulic rods in the base coordinate system, and then calculate the spatial vectors lrg[i] of the six hydraulic rods in the base coordinate system, where i ∈ [1, 6] represents the six hydraulic rods;

[0095] leg[i] = trans + height + tf BP * joint plat [i] - joint base [i]

[0096] After taking the modulus of the leg[i] vector, the length of the hydraulic rod |lrg[i]| is obtained.

[0097] d) In the matrix multiplication operation in step b), data in the pose transformation matrix needs to be accessed in parallel multiple times. Add the ARRAY_PARTITION data partitioning implementation method in HLS to the matrix variables, and store the left and right matrices to be multiplied in a block-by-block manner according to rows and columns respectively to achieve parallel calculation of the multiplication operation, thereby optimizing the parallelism of data access.

[0098] e) Considering the speed requirement of the solution, HLS supports customizing the total bit width and the fractional part bit width of fixed-point numbers. By using the HLS ap_fixed custom data type, the total bit width of the fixed-point number is reduced to 16 bits - 32 bits, and the fractional part bit width is set to half of the total bit width, which improves the solution speed while ensuring the accuracy.

[0099] In one embodiment, the design of the Ethernet data transmission module is specifically as follows:

[0100] a) After the controller is powered on, initialization operations are performed, including the initialization of the TCP connection status detection timer, GPIO initialization, initialization of LwIP-related parameters, and initialization of the DMA interrupt function;

[0101] b) The controller listens for the IP address and port of the host computer. The host computer transmits a command frame to the PS side of the controller through a TCP connection. When the host computer instruction is received, the callback function for receiving data is called, and then the host computer instruction is further judged;

[0102] c) Judge the attribute of the current command frame through the first byte of the command frame; if it is 0x01, the current command frame is to reset the platform structure parameters; if it is 0x00, the current command frame is a pose frame for transmitting the desired pose of the tabletop, that is, the pose frame includes the desired displacements of the moving tabletop in the x, y, and z directions and the rotations of the moving tabletop around the x, y, and z axes, roll, pitch, and yaw; for offline operations without the host computer, read the stored desired trajectory pose data from the EMMC storage module;

[0103] d) After confirming that the current command frame is a pose frame, the PS side passes the desired pose vector into the inverse kinematics algorithm IP through the AXI-Lite protocol by calling Xil_Out32, and then starts the inverse kinematics calculation by calling the startup function XSteback_Start;

[0104] e) For the part of the inverse kinematics result returned to the host computer, detect whether the inverse kinematics calculation is completed through the status detection function XSteback_IsDone; after the inverse kinematics calculation is completed, the desired hydraulic rod length is obtained, and the desired hydraulic rod length is saved in the first 24 bytes of the LwIP TCP transmission area;

[0105] f) Through AXI-DMA, the actual hydraulic rod length data of 256 bytes, a total of 64 sampling points, sampled by the SSI length sensor is passed into the LwIP TCP transmission area after the 24 bytes of the desired hydraulic rod length in the PS side;

[0106] g) After both the expected hydraulic rod length and the actual hydraulic rod length sequence are written into the LwIP TCP transmission area, confirm that the length of the data to be sent is less than the remaining available length of the transmission buffer through the designed transmission function, start the TCP transmission, write the current number of transmission sampling points and the bit width of each sampling point into the header of the TCP data frame, and call the callback function for sending data after sending a batch of data to the host computer to count the amount of data sent.

[0107] In one embodiment, the design of the AXI bus data transmission module between the PS and the PL is specifically as follows:

[0108] a) Customize the AXI-Lite IP, define the AXI-Lite bit width as 32 bits, and use a total of 4 registers to configure the incremental PID parameters. Initialize and define the AXI_M interface connected to the PS side, and assign the corresponding address to the AXI-Lite IP.

[0109] b) The 4 registers store the P parameter, I parameter, D parameter, and calculation step of the PID respectively. The PS side writes these parameters into the instantiated BRAM through the AXI-Lite IP. The incremental PID module calculates the output of the incremental PID by reading these parameters and combining the expected hydraulic rod length and the actual hydraulic rod length.

[0110] c) The PS side enables the AXI-HP interface, declares the DMA_DEVICE_ID, and initializes the DMA interrupt.

[0111] d) After widening the real-time hydraulic rod length value sampled by the SSI length sensor to 32 bits, write it into the FIFO with a depth of 64, and start the DMA transmission when the FIFO full signal is detected.

[0112] e) After the PS side receives the interrupt of the DMA transmission completion, perform sliding filtering on the data of the SSI length sensor received by the PS side and store it in the TCP transmission buffer.

[0113] In one embodiment, the design of the hydraulic rod length control module is specifically as follows:

[0114] a) The actual hydraulic rod length sensor acquisition part is connected to the SSI length sensor on the hydraulic rod through the SSI interface, and the serial data sampled by the SSI interface is converted from serial to parallel and filtered by sliding to obtain the current rod length value.

[0115] b) The control part adopts the incremental PID method. The expected lengths of the six hydraulic rods are the values obtained by the previous inverse kinematics algorithm IP. Substitute the error value calculated by the current sampled rod length value and the sampling data obtained in the previous two sampling periods and the expected value into the incremental PID formula to obtain the output control amount.

[0116] c) Through the state machine model, the control quantity obtained in the previous step is converted into SPI interface signals and output to the DAC. The DAC converts the control signal into an analog electrical signal of the hydraulic servo valve to control the opening of the hydraulic servo valve, and further controls the elongation or shortening length of the hydraulic rod.

[0117] Figure 1 This is the flowchart of the software part provided by the embodiment of the present invention. Figure 2 This is the block diagram of the hardware part connection interface provided by the embodiment of the present invention.

[0118] The present invention also provides a six-degree-of-freedom parallel platform control system based on Zynq and HLS. The system includes:

[0119] Inverse kinematics algorithm module for six-degree-of-freedom parallel platform: The HLS implements the inverse kinematics algorithm IP for the six-degree-of-freedom parallel platform. Based on the spatial coordinate transformation between the tabletop centroid coordinate system and the base centroid coordinate system, according to the desired pose of the tabletop and the platform structure parameters, the lengths of the six hydraulic rods of the six-degree-of-freedom parallel platform are solved. Based on the optimization method of HLS, pipelines and ARRAY_PARTITION are inserted to improve the data throughput of the inverse kinematics algorithm operation and reduce the solution delay. Fixed-point numbers with different bit widths are used to reduce the solution complexity and improve the solution speed while ensuring the solution accuracy.

[0120] Ethernet data transmission module: It includes the initialization of the TCP connection status detection timer; the design of the LwIP TCP send data and receive data callback functions, the design of the TCP transmission function, and the configuration of the target address port; the design of the transmission frame format, which distinguishes the command frame and the data frame between the host computer and the controller by assigning different meanings to different bits of the transmission frame; the design of the data frame length.

[0121] Zynq PS-PL data transmission and external device drive module, including the AXI bus data transmission module and the hydraulic rod length control module between the PS and the PL.

[0122] AXI bus data transmission module between the PS and the PL: Design the HLS AXI-Lite interface at the PL end to receive the parameters of the incremental PID module; design and configure the AXI-Stream IP related to the sensor data transmission from the PL end to the PS end.

[0123] Hydraulic rod length control module: Design of the hydraulic rod incremental PID control module, design of the drive module for the external SSI length sensor; conversion of the sensor data format and fixed-point floating-point conversion; Verilog implementation of the incremental PID algorithm; design of the drive module for the external DAC.

[0124] The following gives a specific implementation example.

[0125] Generate 1,000,000 groups of test inputs through displacements with a step size of 0.1 m in the x, y, and z directions and deflection angles with a step size of 0.1 rad in the roll, pitch, and yaw directions. Calculate the theoretically exact solution using double precision and compute the mean square error at different quantization precisions for the actual solution results in the inverse solution algorithm IP. Figure 3 The solution accuracy for different fixed-point precisions.

[0126] The parameters of the motion stage used in the test are (in cm for all units):

[0127] The coordinates of connection point 1 of the hydraulic rod to the base platform: {6.4108, -6.8747, 0},

[0128] The coordinates of connection point 2 of the hydraulic rod to the base platform: {6.4108, 6.8747, 0},

[0129] The coordinates of connection point 3 of the hydraulic rod to the base platform: {2.7482, 8.9893, 0},

[0130] The coordinates of connection point 4 of the hydraulic rod to the base platform: {-9.1591, 2.1146, 0},

[0131] The coordinates of connection point 5 of the hydraulic rod to the base platform: {-9.1591, -2.1146, 0},

[0132] The coordinates of connection point 6 of the hydraulic rod to the base platform: {2.7482, -8.9893, 0},

[0133] The coordinates of connection point 1 of the hydraulic rod to the moving platform: {10.7181, -2.4745, 0},

[0134] The coordinates of connection point 2 of the hydraulic rod to the moving platform: {10.7181, 2.4745, 0},

[0135] The coordinates of connection point 3 of the hydraulic rod to the moving platform: {-3.2160, 10.5194, 0},

[0136] The coordinates of connection point 4 of the hydraulic rod to the moving platform: {-7.5020, 8.0448, 0},

[0137] The coordinates of connection point 5 of the hydraulic rod to the moving platform: {-7.5020, -8.0448, 0},

[0138] The coordinates of connection point 6 of the hydraulic rod to the moving platform: {-3.2160, -10.5194, 0},

[0139] The initial height of the moving platform: 56.97,

[0140] Select the fixed-point digit width: 8 bits, 12 bits, 16 bits, 20 bits, 24 bits, 28 bits, 32 bits,

[0141] The error results are as follows:

[0142] Fixed-point number width Mean squared error 8 112.325000000000 12 58.0470000000000 16 0.0303679000000000 20 0.0119813000000000 24 0.00124757000000000 28 0.000680625000000000 32 3.81385000000000e-05

[0143] It can be seen that when the fixed-point digit width used is 28 bits, the mean square error can reach 0.00068, meeting the accuracy requirements of the motion platform.

[0144] In addition, the controller is actually tested on a practical six-degree-of-freedom parallel platform using a helical curve rising in space. The reference running trajectory is a helical rising curve that rises from 0 to 100 mm in the z-axis direction in space through four helical periods, and the helical radius in the x-y plane is 100 mm. The controller in this design is used to track the reference trajectory, and the actual running trajectory and the reference running trajectory are as Figure 4 shown. The operation results of the mean square error in the x, y, and z directions for the reference running trajectory and the actual running trajectory are shown in the following table (unit: mm).

[0145] Coordinate axis Mean squared error / mm X 4.13136 Y 2.84993 Z 0.04202

[0146] The above is only the preferred embodiment of the present invention. Although the present invention has been disclosed above with preferred embodiments, it is not intended to limit the present invention. Any person skilled in the art can make many possible changes and modifications to the technical solution of the present invention by using the methods and technical contents disclosed above, or modify it into equivalent embodiments with equivalent changes. Therefore, any simple modification, equivalent change, and modification made to the above embodiments based on the technical essence of the present invention without departing from the technical solution of the present invention still fall within the scope of the protection of the technical solution of the present invention.

Claims

1. A design method for a six-degree-of-freedom parallel platform controller based on Zynq and HLS, characterized in that, Including: Step 1: Implement the inverse kinematics algorithm IP of the six-degree-of-freedom parallel platform in HLS; Based on the spatial coordinate transformation between the table coordinate system and the base coordinate system, and according to the desired pose of the table and the platform structure parameters, solve the lengths of the six hydraulic rods of the six-degree-of-freedom parallel platform; Based on the optimization method of HLS, improve the data throughput of IP operation by inserting pipeline design and reduce the solution delay; Use fixed-point numbers with different bit widths to reduce the solution complexity and improve the solution speed while ensuring the solution accuracy; The design process of the inverse kinematics algorithm IP is as follows: a) The inverse solution algorithm IP, based on the angle information of the moving table surface passed in After obtaining the corresponding sine and cosine values, substitute them into the pose transformation matrix The functional relationships between the angles in the x, y, and z directions and the three pose transformation matrices are as follows: b) The angular pose change matrix tf between the two coordinate systems of the tabletop and the base is obtained by multiplying the pose transformation matrices in the three directions of x, y, and z BP ; tf BP = rot z (orints[3]) * rot y (orints[2]) * rot x (orints[1]) Wherein, orints[1], orints[2], orints[3] are the rotation angles of the moving table around the x, y, and z axes respectively; The matrix multiplication operation requires multiple parallel accesses to the data in the pose transformation matrix. Add the ARRAY_PARTITION data partitioning implementation method in HLS to the matrix variable, partition and store the left and right matrices to be multiplied by rows and columns respectively to achieve parallel calculation of the multiplication operation, thereby optimizing the parallelism of data access; c) Angle pose transformation matrix tf formed by the pose information of the moving table BP and the displacements trans in the x, y, and z directions of the moving table, the vector height from the origin of the base coordinate system to the origin of the table coordinate system in the initial situation, and the coordinates joint plat [i] of the connection point between the hydraulic rod and the table, and the coordinates joint base [i] of the connection point between the hydraulic rod and the base. Calculate the coordinate values of the connection points between the moving table and the hydraulic rods in the base coordinate system, and then calculate the spatial vectors leg[i] of the six hydraulic rods in the base coordinate system, where i ∈ [1, 6] represents the six hydraulic rods; leg[i] = trans + height + tf BP *joint plat [i] - joint base [i] After taking the modulus of the leg[i] vector, the length of the hydraulic rod |leg[i]| is obtained; Considering the speed requirement of the solution, HLS supports customizing the total bit width and the fractional part bit width of the fixed-point number. Use the HLS ap_fixed custom data type to reduce the total bit width of the fixed-point number to 16 bits - 32 bits, and set the fractional part bit width to half of the total bit width to improve the solution speed while ensuring the accuracy; Step 2: Design the Ethernet data transmission module, including the following sub-steps: 2.1 Initialize the TCP connection status detection timer, periodically monitor whether the TCP connection exists through the timer, and re-establish the connection if the connection is detected to be disconnected; 2.2 Develop the LwIP library, design the relevant callback functions for sending and receiving data, design the transmission function for transmitting the lengths of the six hydraulic rods sampled by the SSI length sensor to the host computer, and configure the target address port; 2.3 Design the transmission frame format, distinguish the command frame and the data frame between the host computer and the controller by assigning different meanings to different bits of the transmission frame; 2.4 Design the data frame length, design the bit width of the sampling points of the SSI length sensor and the number of sampling points included in each data frame, and reduce the transmission waiting time by performing ping-pong operations on the transmission buffer; The design of the Ethernet data transmission module is specifically as follows: a) After the controller is powered on, perform initialization operations, including initializing the TCP connection status detection timer, initializing GPIO, initializing LwIP-related parameters, and initializing the DMA interrupt function; b) The controller listens to the IP address and port of the host computer. The host computer transmits a command frame to the PS side of the controller through a TCP connection. When the host computer instruction is received, call the callback function for receiving data, and then further judge the host computer instruction; c) Determine the attribute of the current command frame by the first byte of the command frame; if it is 0x01, the current command frame is to reset the platform structure parameters; if it is 0x00, the current command frame is a pose frame for transmitting the desired pose of the tabletop, that is, the pose frame includes the desired displacements of the moving tabletop in the x, y, and z directions and the rotations of the moving tabletop around the x, y, and z axes, roll, pitch, and yaw; for offline operations without the host computer, read the stored desired trajectory pose data from the EMMC storage module. d) After confirming that the current command frame is a pose frame, the PS side passes the desired pose vector into the inverse kinematics algorithm IP through the AXI-Lite protocol by calling Xil_Out32, and then starts the inverse kinematics calculation by calling the startup function XSteback_Start. e) For the part of the inverse kinematics result returned to the host computer, detect whether the inverse kinematics calculation is completed through the status detection function XSteback_IsDone; after the inverse kinematics calculation is completed, the desired hydraulic rod length is obtained and stored in the first 24 bytes of the LwIP TCP transmission area. f) Transfer the actual hydraulic rod length data of 256 bytes, a total of 64 sampling points, sampled by the SSI length sensor into the LwIP TCP transmission area after the 24 bytes of the desired hydraulic rod length at the PS side through AXI-DMA. g) When both the desired hydraulic rod length and the actual hydraulic rod length sequence are written into the LwIP TCP transmission area, confirm that the length of the data to be sent is less than the remaining available length of the send buffer through the designed transmission function, start the TCP transmission, write the current number of transmission sampling points and the bit width of each sampling point in the header of the TCP data frame, and call the callback function for sending data to count the amount of data sent after sending a batch of data to the host computer. Step 3, Design of the AXI bus data transmission module between the PS and the PL, including the following sub-steps: 3.1 Design the PL-side HLS AXI-Lite interface to receive the parameters of the incremental PID module transmitted from the PS side through the registers in the AXI-Lite IP. The specific steps of 3.1 are as follows: a) Customize the AXI-Lite IP, define the AXI-Lite bit width as 32 bits, and use a total of 4 registers to configure the incremental PID parameters. Initialize and define the AXI_M interface connected to the PS side, and assign the corresponding address to the AXI-Lite IP. b) The 4 registers store the P parameter, I parameter, D parameter, and calculation step of the PID respectively. The PS side writes the incremental PID parameters into the instantiated BRAM through the AXI-Lite IP. The incremental PID module calculates the output of the incremental PID by reading the incremental PID parameters combined with the desired hydraulic rod length and the actual hydraulic rod length. 3.2 Design and configure the AXI-Stream IP related to the data transmission from the PL side to the PS side of the SSI length sensor, batch transfer the SSI length sensor data back to the PS side from the FIFO and generate a PS-side interrupt signal. The specific steps of 3.2 are as follows: a) The PS side enables the AXI-HP interface, declares the DMA_DEVICE_ID, and initializes the DMA interrupt; b) After widening the actual hydraulic rod length data sampled by the SSI length sensor to 32 bits, write it into the FIFO with a depth of 64. When the FIFO full signal is detected, start the DMA transfer; c) After the PS side receives the interrupt of the DMA transfer completion, perform sliding filtering on the data of the SSI length sensor received by the PS side and store it in the TCP send buffer; Step 4, Design of the hydraulic rod length control module, including the following sub-steps; 4.1 Driver design of the SSI length sensor and the DAC servo valve control module; 4.2 Data format conversion and fixed-point to floating-point conversion of the SSI length sensor data; 4.3 Verilog implementation of the incremental PID module; The specific content of Step 4 is as follows: a) The actual hydraulic rod length sensor acquisition part is connected to the SSI length sensor on the hydraulic rod through the SSI interface. The serial data sampled by the SSI interface is converted from serial to parallel and undergoes sliding filtering to obtain the actual hydraulic rod length data; b) The control part adopts the incremental PID method. The expected lengths of the six hydraulic rods are the values obtained by the previous inverse solution algorithm IP. The error values calculated by comparing the actual hydraulic rod length data, the sampling data obtained in the previous two sampling periods, and the expected values are substituted into the incremental PID formula to obtain the output control quantity; c) Through the state machine model, the control quantity obtained in the previous step is converted into an SPI interface signal and output to the DAC. The DAC converts the control signal into an analog electrical signal of the hydraulic servo valve to control the opening of the hydraulic servo valve, thereby controlling the elongation or shortening length of the hydraulic rod.

2. A six-degree-of-freedom parallel platform control system based on Zynq and HLS, characterized in that, This system is implemented based on the method described in Claim 1. The system includes: Inverse solution algorithm module for the six-degree-of-freedom parallel platform: The HLS implements the inverse solution algorithm IP for the six-degree-of-freedom parallel platform; based on the spatial coordinate transformation between the tabletop coordinate system and the base coordinate system, according to the expected pose of the tabletop and the platform structure parameters, solve the lengths of the six hydraulic rods of the six-degree-of-freedom parallel platform; based on the optimization method of HLS, insert pipelines and ARRAY_PARTITION to improve the data throughput of the inverse solution algorithm IP operation and reduce the solution delay; use fixed-point numbers with different bit widths to reduce the solution complexity and improve the solution speed while ensuring the solution accuracy; Ethernet data transmission module: including initialization of the TCP connection status detection timer; design of the LwIP TCP send data and receive data callback functions, design of the TCP transmission function, target address port configuration; transmission frame format design, by assigning different meanings to different bits of the transmission frame to distinguish the command frame and the data frame between the host computer and the controller; data frame length design; Zynq PS and PL data transmission and external device drive module, including the AXI bus data transmission module between the PS and the PL and the hydraulic rod length control module; AXI bus data transfer module between PS and PL: Design the HLS AXI-Lite interface on the PL side to receive the parameters of the incremental PID module; Design and configure the AXI-Stream IP related to the data transfer of the SSI length sensor from the PL side to the PS side; Hydraulic rod length control module: Design of the hydraulic rod incremental PID module, drive design of the SSI length sensor and DAC servo valve control module; Data format conversion and fixed-point to floating-point conversion of the SSI length sensor; Verilog implementation of the incremental PID algorithm.

Images