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

Abstract

The application discloses a kind of positive and inverse kinematics solution method of humanoid robot 2-RSS-1U parallel mechanism, and relates to humanoid robot technical field, inverse kinematics solution method includes: establishing ankle shaft seat point coordinate system and obtaining sole pitch angle, horizontal roll angle;Based on the coordinate of the coordinate point of ankle connecting rod bottom relative to initial coordinate system is calculated based on space triangular relationship;Combination coordinate system known position relationship conversion to drive motor coordinate system;Again, according to the length of connecting rod and vertex coordinate, the angle of drive motor is calculated.The positive kinematics solution method includes: determining the constant parameters of mechanism and the fixed translation relationship of coordinate system;With roll angle, pitch angle as unknown quantity, construct nonlinear equation group;Solve by two-dimensional residual vector combined with Newton iteration method, estimate Jacobian matrix in iteration by finite difference method, and output posture angle according to residual threshold value to judge convergence.The positive and inverse kinematics solution method of the application helps to improve the stability and adaptability of solution.

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-RSS-1U. Background Technology

[0002] With the development of humanoid robots, bionic legged robots, and high-degree-of-freedom robotic arms, the requirements for attitude accuracy and real-time performance of end effectors are becoming increasingly stringent. Spherical parallel mechanisms, represented by the 2-RSS-1U parallel mechanism, are increasingly being widely used in high-dynamic applications such as those involving the ankle joint due to their compact structure, high stiffness, and strong load-bearing capacity. However, these parallel mechanisms contain multiple revolute and prismatic joints, resulting in complex geometric constraints and strong nonlinearity, making the kinematic solution of the platform quite challenging.

[0003] Traditional forward kinematics methods often employ geometric analytical or numerical approximation methods. However, analytical methods are complex to derive and lack versatility, while numerical methods are easily affected by initial values ​​and convergence, making it difficult to guarantee computational accuracy and stability. Furthermore, when manufacturing and assembly errors exist in the mechanism parameters, traditional methods lack robustness to these errors and cannot meet the requirements of real-time control. Summary of the Invention

[0004] To address the technical problems existing in the prior art, this invention provides a method for calculating the forward and inverse kinematics of the parallel mechanism of a humanoid robot 2-RSS-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-RSS-1U, including the following:

[0007] Establish the ankle axis coordinate system of the parallel mechanism, and obtain the pitch angle and roll angle of the foot plate rotating about the y-axis of the ankle axis coordinate system in the parallel mechanism.

[0008] Based on the pitch angle and roll angle, the first coordinate of the bottom coordinate point of the first ankle link in the parallel mechanism relative to the initial coordinate system and the second coordinate of the bottom coordinate point of the second ankle link relative to the initial coordinate system are calculated by spatial trigonometric relationships. The initial coordinate system is the reference coordinate system of the parallel mechanism in the initial motion state.

[0009] Based on the known positional relationship between the ankle pivot point coordinate system and the first drive motor coordinate system of the parallel mechanism in the initial state, the first coordinate is converted into a third coordinate relative to the first drive motor coordinate system; based on the known positional relationship between the ankle pivot point coordinate system and the second drive motor coordinate system of the parallel mechanism in the initial state, the second coordinate is converted into a fourth coordinate relative to the second drive motor coordinate system.

[0010] Obtain the fifth coordinate of the vertex of the first ankle link relative to the first drive motor coordinate system and the first length of the first ankle link; obtain the sixth coordinate of the vertex of the second ankle link relative to the second drive motor coordinate system and the second length of the second ankle link.

[0011] Based on the third coordinate, the fifth coordinate, and the first length, the first angle of the first drive motor is calculated; based on the fourth coordinate, the sixth coordinate, and the second length, the second angle of the second drive motor is calculated.

[0012] Preferably, the calculation of the first coordinate of the bottom coordinate point of the first ankle link relative to the initial coordinate system and the second coordinate of the bottom coordinate point of the second ankle link relative to the initial coordinate system based on the pitch angle and roll angle through spatial triangulation includes the following:

[0013] The platform attitude rotation matrix is ​​constructed using the pitch angle and roll angle as parameters. The expression for the platform attitude rotation matrix is ​​as follows:

[0014]

[0015] Where R is the platform attitude rotation matrix, θ is the pitch angle, and φ is the roll angle.

[0016] Based on the platform attitude rotation matrix, and combined with the inherent relative positions of the bottom coordinates of the first ankle link and the bottom coordinates of the second ankle link relative to the ankle axis coordinate system, the first coordinate of the bottom coordinates of the first ankle link relative to the initial coordinate system and the second coordinate of the bottom coordinates of the second ankle link relative to the initial coordinate system are calculated.

[0017] Preferably, the calculation of the first angle of the first drive motor based on the third coordinate, the fifth coordinate, and the first length includes the following:

