High-precision real-time Stewart platform forward kinematics solving method based on FPGA acceleration

Abstract

The invention discloses a high-precision real-time Stewart platform forward kinematics solving method based on FPGA acceleration, which mainly solves the problems of high forward kinematics calculation complexity, low real-time performance, low precision and the like of the existing Stewart platform, and comprises the following implementation steps of: establishing a Stewart platform model and determining a working space; selecting a data set according to the truncated Gaussian distribution, and performing neural network fitting; quantifying and deploying the neural network; realizing a CORDIC sine and cosine solution, pre-calculating a Jacobian matrix, and storing the pre-calculated Jacobian matrix in an FPGA; the neural network output serves as an iteration initial value, and a positive solution pose is obtained through iteration; and carrying out positive solution acceleration through array segmentation, loop expansion and an assembly line. By introducing a low hardware resource consumption strategy, a positive solution module is deployed into the FPGA, the time consumed by one-time positive solution is less than 250 microseconds, and the positive solution pose error is less than 0.01 cm and 0.01 degree.

Description

Technical Field

[0001] The present invention belongs to the research field of kinematics, dynamics and control of mechanical systems, and particularly relates to a forward kinematics solution method for a Stewart parallel mechanism, specifically a high-precision real-time Stewart platform forward kinematics solution method based on FPGA acceleration. Background Art

[0002] The Stewart platform, as a revolutionary parallel mechanism, has been a hot topic in mechanism research since it was proposed by D. Stewart in 1965. It is a structure composed of two platforms and six telescopic links. In the past few decades, the Stewart platform has received extensive attention from researchers due to its inherent advantages over traditional serial mechanisms. These advantages include simple structure, high stiffness, high precision, strong load-bearing capacity, and no cumulative error. Currently, the Stewart platform has been widely used in multiple fields, such as flight simulators, parallel robot manipulators, machine tools, six-degree-of-freedom coordinate measuring devices, and entertainment and health equipment, etc.

[0003] However, the practical application of the Stewart platform faces a key challenge: the forward kinematics solution problem of the Stewart platform. The forward solution problem refers to solving the pose (position and orientation) of the Stewart moving platform relative to the static platform when the lengths of the six links are known, that is, the forward kinematics of the Stewart platform. However, this problem is very difficult, and it is almost impossible to find a practical closed-form solution. During the solution process of the kinematic forward solution, there are complex coupling relationships among multiple variables. To obtain an analytical solution, a set of multivariate nonlinear equations need to be solved. This process is not only difficult but also cumbersome. Solving this nonlinear equation system will obtain multiple possible solutions, and false solutions need to be excluded to determine the unique positive solution.

[0004] Other effective solution methods for the forward kinematics of the Stewart platform include numerical iteration methods and using kinematic inverse solutions to approximate the kinematic forward solutions, etc. Although these methods can theoretically obtain solutions, in practical applications, especially in scenarios that require real-time control, computational efficiency and real-time performance are still a huge challenge. Compared with the closed-form solution method, the numerical solution method has the following advantages: the mathematical model is relatively simple, reducing the complexity of the problem, avoiding complex mathematical derivation processes, and the solution speed is related to the specified iteration accuracy. However, the solution result of the numerical solution method is closely related to the selection of the initial value. An inappropriate initial value may lead to convergence failure or obtain a wrong solution. At the same time, the iterative process involves the calculation and inversion of a 6×6 dimensional Jacobian matrix, which will result in a heavy computational burden.

[0005] The research on the forward kinematics problem of the Stewart platform not only has theoretical significance but is also crucial for aspects such as real-time control trajectory planning and workspace analysis in practical applications. In fact, the mathematical complexity of the forward kinematics of the Stewart platform has become a serious drawback, hindering the use of the Stewart platform system in many high-speed, real-time, and online engineering applications. Therefore, there is an urgent need to design a high-precision real-time forward kinematics solution method. Summary of the Invention

[0006] The object of the present invention is to provide a high-precision real-time forward kinematics solution method for the Stewart platform based on FPGA acceleration, aiming at the problems of high complexity, poor real-time performance, and low calculation accuracy in the current forward kinematics solution of the Stewart platform. It solves a series of non-linear equations and multi-solution problems involved in the forward kinematics solution of the Stewart platform, and at the same time realizes high precision and real-time performance.

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

[0008] According to the first aspect of this specification, a high-precision real-time forward kinematics solution method for the Stewart platform based on FPGA acceleration is provided, including the following steps:

[0009] S1. Establish a Stewart platform model and determine the workspace. Specifically: Based on the six coupling points P1, P2, P3, P4, P5, P6 of the real Stewart moving platform and the six coupling points B1, B2, B3, B4, B5, B6 of the static platform, establish a Stewart platform model. According to the length ranges of the six connecting rods, obtain the extreme poses of the moving platform and determine the workspace of the moving platform;

[0010] S2. Select a data set according to the truncated Gaussian distribution and perform neural network fitting. Specifically: Given the pose ζ = [x, y, z, α, β, γ] of the moving platform relative to the static platform T , where α, β, γ respectively represent the rotation angles of the moving platform around the x, y, z axes, and x, y, z respectively represent the displacements of the moving platform along the x, y, z axes. Calculate the rotation matrix using the x - y - z type Euler angles. The rotation matrices R x , R y , R z of the x, y, z axes and their rotation angles have the following functional relationships:

