A forward and inverse kinematics solution method for a humanoid robot 2-SPS-1U parallel mechanism

Abstract

The application discloses a kind of human-shaped robot 2-SPS-1U parallel mechanism's positive and inverse kinematics solution method, it is related to human-shaped robot technical field, inverse kinematics solution includes obtaining wrist pitch angle, roll angle, SPS branch chain end ball hinge point inherent position, and the fixed translation vector of wrist rotating axle seat coordinate system relative to drive motor coordinate system;End ball hinge point is converted to real-time coordinates by constructing posture rotation matrix, combined with fixed translation vector conversion to drive motor coordinate system, the length of coordinate vector module is calculated Branch chain extension length.Positive kinematics solution includes obtaining branch chain measured length and constant parameter, with roll angle, pitch angle as unknown quantity to construct nonlinear equation group;It is solved by two-dimensional residual vector combined with Newton iteration method, Jacobian matrix is estimated by finite difference method in iteration, and when residual is less than threshold value, output posture angle.This method is accurate and stable, adapts to robot wrist joint control demand.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of humanoid robots, and particularly relates to a forward and inverse kinematics calculation method of a humanoid robot 2-SPS-1U parallel mechanism. BACKGROUND

[0002] The 2-SPS-1U parallel mechanism, as a spherical parallel mechanism with advantages of compact structure, light weight and strong bearing capacity, is composed of two SPS branch chains (spherical-movable-spherical) and one U pair (universal joint) constraint chain, and is widely applied to parts such as robot wrist joints which need multi-degree-of-freedom posture adjustment, and can realize large-range posture motion in a small space.

[0003] However, due to the complex spatial geometric constraint relationship of the 2-SPS-1U parallel mechanism, the existing forward and inverse kinematics calculation methods have problems of complicated calculation steps, non-uniform coordinate conversion logic or insufficient precision, which leads to deviation of the calculation results from actual requirements, and affects the posture control precision and response speed of the robot wrist. SUMMARY

[0004] To solve the technical problems in the prior art, the present application provides a forward and inverse kinematics calculation method of a humanoid robot 2-SPS-1U parallel mechanism.

[0005] The technical scheme adopted by the present application is as follows:

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

[0007] Obtaining a pitch angle of a wrist around a wrist rotation axis base coordinate system y axis, a roll angle of the wrist rotation axis base coordinate system x axis, inherent position data of a first SPS branch chain end spherical hinge point and a second SPS branch chain end spherical hinge point relative to the wrist rotation axis base coordinate system, a first fixed translation vector of the wrist rotation axis base coordinate system relative to a first driving motor coordinate system, and a second fixed translation vector of the wrist rotation axis base coordinate system relative to a second driving motor coordinate system;

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

[0009] Through the posture rotation matrix, the inherent position data of the first SPS branch chain end spherical hinge point and the inherent position data of the second SPS branch chain end spherical hinge point are respectively converted into real-time coordinate data relative to the wrist rotation axis base coordinate system;

[0010] The real-time coordinate data of the first SPS branch end spherical hinge point is converted into a first coordinate vector relative to the first driving motor coordinate system in combination with the first fixed translation vector; the real-time coordinate data of the second SPS branch end spherical hinge point is converted into a second coordinate vector relative to the second driving motor coordinate system in combination with the second fixed translation vector.

[0011] The module length of the first coordinate vector is calculated to obtain the extension length of the first SPS branch; the module length of the second coordinate vector is calculated to obtain the extension length of the second SPS branch.

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

[0013]

[0014] Wherein, 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 first SPS branch end spherical hinge point and the inherent position data of the second SPS branch end spherical hinge point into real-time coordinate data relative to the wrist rotation axis base coordinate system respectively comprises the following contents:

[0016] The inherent position of the first SPS branch end spherical hinge point relative to the wrist rotation axis base coordinate system is substituted into the posture rotation matrix in the form of a column vector, and the real-time coordinate data of the first SPS branch end spherical hinge point is obtained through multiplication operation of the matrix and the column vector;

[0017] The inherent position of the second SPS branch end spherical hinge point relative to the wrist rotation axis base coordinate system is substituted into the posture rotation matrix in the form of a column vector, and the real-time coordinate data of the second SPS branch end spherical hinge point is obtained through multiplication operation of the matrix and the column vector.

