Forward and inverse kinematics resolving method for 2-SPS-1U parallel mechanism of humanoid robot

Abstract

The invention discloses a forward and inverse kinematics calculation method for a 2-SPS-1U parallel mechanism of a humanoid robot, and relates to the technical field of humanoid robots. Inverse kinematics calculation includes the steps that the pitch angle and the roll angle of a wrist, the inherent position of a spherical hinge point at the tail end of an SPS branched chain and the fixed translation vector of a wrist rotating shaft base coordinate system relative to a driving motor coordinate system are obtained; and constructing an attitude rotation matrix to convert the tail end spherical hinge point to a real-time coordinate, converting the real-time coordinate to a driving motor coordinate system by combining a fixed translation vector, and calculating a coordinate vector modulus length to obtain a branched chain telescopic length. The forward kinematics solution comprises the steps of obtaining the actually measured length and constant parameters of the branched chain, and constructing a nonlinear equation set by taking a roll angle and a pitch angle as unknown quantities; and solving by combining a two-dimensional residual vector with a Newton iteration method, estimating a Jacobian matrix by using a finite difference method in iteration, and outputting an attitude angle when the residual is smaller than a threshold value. The method is accurate and stable in calculation and adapts to the robot wrist joint control requirement.

Description

Technical Field

[0001] This invention relates to the field of humanoid robot technology, and in particular to a method for solving the forward and inverse kinematics of a parallel mechanism for a humanoid robot 2-SPS-1U. Background Technology

[0002] The 2-SPS-1U parallel mechanism is a spherical parallel mechanism with advantages such as compact structure, light weight, and strong load-bearing capacity. It consists of two SPS branches (ball-movement-ball) and one U-joint (universal joint) constraint chain. It is widely used in parts such as robot wrist joints that require multi-degree-of-freedom attitude adjustment and can achieve a large range of attitude movements in a small space.

[0003] However, due to the complex spatial geometric constraints of the 2-SPS-1U parallel mechanism, existing forward and inverse kinematics calculation methods suffer from cumbersome calculation steps, inconsistent coordinate transformation logic, or insufficient accuracy. This leads to deviations between the calculation results and actual requirements, affecting the posture control accuracy and response speed of the robot wrist. Summary of the Invention

[0004] To address the technical problems existing in the prior art, this invention provides a method for solving the forward and inverse kinematics of the parallel mechanism of the humanoid robot 2-SPS-1U.

[0005] The technical solution adopted in this invention is:

[0006] The first aspect of this application provides an inverse kinematics solution method for the parallel mechanism of a humanoid robot 2-SPS-1U, including the following:

[0007] Obtain the pitch angle and roll angle of the wrist about the wrist rotation axis coordinate system in the parallel mechanism; the inherent position data of the ball joint point at the end of the first SPS branch and the ball joint point at the end of the second SPS branch relative to the wrist rotation axis coordinate system; the first fixed translation vector of the wrist rotation axis coordinate system relative to the first drive motor coordinate system and the second fixed translation vector relative to the second drive motor coordinate system;

[0008] Based on the pitch and roll angles, a wrist posture rotation matrix is ​​constructed;

[0009] The inherent position data of the ball joint at the end of the first SPS branch and the inherent position data of the ball joint at the end of the second SPS branch are converted into real-time coordinate data relative to the wrist rotation axis coordinate system through the attitude rotation matrix.

[0010] Combining the first fixed translation vector, the real-time coordinate data of the ball joint point at the end of the first SPS branch is converted into a first coordinate vector relative to the first drive motor coordinate system; combining the second fixed translation vector, the real-time coordinate data of the ball joint point at the end of the second SPS branch is converted into a second coordinate vector relative to the second drive motor coordinate system.

[0011] Calculate the magnitude of the first coordinate vector to obtain the stretch length of the first SPS branch; calculate the magnitude of the second coordinate vector to obtain the stretch length of the second SPS branch.

[0012] Preferably, the wrist posture rotation matrix is:

[0013]

[0014] Where R is the wrist posture rotation matrix, θ is the pitch angle, and φ is the roll angle.

[0015] Preferably, the conversion of the inherent position data of the ball joint at the end of the first SPS branch and the inherent position data of the ball joint at the end of the second SPS branch into real-time coordinate data relative to the wrist rotation axis coordinate system includes the following:

[0016] The inherent position of the ball joint point at the end of the first SPS branch relative to the wrist rotation axis coordinate system is substituted into the attitude rotation matrix in the form of a column vector. The real-time coordinate data of the ball joint point at the end of the first SPS branch is obtained by multiplying the matrix and the column vector.