[0011] Obtain the rotation matrix R BP of the moving platform relative to the static platform using the x - y - z type rotation matrix as follows:

[0012]

[0013] The solution formula for the i-th connecting rod vector leg[i] is as follows:

[0014] leg[i] = T + R BP *P i -B i

[0015] where T is the displacement of the moving platform relative to the static platform; the solution formula for the length of the i-th connecting rod is as follows:

[0016]

[0017] where i = 1 to 6, (x pi , y pi , z pi ) and (x bi , y bi , z bi ) are the coordinate representations of P i and B i in the static platform coordinate system respectively; after inverse solution, the connecting rod lengths corresponding to the pose are obtained. The connecting rod lengths are used as the input of the neural network, and the pose is used as the output of the neural network to form the dataset for neural network fitting; the neural network is trained and evaluated to fit the functional relationship between the lengths of the six connecting rods and the pose;

[0018] S3. Quantize and deploy the neural network. Adopt the static quantization scheme to convert the trained floating-point neural network into an integer neural network. Specifically: Quantize the weights and activation values of each layer according to the scale factor and zero point respectively. The mapping relationship between the real floating-point number r and the quantized data Q is as follows: where round(·) is the rounding operation, s is the scale factor, and Z is the quantized zero point. The calculation formulas are as shown below:

[0019]

[0020] where r max and r min represent the maximum and minimum floating-point values before quantization respectively, and Q max and Q min represent the maximum and minimum integer values after quantization respectively;

[0021] S4. Implement the CORDIC sine and cosine solution method. Pre-calculate the Jacobian matrix and store it in the FPGA. Specifically: Set the iteration number n of the CORDIC algorithm, and initialize the rotation angle θ j , where j = 0 to (n - 1). Through the lookup table of the FPGA, θ jPre-store; in each iteration, rotate forward or backward at a pre-fixed angle, and sum up the rotated angles. If the cumulative angle is greater than the target angle Φ, perform negative rotation; otherwise, perform positive rotation. After the rotation times are completed, determine cosΦ and sinΦ by directly reading the x and y values of the final rotation vector. The rotation calculation formula is as follows:

[0022]

[0023] where R j (θ j ) is the rotation matrix for the j-th rotation, is the vector before the j-th rotation, is the vector after the j-th rotation; transform the rotation matrix into

[0024]

[0025] where is the proportionality coefficient for each rotation operation. After determining the number of rotations, the proportionality coefficient is a fixed constant and is pre-calculated and stored in the FPGA;

[0026] S5. The neural network output is used as the initial value of the iteration, and the correct pose is obtained through iteration. Specifically: reduce the computational complexity by pre-differentiating and storing the functional expression of the Jacobian matrix. The Jacobian matrix required for iteration is only a function of the pose ξ, and the expression of the Jacobian matrix is:

[0027]

[0028] where f i is the inverse solution expression of the i-th link, The specific expressions of each element in the Jacobian matrix are obtained through the jacobian function in matlab and pre-stored in the FPGA;

[0029] Assume that L 6×1 is the link length of the forward solution input. Take L 6×1 as the input of the neural network to obtain the initial pose ξ0 of the iteration. Set the forward solution objective function as: F(ξ k ) = I(ξ k ) - L, where ξ k is the pose after the k-th iteration, I(·) is the inverse solution function, and the forward solution solving target is F(ξ k ) = [0] 6×1 ; Through multiple iterations of ξ k , the final forward solution pose is obtained. The iteration process is as follows:

[0030]

[0031] Among them, is the Jacobian matrix J, and its inverse matrix is obtained through LU decomposition;

[0032] S6 accelerates the forward solution through array segmentation, loop unrolling, and pipelining. Specifically: perform loop unrolling operations on the operations in the inner for loop, perform parallel processing on the uncoupled loop orders. When the loop is fully unrolled, the inner loop operations are completed within one cycle; based on the pineline instruction, pipeline the initialization and loop parts in the rotation matrix multiplication, so that each computing unit works simultaneously in each operation cycle; further accelerate the operation speed by performing array segmentation on the arrays A and B participating in the matrix multiplication operation. Each time an operation is performed, the elements in each row of array A are segmented into separate registers according to dimension 2, and the elements in each column of array B are segmented into separate registers according to dimension 1 for operation.

[0033] Furthermore, the specific content of S1 is as follows: Based on Simscape, model the Stewart platform, and build the model by connecting rigid bodies with hinges; select the body module and adjust its size to match the actual static platform; determine the coordinates of the six coupling points of the static platform and place six flanges; add rotary joints at the coupling points, and use a cross structure to connect the connecting rod and the static platform to form a universal hinge; the connection method between the moving platform and the connecting rod is the same; according to the length ranges of the six connecting rods, obtain the extreme postures of the moving platform, thereby determining the working space of the moving platform.

[0034] Furthermore, the selection of pose data in S2 is specifically as follows: Each time, randomly combine six pose parameters selected based on the Gaussian distribution, and judge whether there are parameters that exceed the working space among the six parameters. If so, discard this pose; select the poses where all six parameters are within the working space and put them into the data set until the number of pose data reaches the preset number.