[0018] Preferably, the conversion of the real-time coordinate data of the first SPS branch end spherical hinge point into a first coordinate vector relative to the first driving motor coordinate system in combination with the first fixed translation vector; the conversion of the real-time coordinate data of the second SPS branch end spherical hinge point into a second coordinate vector relative to the second driving motor coordinate system in combination with the second fixed translation vector comprises the following contents:

[0019] The real-time coordinate data of the first SPS branch end spherical hinge point is subjected to vector addition operation with the first fixed translation vector to obtain the first coordinate vector;

[0020] The real-time coordinate data of the second SPS branch end spherical hinge point is subjected to vector addition operation with the second fixed translation vector to obtain the second coordinate vector.

[0021] The second aspect of the present application provides a forward kinematics solving method of a humanoid robot 2-SPS-1U parallel mechanism, comprising the following contents:

[0022] obtaining the stretch length of the first SPS branch, the stretch length of the second SPS branch, the inherent position data of the end spherical hinge point of the first SPS branch relative to the wrist rotation axis base coordinate system, the inherent position data of the end spherical hinge point of the second SPS branch relative to the coordinate system, the inherent position data of the fixed end spherical hinge point of the first SPS branch relative to the first driving motor coordinate system, the inherent position data of the fixed end spherical hinge point of the second SPS branch relative to the second driving motor coordinate system, the first fixed translation vector of the wrist rotation axis base coordinate system relative to the first driving motor coordinate system, and the second fixed translation vector relative to the second driving motor coordinate system;

[0023] characterizing the spatial posture of the wrist by the roll angle of the wrist around the x-axis of the wrist rotation axis base coordinate system and the pitch angle around the y-axis;

[0024] constructing a wrist posture rotation matrix based on the roll angle and the pitch angle, calculating the theoretical coordinate data of the end spherical hinge point of the first SPS branch relative to the first driving motor coordinate system in combination with the first fixed translation vector, and calculating the theoretical coordinate data of the end spherical hinge point of the second SPS branch relative to the second driving motor coordinate system in combination with the second fixed translation vector;

[0025] taking the distance residual error between the inherent position data of the fixed end spherical hinge point of the first SPS branch and the theoretical coordinate data of the end spherical hinge point of the first SPS branch as the first residual function value, taking the distance residual error between the inherent position data of the fixed end spherical hinge point of the second SPS branch and the theoretical coordinate data of the end spherical hinge point of the second SPS branch as the second residual function value, and forming a nonlinear equation group with the roll angle and the pitch angle as unknown variables;

[0026] constructing a two-dimensional residual error vector, and solving the nonlinear equation group by using the Newton iteration method: in each iteration process, the Jacobian matrix is estimated by the finite difference method, and the solution vector of the roll angle and the pitch angle is updated based on the Jacobian matrix and the current two-dimensional residual error vector;

[0027] if the deviation of the two-dimensional residual error vector corresponding to the updated solution vector is less than a preset threshold, it is determined that the iteration converges; and the converged roll angle and pitch angle are output as the actual posture angle data of the wrist.

[0028] Preferably, the nonlinear equation group is:

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

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

[0031] 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 spherical hinge point of the first SPS branch relative to the first driving motor coordinate system, n2 is the inherent position column vector of the fixed end spherical hinge point of the second SPS branch relative to the second driving motor coordinate system, T1 is the first fixed translation vector of the wrist rotating shaft seat coordinate system relative to the first driving motor coordinate system, T2 is the second fixed translation vector of the wrist rotating shaft seat coordinate system relative to the second driving motor coordinate system, m1 is the inherent position column vector of the end spherical hinge point of the first SPS branch relative to the wrist rotating shaft seat coordinate system, m2 is the inherent position column vector of the end spherical hinge point of the second SPS branch relative to the wrist rotating shaft seat coordinate system; R(θ,φ) is the attitude rotation matrix, L1 is the stretching length of the first SPS branch, and L2 is the stretching length of the second SPS branch.

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

[0033] f=[error1,error2] T

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

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

[0036]

[0037] wherein, x k+1 represents the Euler angle solution vector after the k+1 iteration update, x k represents the Euler angle solution vector at the k iteration, represents the inverse matrix of the Jacobian matrix, and f represents the two-dimensional residual vector.

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