[0017] The inherent position of the ball joint point at the end of the second SPS branch relative to the wrist rotation axis coordinate system is substituted into the attitude rotation matrix in the form of a column vector. The real-time coordinate data of the ball joint point at the end of the second SPS branch is obtained by multiplying the matrix and the column vector.

[0018] Preferably, converting the real-time coordinate data of the ball joint point at the end of the first SPS branch into a first coordinate vector relative to the first drive motor coordinate system, in conjunction with the first fixed translation vector; and converting the real-time coordinate data of the ball joint point at the end of the second SPS branch into a second coordinate vector relative to the second drive motor coordinate system, in conjunction with the second fixed translation vector, includes the following:

[0019] The real-time coordinate data of the ball joint point at the end of the first SPS branch are added to the first fixed translation vector to obtain the first coordinate vector.

[0020] The real-time coordinates of the ball joint at the end of the second SPS branch are added to the second fixed translation vector to obtain the second coordinate vector.

[0021] The second aspect of this application provides a method for calculating the forward kinematics of a parallel mechanism of a humanoid robot 2-SPS-1U, including the following:

[0022] Obtain the telescopic length of the first SPS branch and the telescopic length of the second SPS branch; the inherent position data of the ball joint point at the end of the first SPS branch relative to the wrist rotation axis coordinate system, the inherent position data of the ball joint point at the end of the second SPS branch relative to the same coordinate system, the inherent position data of the ball joint point at the fixed end of the first SPS branch relative to the first drive motor coordinate system, and the inherent position data of the ball joint point at the fixed end of the second SPS branch relative to the second drive motor coordinate system; and the first fixed translation vector of the wrist rotation axis coordinate system relative to the first drive motor coordinate system and the second fixed translation vector relative to the second drive motor coordinate system.

[0023] The spatial attitude of the wrist is characterized by the roll angle around the x-axis and the pitch angle around the y-axis in the wrist rotation axis coordinate system.

[0024] Based on the roll angle and pitch angle, a wrist posture rotation matrix is ​​constructed. The theoretical coordinate data of the ball joint point at the end of the first SPS branch relative to the first drive motor coordinate system is calculated by combining the first fixed translation vector. The theoretical coordinate data of the ball joint point at the end of the second SPS branch relative to the second drive motor coordinate system is calculated by combining the second fixed translation vector.

[0025] The distance residual between the inherent position data of the ball joint at the fixed end of the first SPS branch and the theoretical coordinate data of the ball joint at the end of the first SPS branch is used as the first residual function value, and the distance residual between the inherent position data of the ball joint at the fixed end of the second SPS branch and the theoretical coordinate data of the ball joint at the end of the second SPS branch is used as the second residual function value, forming a nonlinear equation system with roll angle and pitch angle as unknown variables.

[0026] A two-dimensional residual vector is constructed, and the nonlinear equation system is solved using the Newton-Raphson iterative method: In each iteration, the Jacobian matrix is ​​estimated by the finite difference method, and the solution vectors of the roll angle and pitch angle are updated based on the Jacobian matrix and the current two-dimensional residual vector.

[0027] If the deviation of the two-dimensional residual vector corresponding to the updated solution vector is less than the preset threshold, the iteration is determined to be converged; the converged roll angle and pitch angle are output as the actual attitude angle data of the wrist.

[0028] Preferably, the nonlinear equation set is as follows:

[0029] error1=∥n1−(T1+R(θ,φ)⋅m1)∥−L1

[0030] error2=∥n2−(T2+R(θ,φ)⋅m2)∥−L2

[0031] Where error1 is the first residual function value and error2 is the second residual function value; n1 is the column vector of the inherent position of the fixed end ball joint of the first SPS branch relative to the coordinate system of the first drive motor, n2 is the column vector of the inherent position of the fixed end ball joint of the second SPS branch relative to the coordinate system of the second drive motor, T1 is the first fixed translation vector of the wrist rotation axis coordinate system relative to the coordinate system of the first drive motor, T2 is the second fixed translation vector of the wrist rotation axis coordinate system relative to the coordinate system of the second drive motor, m1 is the column vector of the inherent position of the end ball joint of the first SPS branch relative to the coordinate system of the wrist rotation axis, and m2 is the column vector of the inherent position of the end ball joint of the second SPS branch relative to the coordinate system of the wrist rotation axis; R(θ,φ) is the attitude rotation matrix, L1 is the extension length of the first SPS branch, and L2 is the extension length of the second SPS branch.