[0018] Let the length of the first crank of the first drive motor be r1; when the angle of the first drive motor is θ1, the fifth coordinate n1 is (r1・cosθ1, r1・sinθ1, 0); the third coordinate m1 is (x, y, z).

[0019] Based on the third coordinate m1, the fifth coordinate n1, and the first length L1, the spatial distance equation is as follows:

[0020] ||m1-n1||²=L1²;

[0021] Expanding the spatial distance equation, we get r1² + ||m1||² - 2r1(x・cosθ1 + y・sinθ1) = L1²; simplifying the expanded equation into a trigonometric linear equation: 2r1x・cosθ1 + 2r1y・sinθ1 - (r1² + ||m1||² - L1²) = 0; solving the trigonometric linear equation yields the first angle θ1 of the first drive motor.

[0022] Preferably, after calculating the first angle of the first drive motor based on the third coordinate, the fifth coordinate, and the first length, and calculating the second angle of the second drive motor based on the fourth coordinate, the sixth coordinate, and the second length, the method further includes the following:

[0023] Obtain the pitch angle and roll angle of the previous target corresponding to the previous step phase in a continuous gait; obtain the pitch angle and roll angle of the next target corresponding to the next step phase in a continuous gait.

[0024] Set a preset interpolation step size and a preset interpolation algorithm; based on the preset interpolation step size and the preset interpolation algorithm, perform interpolation calculation on the pitch angle of the previous target and the pitch angle of the next target to obtain multiple intermediate pitch angles for the transition process between adjacent gait stages; based on the preset interpolation step size and the preset interpolation algorithm, perform interpolation calculation on the roll angle of the previous target and the roll angle of the next target to obtain multiple intermediate roll angles for the transition process between adjacent gait stages;

[0025] The multiple intermediate pitch angles and multiple intermediate roll angles are used one by one as parameters to construct the platform attitude rotation matrix; combined with the inherent relative position of the bottom coordinate point of the first ankle link relative to the ankle axis coordinate system, the intermediate first coordinate of the bottom coordinate point of the first ankle link relative to the initial coordinate system for each intermediate attitude during the transition process is calculated sequentially; combined with the inherent relative position of the bottom coordinate point of the second ankle link relative to the ankle axis coordinate system, the intermediate second coordinate of the bottom coordinate point of the second ankle link relative to the initial coordinate system for each intermediate attitude during the transition process is calculated sequentially.

[0026] Based on the known positional relationship between the ankle pivot point coordinate system and the first drive motor coordinate system in the initial state, the intermediate first coordinate is converted into the intermediate third coordinate relative to the first drive motor coordinate system; based on the known positional relationship between the ankle pivot point coordinate system and the second drive motor coordinate system in the initial state, the intermediate second coordinate is converted into the intermediate fourth coordinate relative to the second drive motor coordinate system.

[0027] Based on the intermediate third coordinate, fifth coordinate, and first length, multiple intermediate first angles of the first drive motor during the transition process are calculated sequentially; based on the intermediate fourth coordinate, sixth coordinate, and second length, multiple intermediate second angles of the second drive motor during the transition process are calculated sequentially.

[0028] During continuous gait execution, the first drive motor is controlled to gradually switch from the current first angle to the first angle of the next gait stage according to the sequence of the plurality of intermediate first angles; the second drive motor is controlled to gradually switch from the current second angle to the second angle of the next gait stage according to the sequence of the plurality of intermediate second angles.

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

[0030] The constant parameters and coordinate system relationships of the parallel mechanism are determined; wherein, the constant parameters include the first crank length of the first drive motor, the second crank length of the second drive motor, the first length, the second length, the first coordinate, and the second coordinate; the coordinate system relationships include a first fixed translation relationship between the coordinate system of the first drive motor and the coordinate system of the ankle pivot point, and a second fixed translation relationship between the coordinate system of the second drive motor and the coordinate system of the ankle pivot point.

[0031] The foot attitude of the parallel mechanism is characterized by two Euler angles, namely the roll angle of the foot about the x-axis of the ankle axis coordinate system and the pitch angle of the foot about the y-axis of the ankle axis coordinate system.

[0032] Based on the roll angle and pitch angle, taking the first coordinate of the bottom coordinate point of the first ankle link and the second coordinate of the bottom coordinate point of the second ankle link as references, the first coordinate and the second coordinate are rotated by the platform attitude rotation matrix, and then the first fixed translation relationship and the second fixed translation relationship are superimposed respectively to construct a nonlinear equation system with two Euler angles as unknown variables.

[0033] Construct a two-dimensional residual vector corresponding to the nonlinear equation system, solve the nonlinear equation system using the Newton-Raphson iterative method, estimate the Jacobian matrix using the finite difference method in each iteration, and update the solution vectors of the two Euler angles based on the Jacobian matrix and the current residual vector.

[0034] The residual corresponding to the solution vector after each update is judged. If the residual is less than a preset threshold, the iteration is determined to be converged, and the two Euler angles corresponding to the current solution vector are output as the attitude angles of the foot plate.

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

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

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