[0039] The inverse kinematics solving method of the present application realizes the conversion of the inherent position of the end spherical hinge point of the SPS branch to the real-time coordinates through the attitude rotation matrix, accurately completes the cross-coordinate system conversion in combination with the fixed translation vector, and directly obtains the stretching length of the branch through the vector module length calculation, thereby reducing the solving error caused by the deviation of the intermediate calculation link and helping to improve the accuracy of the inverse kinematics solving.

[0040] The forward kinematics solving method of the application helps to reduce the dependence on initial values, improve the reliability of convergence of solving, and improve the stability of forward kinematics solving by constructing a nonlinear equation set and a two-dimensional residual vector, combining Newton iteration method, and using finite difference method to estimate Jacobian matrix. The preset residual threshold is used to judge the iteration convergence, the deviation of the roll angle and the pitch angle can be dynamically corrected in the solving process, the accuracy of the final output attitude angle is ensured, and the demand for accuracy of robot wrist joint attitude detection and feedback control is met. BRIEF DESCRIPTION OF DRAWINGS

[0041] Figure 1 Fig. 1 is a three-dimensional structure schematic diagram of a wrist parallel mechanism of the application;

[0042] Figure 2 Fig. 2 is a schematic diagram of a wrist parallel mechanism of the application;

[0043] Figure 3 Fig. 3 is a method flow schematic diagram of embodiment one of the application;

[0044] Figure 4 Fig. 4 is a method flow schematic diagram of embodiment two of the application. DETAILED DESCRIPTION

[0045] The embodiments of the application will be described in detail below with reference to the accompanying drawings.

[0046] As shown in Figure 1 and Figure 2 A1, A2, C1 and C2 are spherical hinges, B1 and B2 are electric push rods, O is a universal joint, and a coordinate system is defined at A1, A2 and O respectively.

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

[0048] Embodiment one provides a forward kinematics solving method of a humanoid robot 2-SPS-1U parallel mechanism, which is to solve the length L1 of the first SPS branch (i.e. the first electric push rod) B1 and the length L2 of the second SPS branch (i.e. the second electric push rod) B2, given the pitch angle of the y-axis and the roll angle of the x-axis of the wrist rotation axis coordinate system of the wrist.

[0049] Specifically, as shown in Figure 3 the method comprises the following steps:

[0050] Step 1, obtaining the pitch angle of the wrist around the y-axis of the wrist rotation axis base coordinate system, the roll angle of the wrist around the x-axis of the wrist rotation axis base coordinate system, the inherent position data of the first SPS branch end spherical hinge point and the second SPS branch end spherical hinge point relative to the wrist rotation axis base coordinate system, and the first fixed translation vector of the wrist rotation axis base coordinate system relative to the first driving motor coordinate system and the second fixed translation vector of the wrist rotation axis base coordinate system relative to the second driving motor coordinate system.

[0051] It should be noted that the wrist rotation axis base coordinate system is a three-dimensional rectangular coordinate system with the O point of the U pair as the origin, the x-axis along the lateral direction of the wrist (left-right direction), the y-axis along the longitudinal direction of the wrist (forward-backward direction), and the z-axis perpendicular to the end surface of the wrist (up-down direction), which is a reference coordinate system for describing the posture of the wrist; the pitch angle is the angle of rotation of the wrist around the y-axis of the wrist rotation axis base coordinate system, denoted as θ, and the forward inclination is positive and the backward inclination is negative; the roll angle is the angle of rotation of the wrist around the x-axis of the wrist rotation axis base coordinate system, denoted as φ, and the left inclination is positive and the right inclination is negative; the first SPS branch end spherical hinge point, i.e., the spherical hinge C1, is the connection point of the first SPS branch and the wrist; the second SPS branch end spherical hinge point, i.e., the spherical hinge C2, is the connection point of the second SPS branch and the wrist; the inherent position data refers to the fixed position of C1 and C2 relative to the wrist rotation axis base coordinate system, which is represented in the form of a column vector; the first fixed translation vector refers to the fixed position vector of the origin (O point) of the wrist rotation axis base coordinate system relative to the origin (A1) of the first driving motor coordinate system; and the second fixed translation vector refers to the fixed position vector of the origin (O point) of the wrist rotation axis base coordinate system relative to the origin (A2) of the second driving motor coordinate system.

