A method for describing the full-body posture of a humanoid robot

Abstract

The present invention discloses a method for describing the full-body posture of a humanoid robot. Aiming at the problem that it is difficult to accurately describe the full-body posture of a humanoid robot with large-inertia limbs due to the difficult-to-describe trunk posture, the method regards the set of the centers of mass of each rigid body of the robot as a point cloud, and takes the mass of each rigid body as a weight, and calculates the rotation of the mass part of the robot through a point cloud rotation algorithm. Then, according to the relationship between the inertia tensor and the moment of inertia, the rotation of the inertia tensor part of the robot is calculated through a Riemannian gradient descent algorithm. Finally, an angular displacement algorithm is proposed based on point cloud rotation and rotation matrix averaging, realizing the decoupling of the rotation of the mass part and the rotation of the inertia tensor part of the humanoid robot. By using the method of the present invention, high-dynamic motions such as jumping, running, and backflips can be realized on a real humanoid robot.

Description

Technical Field

[0001] The present invention relates to the field of robots, and particularly to a method for describing the full-body posture of a humanoid robot. Background Art

[0002] When researchers adopt model-based motion planning or control algorithms on humanoid robots, in order to reduce the complexity of the model and thus improve the solving efficiency of the algorithms, they usually use the reduced-order model of the robot as the object of planning or control, such as the center-of-mass momentum model and the single-rigid-body model. Although these reduced-order models are low-dimensional compared to the original full-order model, they contain most of the dynamic characteristics of the robot.

[0003] The center-of-mass momentum model describes the relationship between the linear momentum and angular momentum of the entire robot and the external forces and external torques acting on the robot. It can describe the velocity state of the robot, but cannot give the posture of the robot, while the single-rigid-body model can describe both the velocity state and the posture of the robot. Therefore, the present application selects the single-rigid-body model as the control object of model predictive control. The single-rigid-body model usually needs to use a rotation coordinate to represent its posture, and the present application refers to this posture as the full-body posture of the humanoid robot.

[0004] The description of the full-body posture of a humanoid robot mainly includes torso posture, angular offset, and center of angular mass. Generally, for a humanoid robot with a heavy torso and light limbs, the torso posture can be used to approximately represent the full-body posture of the entire robot, and the error caused by the approximation will not affect the actual control effect of the robot. However, for a humanoid robot with heavier or longer limbs, the method of approximating with the torso posture will generate a large error, especially when the humanoid robot performs a jumping motion with an aerial phase. For example, when the robot performs a high jump, the method of approximating with the torso posture will cause the robot to obtain a large angular momentum during the takeoff process, resulting in the robot having an angular velocity in the pitch direction during the airborne phase, and ultimately causing the robot to be unable to land smoothly. In comparison, angular offset and center of angular mass can more accurately describe the full-body posture of a humanoid robot. Among them, angular offset refers to the integral of the angular velocity of the robot's center of mass, and its value depends on the path of the robot's motion. This means that angular offset can take multiple different values for the same robot state, making it difficult for researchers to apply it to real humanoid robots. The center of angular mass, also called the minimum perturbation coordinate, is a global method for solving the optimal full-body posture of a robot. Compared with angular offset, the advantage of the center of angular mass is that it is path-independent, and its disadvantage is that it needs to operate under the assumption of zero angular momentum. Therefore, at present, it is impossible to use the center-of-angular-mass method to achieve high-dynamic motions such as jumping, running, and backflips on humanoid robots. Summary of the Invention

[0005] The object of the present invention is to provide a method for describing the full-body posture of a humanoid robot in view of the deficiencies of the prior art.

[0006] To achieve the above object, the present invention provides a method for describing the full-body posture of a humanoid robot, including the following steps:

[0007] (1) Consider the humanoid robot as a multi-rigid-body system composed of n rigid bodies, and obtain the mass m of each rigid body i and its inertia matrix with respect to the body system At the same time, obtain its centroid position p init|i and rotation matrix R init|i in the initial state, as well as its centroid position p cur|i and rotation matrix R cur|i in the current state;

[0008] (2) Consider the set composed of the centroids of each rigid body as a point cloud, and use the mass m i of each rigid body as a weight, and calculate the rotation matrix R pc of the point cloud from the initial state to the current state by the singular value decomposition method;

[0009] (3) Consider the point cloud as a rigid body, calculate its inertia matrix with respect to the centroid of the point cloud and merge it with other rigid bodies of the multi-rigid-body system;