[0035] Furthermore, in S2, a 4-layer BP neural network is used for training and evaluation. Each layer of the network contains 100 neurons, and the functional relationship between the lengths of the six connecting rods and the poses is fitted.

[0036] Furthermore, in S3, the fitting effects of the neural network before and after quantization are evaluated according to the loss value. Specifically: Set the same training set and test set, and compare the loss values of the neural network after the same number of training rounds; the neural network fitting uses the root mean square error function as the loss function, that is:

[0037]

[0038] Among them, X obs,i is the true pose data in the data set, X model,iIt is the predicted pose data obtained by neural network fitting; the root mean square error is calculated for the u pose data fitted in the test set to reflect the degree to which the fitted pose deviates from the true pose.

[0039] Furthermore, in S4, the number of iterations of the CORDIC algorithm is n=20, and θ is pre-calculated j =arctan(2 -j ), where j = 0 to (n-1), the first rotation angle is 45°, which is suitable for calculating the sine and cosine values of -90° to 90° in the inverse solution; the compensation coefficient K of the rotation vector is set to

[0040]

[0041] Furthermore, in S5, the calculation formula for the element value of the first row and first column in the Jacobian matrix is as follows:

[0042]

[0043] Among them, x b1 ,y b1 ,z b1 It is known that B i Point coordinates, x p1 ,y p1 ,z p1 is a function of (x, y, z, α, β, γ), by inversely solving the equation Solve; the solution method for other elements in the Jacobian matrix is similar.

[0044] Furthermore, in S6, the specific implementation method of matrix operation optimization is as follows: in the three-layer for loop of matrix multiplication, the innermost loop uses loop unrolling to limit the loop period to one clock cycle; the second layer loop uses pipeline parallel initialization and loop operation; each time the array is taken out from the memory, the elements of the entire row or column are stored in a separate register for array segmentation to achieve the purpose of accelerating the operation.

[0045] According to the second aspect of this specification, a high-precision real-time Stewart platform forward kinematics solution device based on FPGA acceleration is provided, comprising a memory and one or more processors, wherein the memory stores executable code, and when the processor executes the executable code, it is used to implement the high-precision real-time Stewart platform forward kinematics solution method based on FPGA acceleration as described in the first aspect.

[0046] According to a third aspect of this specification, a computer-readable storage medium is provided, on which a program is stored. When the program is executed by a processor, the high-precision real-time Stewart platform forward kinematics solution method based on FPGA acceleration as described in the first aspect is implemented.

[0047] The present invention has the following advantages compared with the prior art:

[0048] 1. The present invention combines the advantages of neural network fitting and numerical iteration. By means of initial iteration of the neural network, the problem of non-convergence of the iteration method is avoided, and the number of iterations is greatly reduced, accelerating convergence. By establishing a simulation model, the accuracy of the solution obtained by the present invention is evaluated, and the pose error obtained is less than 0.01 cm and 0.01°.

[0049] 2. The present invention implements the CORDIC algorithm for calculating sine and cosine in FPGA, pre-computes the Jacobian matrix of the Stewart platform, quantizes and deploys the neural network at the same time, and adopts a variety of optimization schemes to accelerate the forward solution operation speed, reduce the number of iterations and the time consumed in each iteration, improve the real-time performance of the forward solution module, and the complete running time of the forward solution scheme of the present invention in FPGA is less than 250 μs. BRIEF DESCRIPTION OF THE DRAWINGS

[0050] Figure 1 is the overall flow block diagram of the present invention.

[0051] Figure 2 is the model established by the present invention according to the parameters of the real Stewart platform.

[0052] Figure 3 is the comparison of the loss values between the floating-point neural network and the integer neural network in the present invention.

[0053] Figure 4 is the schematic diagram of calculating cosΦ and sinΦ by rotating vectors using the CORDIC algorithm in the present invention.

[0054] Figure 5 is the schematic structural diagram for verifying the accuracy of the forward solution module of the present invention.

[0055] Figure 6 is the error schematic diagram of the pose of the moving platform implemented by the forward solution method proposed by the present invention in FPGA. DETAILED DESCRIPTION OF THE EMBODIMENTS

[0056] The technical solutions and effects of the present invention will be further described in detail below with reference to the accompanying drawings.

[0057] For the application of the forward solution of the Stewart platform in practical scenarios such as real-time control trajectory planning and workspace analysis, the existing methods for solving the closed-form forward solution based on a set of nonlinear equations face challenges such as high computational complexity and difficult solution processes. To address the above challenges, the present invention provides an initial value of the forward solution by introducing neural network fitting, uses the Newton iteration method to obtain a high-precision forward solution, and adopts a low-hardware-resource-consumption strategy to deploy the forward solution module to the FPGA, and proposes a high-precision real-time forward kinematics solution method for the Stewart platform based on FPGA acceleration, referring to Figure 1 . The implementation steps of the present invention are as follows:

[0058] Step 1: Establish a Stewart platform model and determine the workspace. In Simulink, according to the coordinates of 12 coupling points of the real Stewart platform, that is, six coupling points P1, P2, P3, P4, P5, P6 of the moving platform and six coupling points B1, B2, B3, B4, B5, B6 of the static platform, establish a Stewart platform model, referring to Figure 2 . Obtain the workspace of the Stewart platform according to the model.