[0052] Step 2, constructing a wrist posture rotation matrix based on the pitch angle and the roll angle.

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

[0054]

[0055] wherein 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 matrix R is a 3x3 matrix representing the rotation of the wrist around the x-axis and the y-axis, which is used to convert the inherent position of C1 and C2 into the real-time position under the current posture. When the wrist rotates, the spatial position of C1 and C2 will change with θ and φ. The rotation matrix can accurately describe the influence of posture change on position through linear algebra operation, avoiding the complexity of traditional geometric derivation. The conversion from posture to position is realized through the standardized rotation matrix, the operation logic is unified and reproducible, the errors caused by manual geometric calculation are reduced, and the accuracy of coordinate conversion is improved.

[0057] Step 3, converting the intrinsic position data of the first SPS branch end spherical hinge point and the intrinsic position data of the second SPS branch end spherical hinge point into real-time coordinate data of the relative wrist rotation axis base coordinate system respectively through the posture rotation matrix.

[0058] In a possible implementation, the converting the intrinsic position data of the first SPS branch end spherical hinge point and the intrinsic position data of the second SPS branch end spherical hinge point into real-time coordinate data of the relative wrist rotation axis base coordinate system respectively includes the following contents:

[0059] The intrinsic position of the first SPS branch end spherical hinge point relative to the wrist rotation axis base coordinate system is substituted into the posture rotation matrix in the form of a column vector, and real-time coordinate data of the first SPS branch end spherical hinge point is obtained through multiplication operation of the matrix and the column vector.

[0060] The intrinsic position of the second SPS branch end spherical hinge point relative to the wrist rotation axis base coordinate system is substituted into the posture rotation matrix in the form of a column vector, and real-time coordinate data of the second SPS branch end spherical hinge point is obtained through multiplication operation of 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 with the rotation matrix R generated in step 2 respectively:

[0062] Real-time coordinate of the first SPS branch end spherical hinge point (matrix multiplication column vector, the result is a 3x1 column vector); real-time coordinate of the second SPS branch end spherical hinge point .

[0063] Step 4, combining the first fixed translation vector, the real-time coordinate data of the first SPS branch end spherical hinge point is converted into a first coordinate vector relative to the first driving motor coordinate system; combining the second fixed translation vector, the real-time coordinate data of the second SPS branch end spherical hinge point is converted into a second coordinate vector relative to the second driving 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 first driving motor coordinate system, denoted as m1', which 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 second driving motor coordinate system, denoted as m2', which is used to calculate the extension length of the second SPS branch.

[0065] In a possible implementation, the real-time coordinate data of the first SPS branch end spherical hinge point is converted into a first coordinate vector relative to the first driving motor coordinate system in combination with the first fixed translation vector; and the real-time coordinate data of the second SPS branch end spherical hinge point is converted into a second coordinate vector relative to the second driving motor coordinate system in combination with the second fixed translation vector, including the following contents:

[0066] The real-time coordinate data of the first SPS branch end spherical hinge point is subjected to vector addition operation with the first fixed translation vector to obtain the first coordinate vector;

[0067] The real-time coordinate data of the second SPS branch end spherical hinge point is subjected to vector addition operation with the second fixed translation vector to obtain the second coordinate vector.

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

[0069] Step 5, the module length of the first coordinate vector is calculated to obtain the stretch length of the first SPS branch; and the module length of the second coordinate vector is calculated to obtain the stretch length of the second SPS branch.

[0070] In the specific implementation process, L1 = ||m1'||; L2 = ||m2'||; the stretch length of the branch is directly obtained through the vector module length, the calculation process is intuitive, there is no additional approximation, the result can be directly connected to the control instruction of the driving motor, and the conversion link and error from the calculation result to the actual control are reduced.

[0071] In view of the above scheme, in the precision detection scene, a slight attitude deviation of the wrist may be caused due to environmental micro-vibration, such as vibration conducted by workshop equipment, and then the detection precision is affected. To solve this problem, after step 5, in a possible implementation, the following contents are further included:

[0072] Step 6: a micro-vibration sensor is installed on the wrist rotating shaft seat of the detection device to collect the vibration displacement Δx, Δy, Δz of the wrist rotating shaft seat in the x, y, z axis directions in real time.