[0038] Where error1 is the first residual function value, error2 is the second residual function value; n1 is the fifth coordinate, n2 is the sixth coordinate, T1 is the first fixed translation relationship between the first drive motor coordinate system and the ankle axis coordinate system, T2 is the second fixed translation relationship between the second drive motor coordinate system and the ankle axis coordinate system, P1 is the first coordinate, P2 is the second coordinate; R(θ,φ) is the platform attitude rotation matrix, L1 is the first length, L2 is the second length.

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

[0040] f=[error1,error2] T

[0041] 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.

[0042] Preferably, the update formula used to update the solution vectors of the two Euler angles based on the Jacobian matrix and the current residual vector is:

[0043]

[0044] 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.

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

[0046] The inverse kinematics solution method of this invention calculates the coordinates of the bottom coordinate point of the ankle link based on spatial trigonometric relationships, and derives the motor angle by combining the known link length and vertex coordinates. This avoids the complex analytical derivation process, reduces the calculation deviations introduced by the cumbersome derivation, and helps to improve the stability of the inverse kinematics solution.

[0047] The forward kinematics solution method of this invention simplifies the derivation of forward kinematics by constructing a system of nonlinear equations with roll angle and pitch angle as unknown variables and defining a clear two-dimensional residual vector as the objective function. It also helps to reduce the dependence of traditional numerical methods on initial values ​​and improve the reliability of solution convergence by estimating the Jacobian matrix through the finite difference method. Furthermore, by judging the convergence of iteration through a preset residual threshold, it can dynamically correct the solution vector deviation during the iteration process, which helps to improve the robustness of forward kinematics solution to actual working conditions and meet the needs of robot ankle joint posture detection and feedback control. Attached Figure Description

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

[0049] Figure 2 This is a simplified diagram of the ankle of the 2-RSS-1U parallel mechanism of the present invention;

[0050] Figure 3 This is a schematic diagram of the ankle three-dimensional structure of the 2-RSS-1U parallel mechanism of the present invention.

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

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

[0053] Spherical parallel mechanisms, represented by the 2-RSS-1U parallel mechanism, are increasingly being widely used in high-dynamic conditions such as those involving the foot and ankle joints due to their compact structure, high stiffness, and strong load-bearing capacity. However, these parallel mechanisms have multiple revolute and prismatic joints, resulting in complex geometric constraints and strong nonlinearity, which makes the kinematic solution of the platform quite difficult.

[0054] like Figure 2 As shown, there are two drive schemes for the foot plate. The first scheme arranges the ankle motor shaft in the forward-backward direction, and the second scheme arranges the ankle motor shaft in the left-right direction. M1 represents the first drive motor, M2 represents the second drive motor, A1, A2, B1, and B2 are spherical hinges, point O is a universal joint hinge, and the link between B1, B2, and O is the foot plate. Although these two arrangements are different, their calculation methods are the same.

[0055] Example 1 provides an inverse kinematics solution method for the parallel mechanism of the humanoid robot 2-RSS-1U, such as... Figure 1 As shown, it includes the following steps:

[0056] Step 1: Establish the ankle axis coordinate system of the parallel mechanism, and obtain the pitch angle and roll angle of the foot plate rotating around the y-axis of the ankle axis coordinate system in the parallel mechanism.

[0057] It should be noted that, as Figure 2 and Figure 3 As shown, the ankle axis coordinate system is a three-dimensional rectangular coordinate system established with point O (the center of the universal joint hinge) as the origin. The x-axis is along the horizontal direction of the sole of the foot (left and right), the y-axis is along the vertical direction of the sole of the foot (front and back), and the z-axis is perpendicular to the sole of the foot and upward (vertical direction). It is the reference coordinate system for describing the posture of the sole of the foot.

[0058] The pitch angle (θ) refers to the angle of rotation of the foot about the y-axis of the ankle axis coordinate system; forward tilt is positive, and backward tilt is negative. The roll angle (φ) refers to the angle of rotation of the foot about the x-axis of the ankle axis coordinate system; leftward tilt is positive, and rightward tilt is negative. The attitude of the foot is determined by the two degrees of freedom of pitch and roll. Establishing the ankle axis coordinate system can uniformly describe the rotational reference of these two angles, providing a spatial reference for subsequent coordinate calculations.

[0059] Step 2: Based on the pitch angle and roll angle, calculate the first coordinate of the bottom coordinate point of the first ankle link in the parallel mechanism relative to the initial coordinate system and the second coordinate of the bottom coordinate point of the second ankle link relative to the initial coordinate system through spatial trigonometric relationships. The initial coordinate system is the reference coordinate system of the parallel mechanism in its initial motion state.