[0032] Preferably, the two-dimensional residual vector is:

[0033] f=[error1,error2] T

[0034] Where f is a two-dimensional residual vector, used to characterize the overall deviation of the nonlinear equation system with roll angle θ and pitch angle φ as unknown variables.

[0035] Preferably, the formula used to update the solution vectors of roll angle and pitch angle based on the Jacobian matrix and the current two-dimensional residual vector is:

[0036]

[0037] Where, x k+1 Let x represent the Euler angle solution vector after the (k+1)th iteration update. k This represents the Euler angle solution vector at the k-th iteration. Let f denote the inverse of the Jacobian matrix, and let f denote the two-dimensional residual vector.

[0038] The beneficial effects of the present invention are at least one of the following:

[0039] The inverse kinematics solution method of this invention realizes the transformation from the inherent position of the ball joint point at the end of the SPS branch to the real-time coordinates through the attitude rotation matrix, and accurately completes the cross-coordinate system conversion by combining a fixed translation vector. Then, the branch extension length is directly obtained by calculating the vector magnitude, which reduces the solution error caused by the deviation of the intermediate calculation link and helps to improve the accuracy of inverse kinematics solution.

[0040] The forward kinematics solution method of this invention, by constructing a system of nonlinear equations and a two-dimensional residual vector, combined with Newton's iterative method and employing the finite difference method to estimate the Jacobian matrix, helps reduce dependence on initial values, improves the reliability of solution convergence, and enhances the stability of forward kinematics solutions. By using a preset residual threshold to determine iterative convergence, deviations in roll and pitch angles can be dynamically corrected during the solution process, ensuring the accuracy of the final output attitude angle and meeting the accuracy requirements of robot wrist joint attitude detection and feedback control. Attached Figure Description

[0041] Figure 1 This is a three-dimensional structural diagram of the wrist parallel mechanism of the present invention;

[0042] Figure 2 This is a simplified schematic diagram of the wrist parallel mechanism of the present invention;

[0043] Figure 3 This is a schematic diagram of the method flow of Embodiment 1 of the present invention;

[0044] Figure 4 This is a schematic diagram of the method flow of Embodiment 2 of the present invention. Detailed Implementation

[0045] The embodiments of the present invention will now be described in detail with reference to the accompanying drawings.

[0046] like Figure 1 and Figure 2 As shown, A1, A2, C1, and C2 are spherical hinges, B1 and B2 are electric actuators, and point O is a universal joint hinge. A coordinate system is defined at points A1, A2, and O respectively.

[0047] The coordinates of A1 and A2 in coordinate system O are column vectors n1 and n2, respectively. The coordinates of C1 and C2 in coordinate system O are column vectors m1 and m2, respectively. The translations of coordinate system O relative to coordinate systems A1 and A2 are column vectors T1 and T2, respectively.

[0048] Example 1 provides an inverse kinematics solution method for the parallel mechanism of humanoid robot 2-SPS-1U. Inverse kinematics is to calculate the length L1 of the first SPS branch (i.e., the first electric actuator) B1 and the length L2 of the second SPS branch (i.e., the second electric actuator) B2 given the pitch angle of the wrist around the wrist rotation axis coordinate system and the roll angle of the wrist rotation axis coordinate system.

[0049] Specifically, such as Figure 3 As shown, it includes the following steps:

[0050] Step 1: Obtain the pitch angle of the wrist around the wrist rotation axis coordinate system and the roll angle of the wrist around the wrist rotation axis coordinate system in the parallel mechanism; the inherent position data of the ball joint point at the end of the first SPS branch and the ball joint point at the end of the second SPS branch relative to the wrist rotation axis coordinate system; the first fixed translation vector of the wrist rotation axis coordinate system relative to the first drive motor coordinate system and the second fixed translation vector relative to the second drive motor coordinate system.