[0073] The micro-vibration sensor installation direction is aligned with the x, y, z axes of the wrist rotating shaft seat coordinate system, and the sampling frequency is ≥1 kHz, so as to ensure that the environmental micro-vibration of 10-50 Hz is captured.

[0074] Step 7: Low-pass filter the collected Δx, Δy, Δz to filter out the high-frequency vibration caused by the electric push rod drive; if the standard deviation of the filtered vibration displacement is less than the preset stable threshold, it is determined that it is environmental micro-vibration, and the subsequent calculation is entered; otherwise, an abnormal vibration alarm is triggered.

[0075] Step 8: Retrieve the distance I1 from point O to the spherical hinge point C1 at the end of the first SPS branch; convert the filtered vibration displacement into pitch angle deviation Δθ and roll angle deviation Δφ:

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

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

[0078] Where Δθ is the wrist rotation axis seat pitch angle offset caused by environmental micro-vibration, Δφ is the wrist rotation axis seat roll angle offset caused by environmental micro-vibration, and I1 is a constant value designed by the mechanism, i.e. the spatial distance constant from point O to C1.

[0079] It should be noted that the above conversion is based on the spatial geometric relationship of the wrist rotation axis seat, I1 is the force arm from point O to C1, Δz / Δy is the displacement of the force arm end caused by vibration, and arcsin operation conforms to the trigonometric function relationship between displacement and angle under small angle, realizing the conversion from linear parameters to attitude parameters: Δx, Δy, Δz, which are difficult to be directly used for attitude control, are converted into pitch angle offset and roll angle offset, which can be directly related to detection accuracy, providing quantifiable and controllable correction targets for subsequent compensation.

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

[0081] Step 10: Substitute θ 修正 , φ 修正 into the solving logic of steps 2-5 to reconstruct the rotation matrix R, convert the real-time coordinates of C1 / C2, convert m1' / m2', and calculate the length, to obtain the compensated extension lengths L1compensation, L2compensation.

[0082] The final output L1compensation, L2compensation can be directly connected to the drive motor control command, and the motor receives the command to drive the electric push rod to the target length, so that the wrist rotation axis seat is quickly adjusted to the compensated attitude: adapting to the needs of precision detection scene, ensuring that the influence of environmental micro-vibration is offset.

[0083] The second aspect of the present application provides a forward kinematics solving method of a humanoid robot 2-SPS-1U parallel mechanism, as shown in the following steps: Figure 4

[0084] In step 100, the length of the first SPS branch and the length of the second SPS branch are obtained; the inherent position data of the spherical hinge point at the end of the first SPS branch relative to the wrist rotation shaft base coordinate system, the inherent position data of the spherical hinge point at the end of the second SPS branch relative to the coordinate system, the inherent position data of the spherical hinge point at the fixed end of the first SPS branch relative to the first driving motor coordinate system, and the inherent position data of the spherical hinge point at the fixed end of the second SPS branch relative to the second driving motor coordinate system are obtained; the first fixed translation vector of the wrist rotation shaft base coordinate system relative to the first driving motor coordinate system and the second fixed translation vector relative to the second driving motor coordinate system are obtained.

[0085] In the specific implementation process, L1 and L2 are read by the displacement sensor of the electric push rod; the same constant parameters as in Example 1 are called: 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; and the inherent positions n1=(0, 0, 0) and n2=(0, 0, 0) of A1 and A2 are confirmed (driving motor coordinate system reference)

[0086] In step 200, the spatial pose of the wrist is represented by the roll angle of the wrist around the x-axis of the wrist rotation shaft base coordinate system and the pitch angle around the y-axis.