[0060] It should be noted that, as Figure 2 As shown, the bottom coordinate point of the first ankle link refers to the endpoint where the first ankle link connects to the sole plate, which is also the center point of the spherical hinge B1; the bottom coordinate point of the second ankle link refers to the endpoint where the second ankle link connects to the sole plate, which is also the center point of the spherical hinge B2; the initial coordinate system refers to the reference coordinate system when the parallel mechanism is in its initial state (sole plate horizontal), which coincides with the origin of the ankle axis coordinate system and has the same axis direction; the first coordinate refers to the three-dimensional coordinate of B1 in the initial coordinate system; the second coordinate refers to the three-dimensional coordinate of B2 in the initial coordinate system.

[0061] In one possible implementation, calculating the first coordinates of the bottom coordinates of the first ankle link relative to the initial coordinate system and the second coordinates of the bottom coordinates of the second ankle link relative to the initial coordinate system based on the pitch and roll angles and using spatial triangulation includes the following:

[0062] The platform attitude rotation matrix is ​​constructed using the pitch angle and roll angle as parameters. The expression for the platform attitude rotation matrix is ​​as follows:

[0063]

[0064] Where R is the platform attitude rotation matrix, θ is the pitch angle, and φ is the roll angle.

[0065] Based on the platform attitude rotation matrix, and combined with the inherent relative positions of the bottom coordinates of the first ankle link and the bottom coordinates of the second ankle link relative to the ankle axis coordinate system, the first coordinate of the bottom coordinates of the first ankle link relative to the initial coordinate system and the second coordinate of the bottom coordinates of the second ankle link relative to the initial coordinate system are calculated.

[0066] In this embodiment, when the foot plate rotates, the spatial positions of B1 and B2 change with the attitude angles (θ, φ). The coordinates in the initial state can be converted into coordinates in the current attitude through the rotation matrix, reflecting the spatial triangular relationship. This can accurately describe the influence of attitude changes on the positions of B1 and B2, avoid the complexity of traditional geometric derivation, and improve the accuracy of coordinate calculation.

[0067] Step 3: Based on the known positional relationship between the ankle pivot point coordinate system and the first drive motor coordinate system of the parallel mechanism in the initial state, the first coordinate is converted into a third coordinate relative to the first drive motor coordinate system; based on the known positional relationship between the ankle pivot point coordinate system and the second drive motor coordinate system of the parallel mechanism in the initial state, the second coordinate is converted into a fourth coordinate relative to the second drive motor coordinate system.

[0068] It should be noted that the first drive motor coordinate system refers to a three-dimensional coordinate system with the rotation center of M1 as the origin, used to describe the position of components such as M1 and A1; the second drive motor coordinate system refers to a three-dimensional coordinate system with the rotation center of M2 as the origin, used to describe the position of components such as M2 and A2; the third coordinate refers to the three-dimensional coordinate of B1 in the M1 coordinate system; and the fourth coordinate refers to the three-dimensional coordinate of B2 in the M2 coordinate system.

[0069] The known positional relationship refers to the spatial position vector of the origin (point O) of the ankle axis coordinate system relative to the origins of the M1 and M2 coordinate systems in the initial state.

[0070] In this step, the calculation of the drive motor angle needs to be based on its own coordinate system. Therefore, the first and second coordinates in the initial coordinate system need to be transformed to the M1 and M2 coordinate systems through a fixed translation relationship. The coordinate cross-system transformation is achieved through the fixed translation relationship to ensure that the subsequent motor angle calculation is based on the correct spatial reference system, reduce coordinate system transformation errors, and ensure the accuracy of geometric constraint calculation.

[0071] Step 4: Obtain the fifth coordinate of the vertex of the first ankle link relative to the coordinate system of the first drive motor and the first length of the first ankle link; obtain the sixth coordinate of the vertex of the second ankle link relative to the coordinate system of the second drive motor and the second length of the second ankle link.

[0072] like Figure 2 and Figure 3 As shown, the vertex of the first ankle link is A1, the vertex of the second ankle link is A2, the fifth coordinate is the three-dimensional coordinate of A1 in the M1 coordinate system, the sixth coordinate is the three-dimensional coordinate of A2 in the M2 coordinate system, the first length L1 refers to the fixed length of the first ankle link A1-B1, and the second length L2 refers to the fixed length of the second ankle link A2-B2.

[0073] Step 5: Calculate the first angle of the first drive motor based on the third coordinate, the fifth coordinate, and the first length; calculate the second angle of the second drive motor based on the fourth coordinate, the sixth coordinate, and the second length.

[0074] In one possible implementation, the calculation of the first angle of the first drive motor based on the third coordinate, the fifth coordinate, and the first length includes the following:

[0075] Let the length of the first crank of the first drive motor be r1; when the angle of the first drive motor is θ1, the fifth coordinate n1 is (r1・cosθ1, r1・sinθ1, 0); the third coordinate m1 is (x, y, z).

[0076] Based on the third coordinate m1, the fifth coordinate n1, and the first length L1, the spatial distance equation is as follows:

[0077] ||m1-n1||²=L1²;