[0059] Specifically, in the Simscape module in Simulink, use Multibody to model the Stewart platform, and build the model by connecting rigid bodies and hinges. First, establish the static platform. Find the module body in the rigid body sub-module of Simscape, adjust the size of the module body according to the actual static platform, determine the position coordinates of the six coupling points, and place six flanges at the coupling point positions to form the basic model of the static platform. Add a revolute joint Revolute Joint at the coupling point, and connect the link and the static platform through a cross structure to form a universal joint. The same method is used for the connection between the moving platform and the link. According to the length range of the six links, obtain the extreme poses of the moving platform, so as to determine the workspace of the moving platform.

[0060] Step 2: Select the dataset according to the truncated Gaussian distribution and perform neural network fitting. In the actual operation of the Stewart platform, the probability of being near the standard pose is much higher than that of being near the extreme pose. The poses in the constructed dataset conform to the truncated Gaussian distribution. The following uses an example to illustrate the specific implementation of this method: Assume that the working space of the moving platform in the x-axis direction is [-10, 10], and the variable m follows the standard normal distribution m ∼ N(0, 1). Then, the displacement x of the x-axis is obtained as x = 10 * m, and the displacement that meets x ∈ [-10, 10] is selected as the displacement of the x-axis. A total of 50,000 poses generated by the truncated Gaussian distribution are selected within the working space of the moving platform. The selection method can be comprehensively determined according to the following principles: Each time, six pose parameters selected based on the Gaussian distribution are randomly combined, and it is judged whether there are parameters that exceed the working space among the six parameters. If so, the pose is discarded; the poses with all six parameters within the working space are selected and put into the dataset until the number of pose data reaches 50,000.

[0061] Calculate the lengths of the six connecting rods corresponding to the poses in the dataset through the inverse solution algorithm to form the dataset for neural network fitting. The specific implementation process of the inverse solution algorithm is as follows: Given the pose ξ = [x, y, z, α, β, γ] of the Stewart moving platform relative to the static platform T , where α, β, γ respectively represent the rotation angles of the moving platform around the x, y, z axes, and x, y, z respectively represent the displacements of the moving platform along the x, y, z axes. The rotation matrix is calculated using the x-y-z type Euler angles. The rotation matrices R x , R y , R z and their rotation angles have the following functional relationships:

[0062]

[0063] Using the x-y-z type rotation matrix, the rotation matrix R BP of the moving platform relative to the static platform is obtained as follows:

[0064]

[0065] The coordinates P i of the six coupling points of the moving platform are obtained by left-multiplying the rotation matrix R BP and adding the displacement T of the moving platform relative to the static platform. The coordinate representation of the coupling points of the moving platform under the static platform can be obtained. By taking the difference with the coordinate B i of the corresponding coupling points of the static platform, the vector representation of the six connecting rods can be obtained. Among them, the solution formula for the vector leg[i] of the i-th connecting rod is as follows:

[0066] leg[i] = T + R BP *P i -Bi

[0067] Taking the modulus of the i-th link vector gives the length of the i-th link:

[0068]

[0069] where i = 1 to 6, ‖·‖ represents the two-norm operation of the vector, and (x pi , y pi , z pi ) is the coordinate representation of the i-th coupling point of the moving platform in the static platform coordinate system, and (x bi , y bi , z bi ) is the coordinate representation of the i-th coupling point of the static platform in the static platform coordinate system. After the inverse solution algorithm, the link lengths corresponding to the pose are obtained. The link lengths are used as the input of the neural network, and the pose is used as the output of the neural network to form the dataset for neural network fitting. In this example, a 4-layer BP neural network is used for training and evaluation. Each layer of the network contains 100 neurons to fit the functional relationship between the lengths L of the six links and the pose ξ.

[0070] Step 3: Quantize and deploy the neural network. The trained floating-point neural network is converted into an integer neural network using a static quantization scheme; after model quantization, the quantization parameters of each layer are fixed and do not change during application, which is suitable for deployment in FPGA. The weights and activation values of each layer are quantized according to the scale factor (scale) and zero point (zero_point). The mapping relationship between the real floating-point number r and the quantized data Q is as follows: r ≈ s(Q - Z), where round(·) is the rounding operation, s is the scale factor, and Z is the zero point after quantization. The calculation formulas are as follows:

[0071]

[0072] where r max and r min represent the maximum and minimum floating-point values before quantization respectively, and Q max and Q min represent the maximum and minimum integer values after quantization respectively.

[0073] Evaluate the effects of the floating-point neural network and the integer neural network; select the root mean square error function RMSE as the loss function of the neural network, evaluate the loss values of the two networks at different training epochs, and refer to Figure 3 . The specific loss calculation formula is as follows:

[0074]

[0075] Among them, X obs,i is the true pose data in the dataset, and X model,i is the predicted pose data obtained by neural network fitting. Calculating the root mean square error for the u pose data fitted in the test set can reflect the degree to which the fitted pose deviates from the true pose. The smaller the RMSE, the better the fitting effect. Therefore, the root mean square error can be used as the standard for evaluating the fitting effect.

[0076] Deploy the quantized neural network using Vitis HLS. Convert the neural network described in C++ into a hardware description language through high-level synthesis HLS, and conduct comprehensive design to evaluate the resource occupancy and running speed of the floating-point neural network and the integer neural network. Compared with the floating-point neural network, the resource occupancy of the integer neural network is reduced by 34%, and the running speed is increased by 30%.