[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 the y-axis, and cannot rotate around the z-axis, so two Euler angles can completely represent the pose, avoiding the introduction of redundant unknown variables to cause complex calculation.

[0088] In step 300, a wrist pose rotation matrix is constructed based on the roll angle and the pitch angle, the theoretical coordinate data of the spherical hinge point at the end of the first SPS branch relative to the first driving motor coordinate system is calculated in combination with the first fixed translation vector, and the theoretical coordinate data of the spherical hinge point at the end of the second SPS branch relative to the second driving motor coordinate system is calculated in combination with the second fixed translation vector.

[0089] In step 400, the distance residual between the inherent position data of the spherical hinge point at the fixed end of the first SPS branch and the theoretical coordinate data of the spherical hinge point at the end of the first SPS branch is taken as the first residual function value, the distance residual between the inherent position data of the spherical hinge point at the fixed end of the second SPS branch and the theoretical coordinate data of the spherical hinge point at the end of the second SPS branch is taken as the second residual function value, and a nonlinear equation group with the roll angle and the pitch angle as unknown variables is formed.

[0090] In a possible implementation, the nonlinear equation group is:​

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

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

[0093] wherein, error1 is a first residual function value, error1 is a second residual function value; n1 is a first SPS branch fixed end spherical hinge point relative to the first driving motor coordinate system Intrinsic position column vector, n2 is a second SPS branch fixed end spherical hinge point relative to the second driving motor coordinate system Intrinsic position column vector, T1 is the first fixed translation vector of the wrist rotating shaft seat coordinate system relative to the first driving motor coordinate system, T2 is the second fixed translation vector of the wrist rotating shaft seat coordinate system relative to the second driving motor coordinate system, m1 is the first SPS branch end spherical hinge point relative to the wrist rotating shaft seat coordinate system Intrinsic position column vector, m2 is the second SPS branch end spherical hinge point relative to the wrist rotating shaft seat coordinate system Intrinsic position column vector; R(θ,φ) is an attitude rotation matrix, L1 is the extension length of the first SPS branch, L2 is the extension length of the second SPS branch.

[0094] Step 500, construct a two-dimensional residual error vector, and solve the nonlinear equation set by using Newton iteration method: in each iteration process, the Jacobian matrix is estimated by finite difference method, and the roll angle and pitch angle solution vector are updated based on the Jacobian matrix and the current two-dimensional residual error vector.

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

[0096] f=[error1,error2] T

[0097] wherein, f is a two-dimensional residual error vector, used to represent the overall deviation degree of the nonlinear equation set with the roll angle θ and the pitch angle φ as unknown variables.

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

[0099]

[0100] wherein, x k+1 represents the Euler angle solution vector after the k+1 iteration update, x k represents the Euler angle solution vector at the k iteration, represents the inverse matrix of the Jacobian matrix, and f represents the two-dimensional residual error vector.

[0101] In the embodiment, the finite difference method avoids complex analytical derivation and reduces the calculation difficulty of the Jacobian matrix; the second-order convergence characteristic of the Newton iteration method can quickly reduce the residual error, meeting the demand of real-time control of the robot on the solution speed

[0102] In step 600, if the deviation of the two-dimensional residual error vector corresponding to the updated solution vector is less than the preset threshold, it is determined that the iteration converges; 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 allowed residual error, and in the embodiment, the residual errors (error1 and error2) of the link lengths are set to 0.001 mm, which is used to quantitatively determine whether the attitude angle solution accuracy meets the demand;

[0104] The iteration convergence refers to that when the residual errors (error1 and error2) of the two link lengths are both less than the preset threshold 0.001, it is indicated that the solution vector tends to be stable, and iteration is not needed.

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

[0106] The residual error will gradually decrease during the iteration, but excessive iteration will waste computing resources, and the preset threshold can balance the solution accuracy and efficiency, so as to ensure that the output attitude angle can meet the control demand and will not occupy too much calculation time. The threshold can clearly define the convergence standard, avoid invalid iteration, and improve the solution efficiency; at the same time, the accuracy of the output attitude angle is ensured, and reliable parameter basis is provided for the wrist attitude feedback control.

[0107] The above-described embodiments only express the specific implementation of the present application, and the description is more specific and detailed, but it should not be understood as a limitation on the scope of the present patent. It should be noted that for ordinary skilled persons in the art, without departing from the concept of the present application, a number of modifications and improvements can be made, which are within the protection scope of the present application.

Claims

1. A method for inverse kinematics solution of a humanoid robot 2-SPS-1U parallel mechanism, 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 solving method of the humanoid robot 2-SPS-1U parallel mechanism according to claim 1, wherein, 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 solving method of the 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(0, p) · m1) || - L 1； error2 = || n2 - (T2 + R(0, f) · m2) || - L 2； 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, 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 6, 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 calculating the forward kinematics of a humanoid robot 2-SPS-1U parallel mechanism according to claim 7, 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