[0078] Expanding the spatial distance equation, we get r1² + ||m1||² - 2r1(x・cosθ1 + y・sinθ1) = L1²; simplifying the expanded equation into a trigonometric linear equation: 2r1x・cosθ1 + 2r1y・sinθ1 - (r1² + ||m1||² - L1²) = 0; solving the trigonometric linear equation yields the first angle θ1 of the first drive motor.

[0079] Similarly, the second angle θ2 of the second drive motor can be obtained. In this step, the motor angle is directly solved through geometric constraint equations, avoiding complex analytical derivation. The calculation process is stable, and the results can be directly used for motor control.

[0080] In one possible implementation, considering that the above solution can only calculate the motor angle corresponding to a single posture, in scenarios of continuous robot gait (such as the switching from the support phase to the swing phase during walking or running), directly switching the target motor angle between the preceding and following gait phases can easily lead to abrupt angle changes, causing motor impact and mechanism vibration, which in turn can cause robot gait lag, center of gravity imbalance, and affect motion stability. To solve this technical problem, the first angle of the first drive motor is calculated based on the third coordinate, the fifth coordinate, and the first length; after calculating the second angle of the second drive motor based on the fourth coordinate, the sixth coordinate, and the second length, the following is also included:

[0081] Obtain the pitch and roll angles of the previous target in the previous step phase of a continuous gait; obtain the pitch and roll angles of the next target in the next step phase of a continuous gait.

[0082] Set a preset interpolation step size and a preset interpolation algorithm; based on the preset interpolation step size and the preset interpolation algorithm, perform interpolation calculation on the pitch angle of the previous target and the pitch angle of the next target to obtain multiple intermediate pitch angles for the transition process between adjacent gait stages; based on the preset interpolation step size and the preset interpolation algorithm, perform interpolation calculation on the roll angle of the previous target and the roll angle of the next target to obtain multiple intermediate roll angles for the transition process between adjacent gait stages.

[0083] The preset interpolation step size refers to the angular interval between two adjacent intermediate attitude angles (intermediate pitch angle / intermediate roll angle), which is used to control the fineness of attitude changes during gait transition; the step size unit can be set to angle or radian. For example, if the angle interval for each sampling is 0.4 degrees, the corresponding radian interval is approximately 0.00698 radians (radians = angle × π / 180).

[0084] The smaller the preset interpolation step size, the more intermediate pose angles are generated, and the smoother the pose transition, but more inverse kinematics calculations need to be performed; the larger the step size, the fewer intermediate angles are generated, and the higher the computational efficiency, but abrupt pose changes may occur, and it needs to be set according to the humanoid robot's gait speed and control accuracy requirements.