[0077] Step 4: Implement the CORDIC sine and cosine solution method, and pre-compute the Jacobian matrix and store it in the FPGA. The iterative process involves inverse solution and requires calculating the sine and cosine values of the rotation angle. Since directly using sine and cosine functions in the FPGA consumes a large amount of DSP resources, the CORDIC algorithm is deployed to reduce resource consumption. According to the key innovative features of the CORDIC algorithm, calculating the sine and cosine values of the input angle is completed through a series of vector rotations. The rotation can be completed in the way that requires the least calculation, and only shift operations are needed, so it occupies very little hardware resources. First, according to the accuracy requirements of the forward solution algorithm, set the number of iterations n of the COEDIC algorithm to 20, and initialize the calculation of the rotation angle θ j = arctan(2 -j ), where j = 0 to (n - 1), and pre-store θ j through the lookup table of the FPGA.

[0078] In each iteration, rotate forward or backward at a pre-fixed angle, and sum the rotated angles, referring to Figure 4 . If the cumulative angle is greater than the target angle Φ, perform negative rotation; otherwise, perform positive rotation. When the number of rotations ends, determine cosΦ and sinΦ by directly reading the x and y values of the final rotated vector. The specific implementation method of rotation is obtained by left-multiplying the coordinate by the rotation matrix, as follows:

[0079]

[0080] Among them, R j (θ j ) is the rotation matrix of the j-th rotation, is the vector before the j-th rotation, is the vector after the j-th rotation.

[0081] The rotation matrix can be transformed through trigonometric identities into

[0082]

[0083] Set tan(θ j ) to a power of 2, i.e., set tan(θ j ) = 2 -j , and pre-store θ j in the FPGA. The rotation operation can be simplified to data shifting and addition. When the number of iterations is determined, the compensation coefficient K of the rotation vector is a fixed constant, which can be pre-calculated and stored in the FPGA. Set the compensation coefficient of the rotation vector of this algorithm to be

[0084]

[0085] Compared with the exact sine and cosine solution algorithm, the error of the sine and cosine values calculated by the CORDIC algorithm used in the present invention is less than 1e-6, which greatly reduces the occupation of DSP resources while meeting the requirements of forward and inverse solution calculation accuracy.

[0086] Step Five: The output of the neural network is used as the initial value of the iteration, and the forward solution pose is obtained through iteration. To avoid re-derivation during each iteration, which increases the computational complexity and occupies unnecessary running time, the present invention pre-computes the specific expression of the Jacobian matrix. The input of the forward solution problem is the lengths of the six connecting rods between the platforms, and the output is the six parameters representing the pose of the moving platform. Therefore, the corresponding Jacobian matrix required for iteration is only a function of the six pose parameters, and the specific function expression of the Jacobian matrix can be obtained by pre-derivation and deployed in the FPGA in advance, avoiding a large amount of operations during the iteration process. The expression of the Jacobian matrix is:

[0087]

[0088] where f i is the inverse solution expression of the i-th connecting rod,

[0089] A specific example is to calculate the numerical value of the element in the first row and first column of the Jacobian matrix:

[0090]

[0091] where x b1 , y b1 , z b1 are the known coordinates of point B1, and x p1 , y p1 , z p1 are functions of (x, y, z, ɑ, β, γ) and can be obtained through the inverse solution equation Solve it. The solution methods for other elements in the Jacobian matrix are similar, and the specific expressions of each element in the Jacobian matrix are obtained through the jacobian function in matlab and pre-stored in the FPGA.

[0092] Assume L 6×1 is the link length of the forward kinematic solution input. Take L 6×1 as the input of the neural network to obtain the initial pose ξ0 of the iteration. Set the forward kinematic objective function as:

[0093] F(ξ k ) = I(ξ k ) - L

[0094] where ξ k is the pose after the k-th iteration. In this example, k = 0 to 3 is set. I(·) is the inverse kinematic function, and the forward kinematic solution target is F(ξ k ) = [0] 6×1 . By performing 3 iterations on ξ k , the final forward kinematic pose ξ3 of the present invention is obtained. The iteration process is as follows:

[0095]

[0096] where is the Jacobian matrix J, and its inverse matrix is obtained through LU decomposition.

[0097] Step 6: Accelerate the forward kinematic solution through array splitting, loop unrolling, and pipelining. The forward kinematic algorithm includes inverse kinematic operations, and both the inverse kinematic and iteration processes involve a large number of matrix multiplications. Therefore, the present invention utilizes the parallel characteristics of the FPGA to optimize the matrix multiplication. First, perform a loop unrolling operation on the operation content in the inner for loop, and perform parallel processing on the loop orders that are not coupled to each other. For the rotation matrix operation, when the loop is fully unrolled, the inner loop operation is completed within one cycle. Based on the pineline instruction in Vitis HLS, the rotation matrix multiplication R BP = R x (α)R y (β)R zThe initialization and loop parts in (γ) are pipelined so that each computing unit is working in each operation cycle, thereby improving data throughput. In addition, the present invention further speeds up the operation speed by performing array partitioning on arrays A and B participating in matrix multiplication. Each time an operation is performed, the elements in each row of array A are divided into separate registers according to dimension 2, and the elements in each column of array B are divided into separate registers according to dimension 1 for operation, accelerating matrix operations. The positive solution algorithm is comprehensively designed using Vitis HLS, and the input and output of the positive solution module are amplified by 10,000 times as the interface to conform to the operation characteristics of the FPGA. The resource differences occupied before and after neural network quantization are evaluated, the design IP of the positive solution module is generated, and Vivado simulation tests are carried out to obtain the running time of the positive solution module.