[0010] (4) Use the Riemannian gradient descent algorithm to solve the problem of the average of the rotation matrix with variable weights, and obtain the full-body posture R of the multi-rigid-body system.

[0011] Furthermore, the step (2) includes the following sub-steps:

[0012] (2.1) Calculate the centroids of the point cloud in the initial state and the current state respectively:

[0013]

[0014]

[0015] Among them, is the centroid of the point cloud in the initial state, is the centroid of the point cloud in the current state, n is the number of rigid bodies, m i is the mass of the i-th rigid body, p init|i is the centroid position in the initial state, p cur|i is the centroid position in the current state;

[0016] (2.2) Calculate the covariance matrix S of the point cloud rotation according to the centroids of the point cloud calculated in step (2.1):

[0017]

[0018] Where T represents the transpose of the vector;

[0019] (2.3) Perform singular value decomposition on the covariance matrix S:

[0020] S=U∑V T

[0021] Among them, U and V are unitary matrices, Σ is a diagonal matrix, and U, V and Σ are all 3×3 matrices;

[0022] (2.4) According to the singular value decomposition result of step (2.3), the rotation matrix R of the point cloud from the initial state to the current state is obtained pc :

[0023]

[0024] Among them, det(VU T ) represents the matrix VU T rank, U T The transpose of the matrix U.

[0025] Furthermore, in step (3), the inertia matrix The calculation formula is:

[0026]

[0027] in,[·] × Represents the antisymmetric matrix form of a vector, let vector a=[a1 a2 a3] T , then [a] × for:

[0028]

[0029] Furthermore, the step (4) includes the following sub-steps:

[0030] (4.1) Assume that the initial value of the whole body posture R is Assume the allowable error is ε>0;

[0031] (4.2) Calculate the corresponding unit rotation axis w according to the value of the whole body posture R:

[0032]

[0033] Among them, ||·||2 represents the 2-norm of the vector, let a=[a1 a2 a3] T ,but Log(·) represents the logarithmic transformation from the rotation Lie group to the vector space:

[0034]

[0035]

[0036] Log(R) = θu

[0037] where tr(·) represents the trace of a matrix, that is, the sum of the elements on the main diagonal from the upper left to the lower right of the matrix; [·] ∨ represents the mapping relationship from the skew-symmetric matrix to the vector, and θ and u respectively represent the rotation angle and the unit rotation axis in the axis-angle corresponding to the rotation matrix R;

[0038] (4.3) Calculate the rotation error in the tangent space:

[0039]

[0040] where Log(·) represents the logarithmic transformation from the rotation Lie group to the Lie algebra, log(R) = [Log(R)] × = [θu] × ;

[0041] (4.4) Project the rotation error e onto the rotation Lie group:

[0042] R = Rexp(e)

[0043] where R is the full-body pose, and exp(·) represents the exponential transformation from the Lie algebra to the rotation Lie group:

[0044]

[0045] (4.5) Determine whether the rotation error is small enough. If ||e|| F < ε, then return the full-body pose R, otherwise go to step (4.2); where ||·|| F represents the Frobenius norm of the matrix.

[0046] Compared with the prior art, the beneficial effects of the present invention are:

[0047] 1. The present invention combines point cloud rotation and rotation matrix averaging to achieve an accurate description of the full-body pose of a humanoid robot.

[0048] 2. The present invention uses the Riemannian gradient descent algorithm for iterative calculation of the full-body pose, improving the efficiency of solving the rotation matrix averaging problem.

[0049] 3. The present invention overcomes the path dependence problem of the angular offset method and the zero angular momentum assumption problem of the angular centroid method, and can use the method of the present invention to achieve high-dynamic motions such as jumping, running, and backflips on a real humanoid robot. Description of the Drawings

[0050] Figure 1 Schematic diagram of the method for describing the full-body posture of a humanoid robot in the present invention;

[0051] Figure 2 Schematic diagram of the multi-rigid-body system in the present invention;

[0052] Figure 3 Schematic diagram of the conversion relationship among Lie group, Lie algebra, and vector space in the present invention. Detailed implementation manners

[0053] To better illustrate the technical solutions and advantages of the present invention, the following will introduce the detailed implementation manners in combination with the accompanying drawings.

[0054] The present invention provides a method for describing the full-body posture of a humanoid robot. Refer to Figure 1 and Figure 2 , which includes the following steps:

[0055] S101: Consider the humanoid robot as a multi-rigid-body system composed of n rigid bodies, and obtain the mass m of each rigid body i and its inertia matrix with respect to the body system At the same time, obtain the centroid position p in it|i and rotation matrix R init|i in its initial state, and the centroid position p cur|i and rotation matrix R cur|i in its current state;