[0051] It should be noted that the wrist rotation axis coordinate system is a three-dimensional rectangular coordinate system with point O as the origin of the U-joint hinge. The x-axis is along the horizontal direction of the wrist (left-right direction), the y-axis is along the vertical direction of the wrist (front-back direction), and the z-axis is perpendicular to the wrist end face (up-down direction). It is the reference coordinate system for describing wrist posture. The pitch angle is the angle of rotation of the wrist around the y-axis of the wrist rotation axis coordinate system, denoted as θ, with forward tilt being positive and backward tilt being negative. The roll angle is the angle of rotation of the wrist around the x-axis of the wrist rotation axis coordinate system, denoted as φ, with left tilt being positive and right tilt being negative. The ball joint point at the end of the first SPS branch is spherical. Hinge C1 is the connection point between the first SPS branch and the wrist; the ball joint point at the end of the second SPS branch, i.e., the ball joint C2, is the connection point between the second SPS branch and the wrist; the inherent position data refers to the fixed positions of C1 and C2 relative to the wrist rotation axis coordinate system, represented in the form of column vectors; the first fixed translation vector refers to the fixed position vector of the origin (O point) of the wrist rotation axis coordinate system relative to the origin (A1) of the first drive motor coordinate system; the second fixed translation vector refers to the fixed position vector of the origin (O point) of the wrist rotation axis coordinate system relative to the origin (A2) of the second drive motor coordinate system.

[0052] Step 2: Based on the pitch angle and roll angle, construct the wrist posture rotation matrix.

[0053] In one possible implementation, the wrist posture rotation matrix is:

[0054]

[0055] Where R is the wrist posture rotation matrix, θ is the pitch angle, and φ is the roll angle.

[0056] It should be noted that the wrist posture rotation moment R is a 3×3 matrix representing the wrist's rotation around the x-axis and y-axis, used to convert the inherent positions of C1 and C2 into their real-time positions under the current posture. During wrist rotation, the spatial positions of C1 and C2 change with θ and φ. The rotation matrix can accurately describe the impact of this posture change on position through linear algebraic operations, avoiding the complexity of traditional geometric derivations. Achieving posture-to-position transformation through a standardized rotation matrix ensures unified and reproducible computational logic, reduces errors caused by manual geometric calculations, and improves the accuracy of coordinate transformation.

[0057] Step 3: Using the attitude rotation matrix, the inherent position data of the ball joint at the end of the first SPS branch and the inherent position data of the ball joint at the end of the second SPS branch are converted into real-time coordinate data relative to the wrist rotation axis coordinate system.

[0058] In one possible implementation, converting the inherent position data of the ball joint at the end of the first SPS branch and the inherent position data of the ball joint at the end of the second SPS branch into real-time coordinate data relative to the wrist rotation axis coordinate system includes the following:

[0059] The inherent position of the ball joint point at the end of the first SPS branch relative to the wrist rotation axis coordinate system is substituted into the attitude rotation matrix in the form of a column vector. The real-time coordinate data of the ball joint point at the end of the first SPS branch is obtained by multiplying the matrix and the column vector.

[0060] The inherent position of the ball joint point at the end of the second SPS branch relative to the wrist rotation axis coordinate system is substituted into the attitude rotation matrix in the form of a column vector. The real-time coordinate data of the ball joint point at the end of the second SPS branch is obtained by multiplying the matrix and the column vector.

[0061] In the specific implementation process, the intrinsic position column vector m1 of C1 and the intrinsic position column vector m2 of C2 are multiplied by the rotation matrix R generated in step 2 respectively:

[0062] Real-time coordinates of the ball joint at the end of the first SPS branch (Matrix multiplication by column vector, resulting in a 3×1 column vector); Real-time coordinates of the ball joint at the end of the second SPS branch. .

[0063] Step 4: Combine the first fixed translation vector to convert the real-time coordinate data of the ball joint point at the end of the first SPS branch into a first coordinate vector relative to the first drive motor coordinate system; combine the second fixed translation vector to convert the real-time coordinate data of the ball joint point at the end of the second SPS branch into a second coordinate vector relative to the second drive motor coordinate system.

[0064] It should be noted that the first coordinate vector is a three-dimensional vector of the real-time coordinate data of C1 relative to the coordinate system of the first drive motor, denoted as m1', and is used to calculate the extension length of the first SPS branch; the second coordinate vector is a three-dimensional vector of the real-time coordinate data of C2 relative to the coordinate system of the second drive motor, denoted as m2', and is used to calculate the extension length of the second SPS branch.

[0065] In one possible implementation, converting the real-time coordinate data of the ball joint point at the end of the first SPS branch into a first coordinate vector relative to the first drive motor coordinate system, in conjunction with the first fixed translation vector; and converting the real-time coordinate data of the ball joint point at the end of the second SPS branch into a second coordinate vector relative to the second drive motor coordinate system, in conjunction with the second fixed translation vector, includes the following:

[0066] The real-time coordinate data of the ball joint point at the end of the first SPS branch are added to the first fixed translation vector to obtain the first coordinate vector.

[0067] The real-time coordinates of the ball joint at the end of the second SPS branch are added to the second fixed translation vector to obtain the second coordinate vector.