[0098] The effects of the method proposed by the present invention will be further described below with reference to examples.

[0099] The parameters of the Stewart platform used in the test are (in cm for all units):

[0100] The coordinates of the coupling point P1 between the connecting rod and the moving platform are (49.1464, -63.1240, 0);

[0101] The coordinates of the coupling point P2 between the connecting rod and the moving platform are (49.1464, 63.1240, 0);

[0102] The coordinates of the coupling point P3 between the connecting rod and the moving platform are (30.0938, 74.1240, 0);

[0103] The coordinates of the coupling point P4 between the connecting rod and the moving platform are (-79.2401, 11.0000, 0);

[0104] The coordinates of the coupling point P5 between the connecting rod and the moving platform are (-79.2401, -11.0000, 0);

[0105] The coordinates of the coupling point P6 between the connecting rod and the moving platform are (30.0938, -74.1240, 0);

[0106] The coordinates of the coupling point B1 between the connecting rod and the static platform are (99.0152, -14.0000, 0);

[0107] The coordinates of the coupling point B2 between the connecting rod and the static platform are (99.0152, 14.0000, 0);

[0108] The coordinates of the coupling point B3 between the connecting rod and the static platform are (-37.3832, 92.7496, 0);

[0109] The coupling point coordinates B4 of the connecting rod and the static platform are (-61.6319, 78.7496, 0);

[0110] The coupling point coordinates B5 of the connecting rod and the static platform are (-61.6319, -78.7496, 0);

[0111] The coupling point coordinates B6 of the connecting rod and the static platform are (-37.3832, -92.7496, 0);

[0112] Set the reference waveform sent as follows:

[0113] x = 4sin(2πt) + 3cos(πt)

[0114] y = 4cos(2πt) + 3sin(1.5πt)

[0115] z = 4sin(πt) + 149.5

[0116] α = 3sin(πt) + 1.5cos(0.5πt)

[0117] β = 3cos(πt) + 1.5sin(0.5πt)

[0118] γ = 1.2sin(1.5πt) + 0.7sin(3πt)

[0119] Among them, the unit of the position reference waveform is cm, and the unit of the attitude reference waveform is deg.

[0120] Establish a control system including reference waveform sending, inverse kinematics, PID controller, Stewart platform model and forward solution module, evaluate the accuracy of the forward solution module implemented by the method of the present invention, refer to Figure 5 ... Implement the above scheme using C++ through Vitis HLS, accelerate the matrix operation by array segmentation, loop unrolling and pipeline optimization techniques, and convert it into Verilog language for comprehensive testing, and package the IP for simulation testing to obtain the running time. Driven by a 10ns system clock, the running time of the forward solution module is tested to be less than 250μs, which is sufficient to meet the real-time requirements in most application scenarios. Save the connecting rod length data obtained after passing the above reference waveform through the PID controller to the FPGA, with a total of 10,000 connecting rod length data, and calculate the corresponding pose. The pose error calculated by the forward solution scheme proposed by the present invention in the FPGA is as Figure 6As shown in the figure. The errors of the six poses calculated by the present invention are all less than 0.01 cm and 0.01°. This shows that the accuracy of the forward solution scheme proposed by the present invention is sufficient to meet the accuracy requirements in most application scenarios. The unity of high precision and real-time performance demonstrates the unique advantages of the forward kinematics solution method of the high-precision real-time Stewart platform based on FPGA acceleration proposed by the present invention.

[0121] Corresponding to the foregoing embodiment of the forward kinematics solution method of the high-precision real-time Stewart platform based on FPGA acceleration, the present invention also provides an embodiment of a device for solving the forward kinematics of a high-precision real-time Stewart platform based on FPGA acceleration.

[0122] The device for solving the forward kinematics of a high-precision real-time Stewart platform based on FPGA acceleration provided by the embodiment of the present invention includes a memory and one or more processors. An executable code is stored in the memory. When the processor executes the executable code, it is used to implement the forward kinematics solution method of the high-precision real-time Stewart platform based on FPGA acceleration in the above embodiment.

[0123] The embodiment of the device for solving the forward kinematics of a high-precision real-time Stewart platform based on FPGA acceleration of the present invention can be applied to any device with data processing capabilities. The any device with data processing capabilities can be a device or apparatus such as a computer. The device embodiment can be implemented by software, or by hardware, or by a combination of software and hardware.

[0124] For the specific implementation process of the functions and roles of each unit in the above device, please refer to the implementation process of the corresponding steps in the above method in detail, and will not be elaborated here.

[0125] For the device embodiment, since it basically corresponds to the method embodiment, the relevant parts can refer to the partial description of the method embodiment. The device embodiments described above are only illustrative. The units described as separate components may or may not be physically separated. The components shown as units may or may not be physical units, that is, they may be located in one place, or may be distributed to multiple network units. Some or all of the modules can be selected according to actual needs to achieve the purpose of the present invention. Those of ordinary skill in the art can understand and implement it without creative work.