[0085] The preset interpolation algorithm refers to an algorithm used to numerically interpolate the previous and subsequent target attitude angles in adjacent gait phases, generating multiple intermediate attitude angles (intermediate pitch angle / intermediate roll angle) during the transition. Its purpose is to avoid abrupt attitude changes during gait transitions and ensure smooth robot ankle movement. Specifically: using the previous target pitch angle θ1 and previous target roll angle φ1 in the previous gait as starting values, and the subsequent target pitch angle θ2 and subsequent target roll angle φ2 in the next gait as ending values; sampling is performed according to the preset interpolation step size, uniformly sampling the pitch angle (from θ1 to θ2) and roll angle (from φ1 to φ2) according to the step size, resulting in two sets of one-to-one corresponding intermediate attitude angle sequences (θ1, θ2, φ1, φ2 ... k ,φ k (k=1,2,...,n, where n is the number of intermediate attitude angles).

[0086] The multiple intermediate pitch angles and multiple intermediate roll angles are used one by one as parameters to construct the platform attitude rotation matrix; combined with the inherent relative position of the bottom coordinate point of the first ankle link relative to the ankle axis coordinate system, the intermediate first coordinate of the bottom coordinate point of the first ankle link relative to the initial coordinate system for each intermediate attitude during the transition process is calculated sequentially; combined with the inherent relative position of the bottom coordinate point of the second ankle link relative to the ankle axis coordinate system, the intermediate second coordinate of the bottom coordinate point of the second ankle link relative to the initial coordinate system for each intermediate attitude during the transition process is calculated sequentially.

[0087] Based on the known positional relationship between the ankle pivot point coordinate system and the first drive motor coordinate system in the initial state, the intermediate first coordinate is converted into the intermediate third coordinate relative to the first drive motor coordinate system; based on the known positional relationship between the ankle pivot point coordinate system and the second drive motor coordinate system in the initial state, the intermediate second coordinate is converted into the intermediate fourth coordinate relative to the second drive motor coordinate system.

[0088] Based on the intermediate third coordinate, fifth coordinate and first length, multiple intermediate first angles of the first drive motor during the transition process are calculated sequentially; based on the intermediate fourth coordinate, sixth coordinate and second length, multiple intermediate second angles of the second drive motor during the transition process are calculated sequentially.

[0089] During continuous gait execution, the first drive motor is controlled to gradually switch from the current first angle to the first angle of the next gait stage according to the sequence of the plurality of intermediate first angles; the second drive motor is controlled to gradually switch from the current second angle to the second angle of the next gait stage according to the sequence of the plurality of intermediate second angles.

[0090] In the specific implementation process, the values ​​of θ1 and φ1 in the previous step and θ2 and φ2 in the next step are obtained; the interpolation step size (e.g., 0.1°) and algorithm (e.g., linear interpolation) are set to generate the intermediate angle sequence θ. k φ k (k=1,2,...,n); For each intermediate angle, repeat steps 2-5 to calculate the intermediate coordinates and intermediate motor angles; control M1 and M2 to gradually switch to the target angle according to the intermediate angle sequence, thereby avoiding sudden changes in motor angles, reducing mechanism vibration and impact, and improving the smoothness of continuous motion and the fluidity of robot gait.

[0091] Example 2 provides a method for solving the forward kinematics of the parallel mechanism of the humanoid robot 2-RSS-1U, such as... Figure 4 As shown, it includes the following steps:

[0092] Step 100: Determine the constant parameters and coordinate system relationships of the parallel mechanism; wherein, the constant parameters include the first crank length of the first drive motor, the second crank length of the second drive motor, the first length, the second length, the first coordinate, and the second coordinate; the coordinate system relationships include the first fixed translation relationship between the coordinate system of the first drive motor and the coordinate system of the ankle pivot point, and the second fixed translation relationship between the coordinate system of the second drive motor and the coordinate system of the ankle pivot point.

[0093] In this embodiment, the constant parameters include the first crank length r1 of the first drive motor, the second crank length r2 of the second drive motor, the first length L1 of the first ankle link, the second length L2 of the second ankle link, the first coordinate P1 of the bottom coordinate point of the first ankle link, and the second coordinate P2 of the bottom coordinate point of the second ankle link.

[0094] The first fixed translation relationship T1 refers to the fixed translation vector between the first drive motor coordinate system and the ankle axis coordinate system; the second fixed translation relationship T2 refers to the fixed translation vector between the second drive motor coordinate system and the ankle axis coordinate system.

[0095] Step 200: Determine that the foot plate posture of the parallel mechanism is characterized by two Euler angles, wherein the two Euler angles are the roll angle of the foot plate rotating about the x-axis of the ankle axis coordinate system and the pitch angle of the foot plate rotating about the y-axis of the ankle axis coordinate system.

[0096] Step 300: Based on the roll angle and pitch angle, using the first coordinate of the bottom coordinate point of the first ankle link and the second coordinate of the bottom coordinate point of the second ankle link as references, the first coordinate and the second coordinate are rotated by the platform attitude rotation matrix, and then the first fixed translation relationship and the second fixed translation relationship are superimposed respectively to construct a nonlinear equation system with two Euler angles as unknown variables.

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

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

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

[0100] Where error1 is the first residual function value, error2 is the second residual function value; n1 is the fifth coordinate, n2 is the sixth coordinate, T1 is the first fixed translation relationship between the first drive motor coordinate system and the ankle axis coordinate system, T2 is the second fixed translation relationship between the second drive motor coordinate system and the ankle axis coordinate system, P1 is the first coordinate, P2 is the second coordinate; R(θ,φ) is the platform attitude rotation matrix, L1 is the first length, L2 is the second length.

[0101] In this embodiment, the problem of determining the attitude from a known motor angle in forward kinematics is transformed into a problem of solving a two-dimensional nonlinear equation system. The logic of the equations is clarified through geometric constraints, providing a quantitative target for subsequent iterative solutions.

[0102] Step 400: Construct the two-dimensional residual vector corresponding to the nonlinear equation system, solve the nonlinear equation system using the Newton-Raphson iteration method, estimate the Jacobian matrix using the finite difference method in each iteration, and update the solution vectors of the two Euler angles based on the Jacobian matrix and the current residual vector.

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

[0104] f=[error1,error2] T

[0105] 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.

[0106] In one possible implementation, the update formula used to update the solution vectors of the two Euler angles based on the Jacobian matrix and the current residual vector is:

[0107]

[0108] 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.

[0109] In this step, Newton's iteration method iteratively corrects the unknowns θ and φ, gradually converging the residual vector to 0, thus obtaining the accurate attitude angle. The finite difference method avoids complex analytical differentiation, improving the method's versatility. Furthermore, Newton's iteration method has second-order convergence, and combined with the finite difference method to estimate the Jacobian matrix, high-precision solutions can be achieved in fewer iterations, solving the problems of slow convergence and sensitivity to initial values ​​in traditional numerical methods.

[0110] Step 500: Judge the residual corresponding to the solution vector after each update. If the residual is less than a preset threshold, the iteration is determined to be converged, and the two Euler angles corresponding to the current solution vector are output as the attitude angles of the foot plate.

[0111] 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.

[0112] 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.

[0113] In this step, the residual gradually decreases during iteration. When the residual is less than the threshold, the accuracy of the solution vector meets the application requirements, and the benefit of continuing iteration is limited. Therefore, the iteration can be stopped and the result output. By defining the iteration termination condition through the residual threshold, the accuracy of the solution result is guaranteed, meaningless iteration waste is avoided, and the efficiency of forward kinematics solution is improved.

[0114] 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 solution of a humanoid robot 2-RSS-1U parallel mechanism, characterized in that: The method comprises the following steps: establishing a coordinate system of an ankle shaft seat point of the parallel mechanism, and obtaining a pitch angle of a sole plate in the parallel mechanism rotating around a y-axis of the coordinate system of the ankle shaft seat point and a roll angle of the sole plate rotating around an x-axis of the coordinate system of the ankle shaft seat point; based on the pitch angle and the roll angle, calculating a first coordinate of a bottom coordinate point of a first ankle connecting rod in the parallel mechanism relative to an initial coordinate system and a second coordinate of a bottom coordinate point of a second ankle connecting rod relative to the initial coordinate system through a spatial triangle relationship, wherein the initial coordinate system is a reference coordinate system of the parallel mechanism in an initial motion state; based on a known positional relationship of the coordinate system of the ankle shaft seat point relative to a first driving motor coordinate system of the parallel mechanism in the initial state, converting the first coordinate into a third coordinate relative to the first driving motor coordinate system; and based on a known positional relationship of the coordinate system of the ankle shaft seat point relative to a second driving motor coordinate system of the parallel mechanism in the initial state, converting the second coordinate into a fourth coordinate relative to the second driving motor coordinate system; obtaining a fifth coordinate of a top point of the first ankle connecting rod relative to the first driving motor coordinate system and a first length of the first ankle connecting rod, and obtaining a sixth coordinate of a top point of the second ankle connecting rod relative to the second driving motor coordinate system and a second length of the second ankle connecting rod; based on the third coordinate, the fifth coordinate and the first length, calculating a first angle of the first driving motor; and based on the fourth coordinate, the sixth coordinate and the second length, calculating a second angle of the second driving motor.

2. The inverse kinematics solving method of the humanoid robot 2-RSS-1U parallel mechanism according to claim 1, characterized in that, The method for calculating the first coordinate and the second coordinate based on the pitch angle and the roll angle through the spatial triangle relationship comprises the following steps: constructing a platform posture rotation matrix with the pitch angle and the roll angle as parameters, and the expression of the platform posture rotation matrix is: wherein R is the platform posture rotation matrix, θ is the pitch angle, and φ is the roll angle. based on the platform posture rotation matrix and the inherent relative positions of the bottom coordinate points of the first ankle connecting rod and the second ankle connecting rod relative to the coordinate system of the ankle shaft seat point, the first coordinate of the bottom coordinate point of the first ankle connecting rod relative to the initial coordinate system and the second coordinate of the bottom coordinate point of the second ankle connecting rod relative to the initial coordinate system are calculated.

3. The inverse kinematics solving method of humanoid robot 2-RSS-1U parallel mechanism according to claim 2, characterized in that, The method for calculating the first angle of the first driving motor based on the third coordinate, the fifth coordinate and the first length comprises the following steps: assuming that a first crank length of the first driving motor is r1; when the angle of the first driving motor is θ1, the fifth coordinate n1 is (r1・cosθ1, r1・sinθ1, 0); and the third coordinate m1 is (x, y, z); based on the third coordinate m1, the fifth coordinate n1 and the first length L1, a spatial distance equation is listed: ||m1-n1||²=L1²; The spatial distance equation is expanded to obtain r1²+||m1||²-2r1(x・cosθ1+y・sinθ1)=L1²; the expanded equation is simplified to a triangular linear equation: 2r1x・cosθ1+2r1y・sinθ1-(r1²+||m1||²-L1²)=0; the triangular linear equation is solved to obtain the first angle θ1 of the first driving motor.

4. The inverse kinematics solving method of humanoid robot 2-RSS-1U parallel mechanism according to claim 2, characterized in that, Based on the third coordinate, the fifth coordinate and the first length, the first angle of the first driving motor is calculated; based on the fourth coordinate, the sixth coordinate and the second length, the second angle of the second driving motor is calculated. Obtain a previous target pitch angle and a previous target roll angle corresponding to a previous step phase in a continuous gait; obtain a next target pitch angle and a next target roll angle corresponding to a next step phase in the continuous gait; Set a preset interpolation step size and a preset interpolation algorithm; based on the preset interpolation step size and the preset interpolation algorithm, the previous target pitch angle and the next target pitch angle are calculated by interpolation to obtain a plurality of intermediate pitch angles in a transition process between adjacent step phases; based on the preset interpolation step size and the preset interpolation algorithm, the previous target roll angle and the next target roll angle are calculated by interpolation to obtain a plurality of intermediate roll angles in the transition process between adjacent step phases; The plurality of intermediate pitch angles and the plurality of intermediate roll angles are taken as parameters for constructing a platform posture rotation matrix one by one; combined with the inherent relative position of the first ankle connecting rod bottom coordinate point relative to the ankle shaft point coordinate system, the intermediate first coordinates of the first ankle connecting rod bottom coordinate point relative to the initial coordinate system corresponding to each intermediate posture in the transition process are calculated in turn; combined with the inherent relative position of the second ankle connecting rod bottom coordinate point relative to the ankle shaft point coordinate system, the intermediate second coordinates of the second ankle connecting rod bottom coordinate point relative to the initial coordinate system corresponding to each intermediate posture in the transition process are calculated in turn; Based on the known positional relationship of the ankle shaft point coordinate system relative to the first driving motor coordinate system in the initial state, the intermediate first coordinates are converted into intermediate third coordinates relative to the first driving motor coordinate system; based on the known positional relationship of the ankle shaft point coordinate system relative to the second driving motor coordinate system in the initial state, the intermediate second coordinates are converted into intermediate fourth coordinates relative to the second driving motor coordinate system; Based on the intermediate third coordinates, the fifth coordinate and the first length, a plurality of intermediate first angles of the first driving motor in the transition process are calculated in turn; based on the intermediate fourth coordinates, the sixth coordinate and the second length, a plurality of intermediate second angles of the second driving motor in the transition process are calculated in turn; When the continuous gait is executed, the first driving motor is controlled to switch from the current first angle to the first angle of the next step phase in sequence of the plurality of intermediate first angles; the second driving motor is controlled to switch from the current second angle to the second angle of the next step phase in sequence of the plurality of intermediate second angles.

5. A method for forward kinematics of a humanoid robot 2-RSS-1U parallel mechanism, characterized in that, The following contents are included: Determine constant parameters and coordinate system relationship of the parallel mechanism; wherein, the constant parameters include a first crank length of the first driving motor, a second crank length of the second driving motor, the first length, the second length, the first coordinate and the second coordinate; the coordinate system relationship includes a first fixed translation relationship between the first driving motor coordinate system and the ankle shaft base point coordinate system, a second fixed translation relationship between the second driving motor coordinate system and the ankle shaft base point coordinate system; Determine that the attitude of the sole plate of the parallel mechanism is represented by two Euler angles, wherein the two Euler angles are respectively a roll angle of the sole plate rotating around the x-axis of the ankle shaft base point coordinate system and a pitch angle of the sole plate rotating around the y-axis of the ankle shaft base point coordinate system; Based on the roll angle and the pitch angle, taking the first coordinate of the first ankle link bottom coordinate point and the second coordinate of the second ankle link bottom coordinate point as the reference, the first coordinate and the second coordinate are rotated in attitude by a platform attitude rotation matrix, and then the first fixed translation relationship and the second fixed translation relationship are superimposed respectively to construct a nonlinear equation group with the two Euler angles as unknown variables; A two-dimensional residual error vector corresponding to the nonlinear equation group is constructed, and Newton iteration method is used to solve the nonlinear equation group. In each iteration process, finite difference method is used to estimate Jacobian matrix, and based on the Jacobian matrix and the current residual error vector, the solution vector of the two Euler angles is updated; The residual error corresponding to each updated solution vector is judged. If the residual error is less than a preset threshold, it is determined that the iteration converges, and the two Euler angles corresponding to the current solution vector are output as the attitude angles of the sole plate.

6. The method according to claim 5, wherein, The nonlinear equation group is: error1=∥n1−(T1+R(θ,φ)⋅P1)∥−L1 error2=∥n2−(T2+R(θ,φ)⋅P2)∥−L2 Wherein, error1 is the first residual function value, error2 is the second residual function value; n1 is the fifth coordinate, n2 is the sixth coordinate, T1 is the first fixed translation relationship between the first driving motor coordinate system and the ankle shaft base point coordinate system, T2 is the second fixed translation relationship between the second driving motor coordinate system and the ankle shaft base point coordinate system, P1 is the first coordinate, P2 is the second coordinate; R(θ,φ) is the platform attitude rotation matrix, L1 is the first length, L2 is the second length.

7. The method according to claim 5, wherein, The two-dimensional residual error vector is: f = [error1, error2] T Wherein, f is a two-dimensional residual error vector, used to represent the overall deviation degree of the nonlinear equation group with the roll angle θ and the pitch angle φ as unknown variables.

8. The method according to claim 5, wherein, The update formula used to update the solution vector of the two Euler angles based on the Jacobian matrix and the current residual error vector is: where x k+1 represents the Euler angle solution vector after the k+1th iteration, x k represents the Euler angle solution vector at the kth iteration, represents the inverse of the Jacobian matrix, and f represents a two-dimensional residual vector.

Images