[0068] In the specific implementation process, the first coordinate vector Second coordinate vector Since the coordinates of A1 in the first drive motor coordinate system are (0,0,0), m1', which is the position vector of C1 relative to A1, directly corresponds to the length direction of the first SPS branch.

[0069] Step 5: Calculate the magnitude of the first coordinate vector to obtain the stretch length of the first SPS branch; calculate the magnitude of the second coordinate vector to obtain the stretch length of the second SPS branch.

[0070] In the specific implementation process, L1=||m1'||; L2=||m2'||; the branch extension length is directly obtained through the vector magnitude. The calculation process is intuitive and has no additional approximations. The results can be directly connected to the control commands of the drive motor, reducing the conversion links and errors from the solution results to the actual control.

[0071] Considering that in precision testing scenarios, the above solution may be affected by slight wrist posture shifts caused by environmental micro-vibrations, such as vibrations transmitted from workshop equipment, which could impact testing accuracy. To address this issue, after step 5, in one possible implementation, the following is also included:

[0072] Step 6: Install a miniature vibration sensor on the wrist rotation axis of the testing device to collect the vibration displacement Δx, Δy, and Δz of the wrist rotation axis in the x, y, and z axes in real time.

[0073] The miniature vibration sensor is installed with its orientation aligned with the x, y, and z axes of the wrist rotation axis coordinate system, and its sampling frequency is ≥1kHz to ensure the capture of environmental micro-vibrations of 10-50Hz.

[0074] Step 7: Perform low-pass filtering on the collected Δx, Δy, and Δz values ​​to filter out the high-frequency vibrations generated by the electric actuator. If the standard deviation of the filtered vibration displacement is less than the preset stability threshold, it is determined to be environmental micro-vibration and proceeds to subsequent calculations; otherwise, an abnormal vibration alarm is triggered.

[0075] Step 8: Obtain the distance I1 from point O to the ball joint point C1 at the end of the first SPS branch; calculate the pitch angle deviation Δθ and roll angle deviation Δφ corresponding to the filtered vibration displacement:

[0076] Δθ = arcsin(Δz / I1);

[0077] Δφ = arcsin(Δy / I1);

[0078] Where Δθ is the pitch angle offset of the wrist rotation axis caused by environmental micro-vibration, Δφ is the roll angle offset of the wrist rotation axis caused by environmental micro-vibration, and I1 is a fixed value of the mechanism design, that is, the constant spatial distance between point O and C1.

[0079] It should be noted that the above conversion is based on the spatial geometry of the wrist rotation axis. I1 is the lever arm from point O to C1, Δz / Δy is the displacement of the lever arm endpoint caused by vibration, and the arcsin operator combines the trigonometric function relationship between displacement and angle at small angles, realizing the conversion from linear parameters to attitude parameters: Δx, Δy, and Δz, which are difficult to use directly for attitude control, are converted into pitch angle offset and roll angle offset that can be directly correlated with detection accuracy after conversion, providing quantifiable and controllable correction targets for subsequent compensation.

[0080] Step 9: Superimpose the Δθ and Δφ calculated in Step 8 onto the initial target attitude angle to obtain the target attitude angle after vibration compensation: θ 修正 =θ 目标 -Δθ, φ 修正 =φ 目标 -Δφ.

[0081] Step 10: Set θ 修正 φ 修正 Substitute the solution logic from steps 2-5, reconstruct the rotation matrix R, transform the real-time coordinates of C1 / C2, convert m1' / m2', and calculate the modulus to obtain the compensated stretching lengths L1 compensation and L2 compensation.

[0082] The final output of L1 compensation and L2 compensation can be directly connected to the drive motor control command. After receiving the command, the motor can drive the electric push rod to extend and retract to the target length, so that the wrist rotation axis can be quickly adjusted to the compensated posture: adapting to the needs of precision detection scenarios and ensuring that the influence of environmental micro-vibrations is canceled out.

[0083] The second aspect of this application provides a method for solving the positive kinematics of a parallel mechanism of a humanoid robot 2-SPS-1U, such as... Figure 4 As shown, it includes the following steps:

[0084] Step 100: Obtain the extension length of the first SPS branch and the extension length of the second SPS branch; the inherent position data of the ball joint point at the end of the first SPS branch relative to the wrist rotation axis coordinate system, the inherent position data of the ball joint point at the end of the second SPS branch relative to the same coordinate system, the inherent position data of the ball joint point at the fixed end of the first SPS branch relative to the first drive motor coordinate system, and the inherent position data of the ball joint point at the fixed end of the second SPS branch relative to the second drive motor coordinate system; and the first fixed translation vector of the wrist rotation axis coordinate system relative to the first drive motor coordinate system and the second fixed translation vector relative to the second drive motor coordinate system.