[0056] S102: Consider the set composed of the centroids of each rigid body as a point cloud, and use the mass m i of each rigid body as a weight to calculate the rotation matrix R pc from the initial state to the current state of the point cloud through the singular value decomposition method;

[0057] S103: Consider the point cloud as a rigid body, calculate the inertia matrix of the rigid body with respect to the centroid of the point cloud, and merge the rigid body with other rigid bodies of the multi-rigid-body system;

[0058] S104: Use the Riemannian gradient descent algorithm to solve the problem of the average of the rotation matrix with varying weights, and obtain the full-body posture R of the multi-rigid-body system.

[0059] Furthermore, the implementation manner in S102 includes:

[0060] 1) Calculate the centroids of the point cloud in the initial state and the current state respectively:

[0061]

[0062]

[0063] Among them, is the centroid of the point cloud in the initial state, is the centroid of the point cloud in the current state, n is the number of rigid bodies, and m i is the mass of the i-th rigid body, and p init|i is the centroid position in the initial state, and p cur|i is the centroid position in the current state.

[0064] 2) Calculate the covariance matrix S of the point cloud rotation according to the calculated centroid of the point cloud:

[0065]

[0066] Among them, T represents the transpose of the vector. In the present invention, the masses of the respective rigid bodies are used as weights to calculate the value of the covariance matrix. Compared with the calculation method without weights, the rotation of the point cloud composed of the centroids of the respective rigid bodies can be calculated more accurately.

[0067] 3) Perform singular value decomposition on the covariance matrix S:

[0068] S = U∑V T

[0069] Among them, both U and V are unitary matrices, Σ is a diagonal matrix, and U, V, and Σ are all 3×3 matrices;

[0070] 4) Obtain the rotation matrix R of the point cloud from the initial state to the current state according to the singular value decomposition result pc :

[0071]

[0072] Among them, det(VU T ) represents the rank of the matrix VU T , and U T is the transpose of the matrix U. This step can ensure that the calculated R pc is a rotation matrix, rather than just an orthogonal matrix.

[0073] Furthermore, the calculation formula of the inertia matrix in S103 is:

[0074]

[0075] Among them, [·] × represents the skew-symmetric matrix form of the vector. Let the vector a = [a1 a2 a3] T , then [a] × is:

[0076]

[0077] Furthermore, the implementation method in S104 includes:

[0078] 1) Set the initial value of the full-body pose R as Set the allowable error as ε > 0; where, R init|1 and R cur|1 respectively represent the rotation matrices of the first rigid body in the initial state and the current state.

[0079] 2) Calculate the corresponding unit rotation axis w according to the value of the full-body pose R:

[0080]

[0081] where, ||·||2 represents the 2-norm of the vector, set a = [a1 a2 a3] T , then In this step, the setting of the piecewise function can ensure that the unit rotation axis w does not exhibit a singular phenomenon when the rotation matrix R is close to the identity matrix. Log(·) represents the logarithmic transformation from the rotation Lie group to the vector space:

[0082]

[0083]

[0084] Log(R) = θu

[0085] where, tr(·) represents the trace of the matrix, that is, the sum of the elements on the main diagonal (from the upper left to the lower right) of the matrix. [·] ∨ represents the mapping relationship from the skew-symmetric matrix to the vector, that is, the inverse operation of [·] × , θ and u respectively represent the rotation angle and the unit rotation axis in the axis-angle corresponding to the rotation matrix R, and the range of the rotation angle θ is 0 ≤ θ ≤ π.

[0086] 3) Calculate the rotation error in the tangent space:

[0087]

[0088] where, Log(·) represents the logarithmic transformation from the rotation Lie group to the Lie algebra, log(R) = [Log(R)] × = [θu] × . The conversion relationships among the rotation Lie group, the Lie algebra, and the vector space are as Figure 3 shown. log(R T R pc ) represents the error between the full-body pose R and the point cloud rotation matrix R pc in the tangent space. Denote the relative rotation matrix of the full-body pose R and the i-th rigid body The error in the tangent space. and respectively represent the point cloud composed of the centroids of each rigid body and the moment of inertia of the i-th rigid body about the corresponding unit rotation axis w of the full-body pose R. This step uses the mapping relationship between the inertia matrix and the moment of inertia to transform the weights corresponding to each rotation matrix from a matrix to a real number, which greatly reduces the difficulty of solving the average problem of the rotation matrix.