[0126] The embodiment of the present invention also provides a computer-readable storage medium, on which a program is stored. When the program is executed by a processor, it implements the forward kinematics solution method of the high-precision real-time Stewart platform based on FPGA acceleration in the above embodiment.

[0127] The computer-readable storage medium may be an internal storage unit of any device with data processing capabilities described in any of the foregoing embodiments, such as a hard disk or memory. The computer-readable storage medium may also be an external storage device of any device with data processing capabilities, such as a plug-in hard disk, a Smart Media Card (SMC), an SD card, a Flash Card, etc. equipped on the device. Further, the computer-readable storage medium may also include both an internal storage unit and an external storage device of any device with data processing capabilities. The computer-readable storage medium is used to store the computer program and other programs and data required by any device with data processing capabilities, and may also be used to temporarily store data that has been output or is to be output.

[0128] The foregoing are only the preferred embodiments of one or more embodiments of this specification, and are not intended to limit one or more embodiments of this specification. Any modifications, equivalent replacements, improvements, etc. made within the spirit and principle of one or more embodiments of this specification shall be included within the scope of protection of one or more embodiments of this specification.

Claims

1. A forward kinematics solution method for a high-precision real-time Stewart platform based on FPGA acceleration, characterized in that, It includes the following steps: S1. Establish a Stewart platform model and determine the workspace. Specifically, according to the six coupling points P1, P2, P3, P4, P5, P6 of the real Stewart moving platform and the six coupling points B1, B2, B3, B4, B5, B6 of the static platform, establish a Stewart platform model. According to the length ranges of the six connecting rods, obtain the extreme poses of the moving platform and determine the workspace of the moving platform; S2. Select a data set according to the truncated Gaussian distribution and perform neural network fitting. Specifically: Given the pose ξ = [x, y, z, α, β, γ] of the moving platform relative to the static platform T , where α, β, and γ respectively represent the rotation angles of the moving platform around the x, y, and z axes, and x, y, and z respectively represent the displacements of the moving platform along the x, y, and z axes. The rotation matrix is calculated using the x - y - z type Euler angles. The rotation matrices R x , R y , R z and their rotation angles have the following functional relationships: The rotation matrix R of the moving platform with respect to the static platform is obtained by using the x-y-z type rotation matrix BP as follows: The solution formula for the vector of the i-th connecting rod leg[i] is as follows: leg[i] = T + R BP *P i -B i where T is the displacement of the moving platform relative to the static platform; the solution formula for the length of the i-th connecting rod is as follows: where i = 1 to 6, (x pi , y pi , z pi ) and (x bi , y bi , z bi ) are the coordinate representations of P i and B i in the static platform coordinate system respectively; after inverse solution, the link lengths corresponding to the pose are obtained. The link lengths are used as the input of the neural network, and the pose is used as the output of the neural network, forming a data set for neural network fitting; the neural network is trained and evaluated to fit the functional relationship between the lengths of the six links and the pose. S3. Quantize and deploy the neural network. Adopt a static quantization scheme to convert the trained floating-point neural network into an integer neural network. Specifically: Quantize the weights and activation values of each layer according to the scale factor and zero point respectively. The mapping relationship between the real floating-point number r and the quantized data Q is as follows: r≈s(Q-Z), where round(·) is the rounding operation, s is the scale factor, and Z is the zero point after quantization. The calculation formulas are as follows: where r max and r min represent the maximum floating-point value and the minimum floating-point value before quantization, respectively, and Q max and Q min represent the maximum integer value and the minimum integer value after quantization, respectively; S4. Implement the CORDIC sine and cosine solution, and pre-compute the Jacobian matrix and store it in the FPGA. Specifically: Set the number of iterations n of the CORDIC algorithm, and initialize the rotation angle θ for each time, where j = 0 to (n - 1). Pre-store θ through the lookup table of the FPGA. In each iteration, perform a positive or negative rotation at a pre-fixed angle, and sum the rotated angles. If the cumulative angle is greater than the target angle Φ, perform a negative rotation; otherwise, perform a positive rotation. After the rotation is completed, determine cosΦ and sinΦ by directly reading the x and y values of the final rotation vector. The rotation calculation formula is as follows: j , where j = 0 to (n - 1), and pre-store θ through the lookup table of the FPGA j . In each iteration, perform a positive or negative rotation at a pre-fixed angle, and sum the rotated angles. If the cumulative angle is greater than the target angle Φ, perform a negative rotation; otherwise, perform a positive rotation. After the rotation is completed, determine cosΦ and sinΦ by directly reading the x and y values of the final rotation vector. The rotation calculation formula is as follows: where R j (θ j ) is the rotation matrix for the j-th rotation, is the vector before the j-th rotation, is the vector after the j-th rotation; the rotation matrix is transformed into by trigonometric identities Among them, is the proportionality coefficient for each rotation operation. After determining the number of rotations, the proportionality coefficient is a fixed constant, which is pre-calculated and stored in the FPGA; S5. Use the output of the neural network as the initial value of iteration and obtain the forward solution pose through iteration. Specifically, reduce the computational complexity by pre-differentiating and storing the functional expression of the Jacobian matrix. The Jacobian matrix required for iteration is only a function of the pose ξ, and the expression of the Jacobian matrix is: Among them, f i is the inverse solution expression of the i-th connecting rod, The specific expressions of each element in the Jacobian matrix are obtained through the jacobian function in matlab and pre-stored in the FPGA; Assume L 6×1 is the link length of the correct solution input. Take L 6×1 as the input of the neural network to obtain the initial pose ξ0 of the iteration. Set the correct solution objective function as: F(ξ k ) = I(ξ k ) - L, where ξ k is the pose after the k-th iteration, I(·) is the inverse solution function, and the correct solution solving objective is F(ξ k ) = [0] 6×1 ; By performing multiple iterations on ξ k , the final correct solution pose is obtained. The iteration process is as follows: Among them, is the Jacobian matrix J, and its inverse matrix is obtained through LU decomposition; S6. Accelerate the forward solution through array segmentation, loop unrolling, and pipelining. Specifically, perform loop unrolling operations on the operations in the inner for loop, perform parallel processing on the non-coupled loop orders, and when the loop is fully unrolled, the operations in the inner loop are completed within one cycle; based on the pineline instruction, pipeline the initialization and loop parts in the rotation matrix multiplication so that each computing unit works simultaneously in each operation cycle; further accelerate the operation speed by performing array segmentation on the arrays A and B participating in the matrix multiplication operation. Each time during the operation, divide each row element in array A into separate registers according to dimension 2, and divide each column element in array B into separate registers according to dimension 1 for operation.