[0085] In the specific implementation process, L1 and L2 are read by the displacement sensor built into the electric actuator; the same constant parameters as in Example 1 are retrieved: the inherent position m1 of C1, the inherent position m2 of C2, the first fixed translation vector T1, and the second fixed translation vector T2; the inherent positions n1=(0,0,0) and n2=(0,0,0) of A1 and A2 are confirmed (reference to the drive motor coordinate system).

[0086] Step 200: The spatial attitude of the wrist is characterized by the roll angle around the x-axis and the pitch angle around the y-axis of the wrist rotation axis coordinate system.

[0087] It should be noted that the wrist of the 2-SPS-1U mechanism is constrained by the U-pair and can only rotate around the x-axis and y-axis, but not around the z-axis. Therefore, its attitude can be fully represented by two Euler angles, avoiding the introduction of unnecessary unknown variables that would complicate the solution.

[0088] Step 300: Construct a wrist posture rotation matrix based on the roll angle and pitch angle, calculate the theoretical coordinate data of the ball joint point at the end of the first SPS branch relative to the coordinate system of the first drive motor by combining the first fixed translation vector, and calculate the theoretical coordinate data of the ball joint point at the end of the second SPS branch relative to the coordinate system of the second drive motor by combining the second fixed translation vector.

[0089] Step 400: The distance residual between the inherent position data of the fixed end ball joint point of the first SPS branch and the theoretical coordinate data of the end ball joint point of the first SPS branch is taken as the first residual function value, and the distance residual between the inherent position data of the fixed end ball joint point of the second SPS branch and the theoretical coordinate data of the end ball joint point of the second SPS branch is taken as the second residual function value, forming a nonlinear equation system with roll angle and pitch angle as unknown variables.

[0090] In one possible implementation, the nonlinear equation set is:

[0091] error1=∥n1−(T1+R(θ,φ)⋅m1)∥−L1

[0092] error2=∥n2−(T2+R(θ,φ)⋅m2)∥−L2

[0093] Wherein, error1 is the first residual function value, error2 is the second residual function value; n1 is the inherent position column vector of the fixed end ball joint point of the first SPS branch relative to the first drive motor coordinate system, n2 is the inherent position column vector of the fixed end ball joint point of the second SPS branch relative to the second drive motor coordinate system, T1 is the first fixed translation vector of the wrist rotation axis coordinate system relative to the first drive motor coordinate system, T2 is the second fixed translation vector of the wrist rotation axis coordinate system relative to the second drive motor coordinate system, m1 is the inherent position column vector of the end ball joint point of the first SPS branch relative to the wrist rotation axis coordinate system, m2 is the inherent position column vector of the end ball joint point of the second SPS branch relative to the wrist rotation axis coordinate system; R(θ,φ) is the attitude rotation matrix, L1 is the extension length of the first SPS branch, and L2 is the extension length of the second SPS branch.

[0094] Step 500: Construct a two-dimensional residual vector and solve the nonlinear equation system using the Newton-Raphson iterative method: In each iteration, estimate the Jacobian matrix using the finite difference method, and update the solution vectors of the roll angle and pitch angle based on the Jacobian matrix and the current two-dimensional residual vector.

[0095] In one possible implementation, the two-dimensional residual vector is:

[0096] f=[error1,error2] T

[0097] Where f is a two-dimensional residual vector, used to characterize the overall deviation of the nonlinear equation system with roll angle θ and pitch angle φ as unknown variables.

[0098] In one possible implementation, the formula used to update the solution vectors of roll and pitch angles based on the Jacobian matrix and the current two-dimensional residual vector is:

[0099]

[0100] Where, x k+1 Let x represent the Euler angle solution vector after the (k+1)th iteration update. k This represents the Euler angle solution vector at the k-th iteration. Let f denote the inverse of the Jacobian matrix, and let f denote the two-dimensional residual vector.

[0101] In this embodiment, the finite difference method avoids complex analytical differentiation, reducing the computational difficulty of the Jacobian matrix; the second-order convergence property of the Newton iteration method can quickly reduce the residual, meeting the speed requirements of real-time robot control.

[0102] Step 600: If the deviation of the two-dimensional residual vector corresponding to the updated solution vector is less than a preset threshold, the iteration is determined to be converged; the converged roll angle and pitch angle are output as the actual attitude angle data of the wrist.