[0089] 4) Project the rotation error e onto the rotation Lie group:

[0090] R = Rexp(e)

[0091] where R is the full-body pose and exp(·) represents the exponential transformation from the Lie algebra to the rotation Lie group:

[0092]

[0093] Using the exponential transformation to project the rotation error can ensure that the projected result is still in the rotation Lie group.

[0094] 5) Determine whether the rotation error is small enough. If ||e|| F < ε, then return the full-body pose R; otherwise, go to step 2) in step S104. Where ||·|| F represents the Frobenius norm of the matrix. For any matrix its Frobenius norm is

[0095] The above embodiments are used to explain the present invention, rather than limit the present invention. Any modifications and changes made to the present invention within the spirit and scope of the claims of the present invention fall within the protection scope of the present invention.

Claims

1. A method for describing the full-body posture of a humanoid robot, characterized in that, The steps are as follows: (1) Consider the humanoid robot as a multi-rigid-body system composed of n rigid bodies, and obtain the mass m of each rigid body i and its inertial matrix with respect to the body system At the same time, obtain its center-of-mass position p init|i and rotation matrix R init|i in the initial state, as well as its center-of-mass position p cur|i and rotation matrix R cur|i ; (2) Consider the set composed of the centroids of each rigid body as a point cloud, and use the mass \(m\) of each rigid body i as the weight, and calculate the rotation matrix \(R\) of the point cloud from the initial state to the current state through the singular value decomposition method pc ; (3) Treat the point cloud as a rigid body and calculate its inertia matrix with respect to the centroid of the point cloud. And merge it with other rigid bodies in the multi-rigid body system. (4) Use the Riemannian gradient descent algorithm to solve the problem of averaging rotation matrices with varying weights, and obtain the full-body pose R of the multi-rigid-body system.

2. The method for describing the full-body posture of a humanoid robot according to claim 1, characterized in that, The step (2) includes the following sub-steps: (2.1) Calculate the centroids of the point clouds in the initial state and the current state respectively: Among them, is the centroid of the point cloud in the initial state, is the centroid of the point cloud in the current state, n is the number of rigid bodies, m i is the mass of the i-th rigid body, p init|i is the centroid position in the initial state, p cur|i is the centroid position in the current state; (2.2) Calculate the covariance matrix S of the point cloud rotation according to the centroids of the point clouds calculated in step (2.1): where T represents the transpose of the vector; (2.3) Perform singular value decomposition on the covariance matrix S: S = U∑V T where U and V are both unitary matrices, Σ is a diagonal matrix, and U, V, and Σ are all 3×3 matrices; (2.4) Obtain the rotation matrix R of the point cloud from the initial state to the current state according to the singular value decomposition result in step (2.3). pc : where, det(VU T ) represents the rank of the matrix VU T , and U T is the transpose of the matrix U.

3. The method for describing the full-body posture of a humanoid robot according to claim 1, wherein In the step (3), the inertia matrix is calculated by the following formula: Among them, [·] × represents the skew-symmetric matrix form of a vector. Let the vector a = [a1 a2 a3] T , then [a] × is as follows:

4. The method for describing the full-body posture of a humanoid robot according to claim 1, wherein The step (4) includes the following sub-steps: (4.1) Let the initial value of the full body pose R be Let the allowable error be ε > 0; (4.2) Calculate the corresponding unit rotation axis w according to the value of the full-body pose R: where, ||·||2 represents the 2-norm of a vector, and let a = [a1 a2 a3] T , then Log(·) represents the logarithmic transformation from the rotation Lie group to the vector space: Log(R) = θu where tr(·) represents the trace of a matrix, i.e., the sum of the elements on the main diagonal from the upper left to the lower right of the matrix; [·] ∨ represents the mapping relationship from an anti-symmetric matrix to a vector, and θ and u respectively represent the rotation angle and the unit rotation axis in the axis angle corresponding to the rotation matrix R; (4.3) Calculate the rotation error in the tangent space: where Log(·) represents the logarithmic transformation from the rotation Lie group to the Lie algebra, and log(R) = [Log(R)] × = [θu] × ; (4.4) Project the rotation error e onto the rotation Lie group: R = Rexp(e) where R is the full-body pose, and exp(·) represents the exponential transformation from the Lie algebra to the rotation Lie group: (4.5) Determine whether the rotational error is small enough. If ||e|| F < ε, then return the full-body pose R; otherwise, go to step (4.2). Here, ||·|| F represents the Frobenius norm of the matrix.

Images