2. The forward kinematics solution method of the high-precision real-time Stewart platform based on FPGA acceleration according to claim 1, wherein The specific content of S1 is: Based on Simscape, model the Stewart platform, and build the model by connecting rigid bodies with hinges; select the body module and adjust its size to match the actual static platform; Determine the coordinates of the six coupling points of the static platform and place six flanges; add rotational joints at the coupling points, use a cross structure to connect the connecting rods and the static platform to form a universal hinge; the connection method between the moving platform and the connecting rods is the same; according to the length ranges of the six connecting rods, obtain the extreme poses of the moving platform, thereby determining the workspace of the moving platform.

3. The forward kinematics solution method for a high-precision real-time Stewart platform based on FPGA acceleration according to claim 1, characterized in that The selection of pose data in S2 is specifically: Each time, randomly combine six pose parameters selected based on the Gaussian distribution, and judge whether there are parameters that exceed the workspace among the six parameters. If so, discard this pose; select the poses where all six parameters are within the workspace and put them into the data set until the number of pose data reaches the preset number.

4. The forward kinematics solution method of the high-precision real-time Stewart platform based on FPGA acceleration according to claim 1, wherein In S2, a 4-layer BP neural network is used for training and evaluation. Each layer of the network contains 100 neurons to fit the functional relationship between the lengths of the six connecting rods and the pose.

5. The forward kinematics solution method of the high-precision real-time Stewart platform based on FPGA acceleration according to claim 1, characterized in that, In S3, evaluate the fitting effect of the neural network before and after quantization according to the loss value. Specifically, set the same training set and test set, and compare the loss values of the neural network after the same number of training rounds; the neural network fitting uses the root mean square error function as the loss function, that is: Among them, X obs,i is the true pose data in the dataset, and X model,i is the predicted pose data obtained by neural network fitting; calculate the root mean square error for the u pose data fitted in the test set to reflect the degree of deviation of the fitted pose from the true pose.

6. The forward kinematics solution method of the high-precision real-time Stewart platform based on FPGA acceleration according to claim 1, characterized in that In S4, the number of iterations n of the CORDIC algorithm is 20, and θ is pre-calculated j = arctan(2 -j ). Here, j = 0 to (n - 1). Then the first rotation angle is 45°, which is applicable to calculating the sine and cosine values from -90° to 90° in the inverse solution; the compensation coefficient K of the rotation vector is set to 7. The forward kinematics solution method of the high-precision real-time Stewart platform based on FPGA acceleration according to claim 1, characterized in that In S5, the calculation formula for the element value in the first row and first column of the Jacobian matrix is as follows: Among them, x b1 , y b1 , z b1 are the known coordinates of point B i , x p1 , y p1 , z p1 is a function of (x, y, z, α, β, γ) and is solved by the inverse equation .

8. The forward kinematics solution method of the high-precision real-time Stewart platform based on FPGA acceleration according to claim 1, characterized in that, In S6, the specific implementation method for matrix operation optimization is as follows: In the three-layer for loop of matrix multiplication, the innermost loop uses loop unrolling to limit the loop period to one clock cycle; the second layer loop uses pipeline parallelism for initialization and loop operations; Each time an array is fetched from memory, the elements of an entire row or column are stored in a separate register for array segmentation to achieve the purpose of accelerating the operation.

9. A high-precision real-time forward kinematics solving device for a Stewart platform based on FPGA acceleration, comprising a memory and one or more processors, wherein executable code is stored in the memory, and is characterized in that, When the processor executes the executable code, it is used to implement the forward kinematics solution method of the high-precision real-time Stewart platform based on FPGA acceleration as described in any one of claims 1-8.

10. A computer-readable storage medium having a program stored thereon, characterized in that, When the program is executed by the processor, it implements the forward kinematics solution method of the high-precision real-time Stewart platform based on FPGA acceleration as described in any one of claims 1-8.

Images