[0103] The preset threshold refers to the maximum allowable value of the residual. In this embodiment, the residuals (error1, error2) of the link length are set to 0.001mm, which is used to quantitatively determine whether the attitude angle calculation accuracy meets the requirements.

[0104] Iterative convergence means that when the residuals (error1, error2) of the lengths of the two links are both less than the preset threshold of 0.001, it indicates that the solution vector tends to be stable and there is no need to continue iterating.

[0105] Actual attitude angle data refers to the roll angle and pitch angle corresponding to the converged solution vector, which can be directly used to feed back the current spatial attitude of the wrist.

[0106] During the iteration process, the residual gradually decreases, but excessive iteration wastes computational resources. Presetting a threshold can balance the accuracy and efficiency of the solution, ensuring that the output attitude angle meets the control requirements without consuming too much computation time. By defining the convergence criterion through the threshold, invalid iterations are avoided, and the solution efficiency is improved; at the same time, the accuracy of the output attitude angle is guaranteed, providing a reliable parameter basis for wrist attitude feedback control.

[0107] The embodiments described above are merely illustrative of specific implementations of the present invention, and while the descriptions are detailed, they should not be construed as limiting the scope of the present invention. It should be noted that those skilled in the art can make various modifications and improvements without departing from the concept of the present invention, and these modifications and improvements all fall within the scope of protection of the present invention.

Claims

1. A method for inverse kinematics calculation of a parallel mechanism of a humanoid robot 2-SPS-1U, characterized in that: Includes the following: Obtain the pitch angle and roll angle of the wrist about the wrist rotation axis coordinate system in the parallel mechanism; the inherent position data of the ball joint point at the end of the first SPS branch and the ball joint point at the end of the second SPS branch relative to the wrist rotation axis coordinate system; the first fixed translation vector of the wrist rotation axis coordinate system relative to the first drive motor coordinate system and the second fixed translation vector relative to the second drive motor coordinate system; Based on the pitch and roll angles, a wrist posture rotation matrix is ​​constructed; The inherent position data of the ball joint at the end of the first SPS branch and the inherent position data of the ball joint at the end of the second SPS branch are converted into real-time coordinate data relative to the wrist rotation axis coordinate system through the attitude rotation matrix. Combining the first fixed translation vector, the real-time coordinate data of the ball joint point at the end of the first SPS branch is converted into a first coordinate vector relative to the first drive motor coordinate system; combining the second fixed translation vector, the real-time coordinate data of the ball joint point at the end of the second SPS branch is converted into a second coordinate vector relative to the second drive motor coordinate system. Calculate the magnitude of the first coordinate vector to obtain the stretch length of the first SPS branch; calculate the magnitude of the second coordinate vector to obtain the stretch length of the second SPS branch.

2. The inverse kinematics solution method for the parallel mechanism of a humanoid robot 2-SPS-1U according to claim 1, characterized in that, The wrist posture rotation matrix is: Where R is the wrist posture rotation matrix, θ is the pitch angle, and φ is the roll angle.

3. The inverse kinematics solution method for a humanoid robot 2-SPS-1U parallel mechanism according to claim 2, characterized in that, The process of converting the inherent position data of the ball joint at the end of the first SPS branch and the inherent position data of the ball joint at the end of the second SPS branch into real-time coordinate data relative to the wrist rotation axis coordinate system includes the following: The inherent position of the ball joint point at the end of the first SPS branch relative to the wrist rotation axis coordinate system is substituted into the attitude rotation matrix in the form of a column vector. The real-time coordinate data of the ball joint point at the end of the first SPS branch is obtained by multiplying the matrix and the column vector. The inherent position of the ball joint point at the end of the second SPS branch relative to the wrist rotation axis coordinate system is substituted into the attitude rotation matrix in the form of a column vector. The real-time coordinate data of the ball joint point at the end of the second SPS branch is obtained by multiplying the matrix and the column vector.

4. The inverse kinematics solution method for a humanoid robot 2-SPS-1U parallel mechanism according to claim 2, characterized in that, Combining the first fixed translation vector, converting the real-time coordinate data of the ball joint point at the end of the first SPS branch into a first coordinate vector relative to the first drive motor coordinate system; combining the second fixed translation vector, converting the real-time coordinate data of the ball joint point at the end of the second SPS branch into a second coordinate vector relative to the second drive motor coordinate system includes the following: The real-time coordinate data of the ball joint point at the end of the first SPS branch are added to the first fixed translation vector to obtain the first coordinate vector. The real-time coordinates of the ball joint at the end of the second SPS branch are added to the second fixed translation vector to obtain the second coordinate vector.

5. A method for calculating the forward kinematics of a parallel mechanism of a humanoid robot 2-SPS-1U, characterized in that, Includes the following: Obtain the telescopic length of the first SPS branch and the telescopic length of the second SPS branch; the inherent position data of the ball joint point at the end of the first SPS branch relative to the wrist rotation axis coordinate system, the inherent position data of the ball joint point at the end of the second SPS branch relative to the same coordinate system, the inherent position data of the ball joint point at the fixed end of the first SPS branch relative to the first drive motor coordinate system, and the inherent position data of the ball joint point at the fixed end of the second SPS branch relative to the second drive motor coordinate system; and the first fixed translation vector of the wrist rotation axis coordinate system relative to the first drive motor coordinate system and the second fixed translation vector relative to the second drive motor coordinate system. The spatial attitude of the wrist is characterized by the roll angle around the x-axis and the pitch angle around the y-axis in the wrist rotation axis coordinate system. Based on the roll angle and pitch angle, a wrist posture rotation matrix is ​​constructed. The theoretical coordinate data of the ball joint point at the end of the first SPS branch relative to the first drive motor coordinate system is calculated by combining the first fixed translation vector. The theoretical coordinate data of the ball joint point at the end of the second SPS branch relative to the second drive motor coordinate system is calculated by combining the second fixed translation vector. The distance residual between the inherent position data of the ball joint at the fixed end of the first SPS branch and the theoretical coordinate data of the ball joint at the end of the first SPS branch is used as the first residual function value, and the distance residual between the inherent position data of the ball joint at the fixed end of the second SPS branch and the theoretical coordinate data of the ball joint at the end of the second SPS branch is used as the second residual function value, forming a nonlinear equation system with roll angle and pitch angle as unknown variables. A two-dimensional residual vector is constructed, and the nonlinear equation system is solved using the Newton-Raphson iterative method: In each iteration, the Jacobian matrix is ​​estimated by the finite difference method, and the solution vectors of the roll angle and pitch angle are updated based on the Jacobian matrix and the current two-dimensional residual vector. If the deviation of the two-dimensional residual vector corresponding to the updated solution vector is less than the preset threshold, the iteration is determined to be converged; the converged roll angle and pitch angle are output as the actual attitude angle data of the wrist.

6. The method for calculating the forward kinematics of a humanoid robot 2-SPS-1U parallel mechanism according to claim 5, characterized in that, The nonlinear equation system is as follows: error1=∥n1−(T1+R(θ,φ)⋅m1)∥−L1 error2=∥n2−(T2+R(θ,φ)⋅m2)∥−L2 Where error1 is the first residual function value and error2 is the second residual function value; n1 is the column vector of the inherent position of the fixed end ball joint of the first SPS branch relative to the coordinate system of the first drive motor, n2 is the column vector of the inherent position of the fixed end ball joint of the second SPS branch relative to the coordinate system of the second drive motor, T1 is the first fixed translation vector of the wrist rotation axis coordinate system relative to the coordinate system of the first drive motor, T2 is the second fixed translation vector of the wrist rotation axis coordinate system relative to the coordinate system of the second drive motor, m1 is the column vector of the inherent position of the end ball joint of the first SPS branch relative to the coordinate system of the wrist rotation axis, and m2 is the column vector of the inherent position of the end ball joint of the second SPS branch relative to the coordinate system of the wrist rotation axis; R(θ,φ) is the attitude rotation matrix, L1 is the extension length of the first SPS branch, and L2 is the extension length of the second SPS branch.

7. The method for calculating the forward kinematics of a humanoid robot 2-SPS-1U parallel mechanism according to claim 5, characterized in that, The two-dimensional residual vector is: f=[error1,error2] T Where f is a two-dimensional residual vector, used to characterize the overall deviation of the nonlinear equation system with roll angle θ and pitch angle φ as unknown variables.

8. The method for solving the forward and inverse kinematics of a parallel mechanism of a humanoid robot 2-SPS-1U according to claim 5, characterized in that, Based on the Jacobian matrix and the current two-dimensional residual vector, the formulas used to update the solution vectors for roll and pitch angles are as follows: Where, x k+1 Let x represent the Euler angle solution vector after the (k+1)th iteration update. k This represents the Euler angle solution vector at the k-th iteration. Let f denote the inverse of the Jacobian matrix, and let f denote the two-dimensional residual vector.

Images