Humanoid robot upper limb attitude estimation method based on adaptive filtering

Abstract

The invention discloses a humanoid robot upper limb attitude estimation method based on adaptive filtering, and the method comprises the steps: taking an end point of a humanoid robot upper limb segment as a key point, simulating the relation between the position and the speed in a local coordinate system, and constructing a prior model of a humanoid robot key joint. By combining the acceleration, the attitude direction and the prior model, a filtering framework is established, and the influence of the transient sensor error on the overall precision is effectively reduced. In addition, actual measurement conditions are reflected by using distribution of prior residual errors, and a self-adaptive measurement noise adjustment strategy is provided. According to the strategy, the prior residual error and the maximum likelihood criterion are combined, and the accuracy and robustness of attitude estimation of the humanoid robot are effectively improved.

Description

Technical Field

[0001] The present invention relates to the field of humanoid robots, and in particular to a method for estimating the upper limb posture of a humanoid robot based on adaptive filtering. Background Art

[0002] With advances in sensor technology, the use of micro inertial measurement units (MIMUs) for humanoid robot pose estimation has become an emerging research hotspot. By integrating data from gyroscopes, accelerometers, and magnetometers, MIMUs can accurately calculate the joint poses of humanoid robots and achieve overall pose estimation through forward kinematics. The motion of a humanoid robot in three-dimensional space can be described as a correlated rigid-body motion model. The joints of a humanoid robot form a hierarchical tree structure, and the pose of the joint segments is characterized by their orientations and positions. Traditional methods for upper limb pose estimation in humanoid robots rely primarily on forward kinematics to calculate joint positions and orientations. However, these methods rely too heavily on orientation data and fail to fully utilize the multi-source information provided by the MIMU. This limitation leads to the accumulation of sensor errors during joint pose estimation, ultimately affecting the accuracy of overall pose estimation. To improve pose estimation accuracy, multi-sensor data fusion methods using MIMUs have attracted widespread attention. However, issues such as gyroscope drift, which can lead to angular velocity integration errors, the coupling effect between accelerometer motion and gravity acceleration, and the susceptibility of magnetometers to magnetic interference can reduce estimation accuracy. In recent years, deep learning methods have been introduced into posture estimation, allowing neural networks to learn the mapping between acceleration data and joint angles and predict the 3D motion trajectory of joints using multi-source data. However, the applicability of deep learning methods is limited by the availability of training data and specific contextual factors, which restricts the generalization ability of the model. To address the challenges posed by the complexity and variability of upper limb posture estimation, this paper proposes a humanoid robot upper limb posture estimation method based on adaptive filtering. By integrating acceleration, orientation and prior models, this method constructs a filtering framework that effectively reduces the impact of individual data on the overall accuracy. In addition, an adaptive measurement noise adjustment strategy is introduced, which combines the prior residual distribution with the maximum likelihood criterion to enhance the robustness of humanoid robot posture estimation. This method significantly improves the accuracy and stability of posture estimation, providing a new solution for humanoid robot posture estimation in complex motion scenes. Summary of the Invention

[0003] The purpose of the present invention is to provide a method for estimating the upper limb posture of a humanoid robot based on adaptive filtering to solve the problems raised in the above background technology.

[0004] To achieve the above object, the present invention provides the following technical solutions:

[0005] A method for estimating the upper limb posture of a humanoid robot based on adaptive filtering comprises the following steps:

[0006] Step 1: Construct the global coordinate system position of the humanoid robot's upper limb multi-joints;

[0007] Step 2: Establish a discrete posture estimation system based on the multi-joint kinematic model;

[0008] Step 3: Calculate the state prediction of each joint and its covariance

[0009] Step 4: Calculate the prior residual and its covariance Adaptively adjust the measurement noise covariance through the covariance of the prior residual

[0010] Step 5: Update the state of each joint and calculate its posterior estimate and its covariance

[0011] Step 6: Repeat steps 3 to 5 to obtain the pose estimation results of each joint at all times;

[0012] Furthermore, the multiple joints of the upper limbs of the humanoid robot in step 1 include: multiple ones of the hip joint, thoracic joint, left shoulder joint, left elbow joint, left wrist joint, right shoulder joint, right elbow joint, and right wrist joint, wherein the hip joint serves as the root node, and the changes between the joint nodes are described as the posture changes of the child node relative to the parent node.

[0013] Furthermore, the global coordinate system position of each joint point in step 1 is realized by the initial alignment method, the thoracic joint is selected as the global coordinate system G, and the hip joint coordinates and the thoracic joint coordinates are assumed to be the same to provide a unified coordinate reference. The position of the thoracic joint is expressed as p 1 =(0,0,0) T , under the initial coordinate correction, the position information of each joint in the global coordinate system is expressed as:

[0014]

[0015] Where, are the position information of the left shoulder joint, left elbow joint, left wrist joint, right shoulder joint, right elbow joint and right wrist joint in the global coordinate system, (p 2 )'、(p 3 )'、(p 4 )'、(p 5 )'、(p 6 )'、(p 7)' are the position changes of the left shoulder joint, left elbow joint, left wrist joint, right shoulder joint, right elbow joint and right wrist joint after posture transformation.

[0016] Furthermore, in step 2, the motion posture of each joint of the human body is modeled as a uniform acceleration motion model. Considering the information of a joint point, its position discrete equation is described as:

[0017]

[0018] Where, Represents the X, Y, and Z position information of the i-th joint at time k in the global coordinate system G. represents the X, Y, and Z velocity information of the i-th joint at time k in the global coordinate system G. δT is the sampling period of the sensor. The kinematic model of the joint is expressed as:

[0019]

[0020] Where, is the process noise of the i-th joint at time k. In the global coordinate system, the motion velocity of each joint is obtained by the acceleration. The velocity discrete equation of the joint can be expressed as:

[0021]

[0022] Where, Represents the X, Y, and Z acceleration information of the i-th joint at time k in the global coordinate system G;

[0023] According to formulas (1) and (4), the measurement equation is:

[0024]

[0025] Where, is the measurement information of the i-th joint at time k, is the measurement noise of the i-th joint at time k;

[0026] The state equations of the discrete posture estimation system of each joint of the humanoid robot's upper limbs are described as:

[0027]

[0028] Where x k =[(x 1 ) T (x 2 ) T …(x i ) T ] Tis the posture information of each joint point, T represents the transposed matrix, i=0,1,…dd is the number of joint points, the first joint point x 1 =[(p G,1 ) T (v G,1 ) T ] T ,(p G,1 ) T and (v G,1 ) T are the position and velocity of the first joint in the global coordinate system, and the process noise ω k =[(ω 1 ) T (ω 2 ) T …(ω i ) T ] T , whose covariance is In addition, the state matrix Ψ is composed of the matrix Ψ i The constructed diagonal matrix, Ψ i is represented as:

[0029]

[0030] z in the equation of state k =[(z 1 ) T (z 2 ) T …(z i ) T ] T is the measurement information, and z 1 =[(p G,1 ) T (v G,1 ) T ] T ,υ k =[(υ 1 ) T (υ 2 ) T …(υ i ) T ] T To measure noise, the measurement matrix H is composed of multiple 6×6 identity matrices I6, expressed as:

[0031]

[0032] Furthermore, the state prediction of each joint point in step 3 and its covariance The calculation process is expressed as:

[0033]

[0034] Where, are the prior estimates and their covariances, Ψ Τ is the transposed matrix of the state matrix Ψ, is the process noise covariance.

[0035] Furthermore, the prior residual in step 4 and its covariance The calculation process is expressed as:

[0036]

[0037] Where, is the measured value, Η Τ is the transposed matrix of the measurement matrix H, and the measurement noise covariance is adaptively adjusted by the covariance of the prior residual

[0038] Furthermore, the adaptive strategy in step 4 is the maximum likelihood criterion, which transforms the adaptive measurement noise problem into In the maximum likelihood estimation of variables To solve the problem under the condition of Find the partial derivative and we get:

[0039]

[0040] Where, Represents the state prediction covariance to the maximum likelihood estimation variable Find the partial derivative, Represents the measurement noise covariance to the maximum likelihood estimation variable To find the partial derivative, according to formula (8), Expressed as:

[0041]

[0042] Where, Represents the prior estimated covariance of the maximum likelihood estimate of the variable Find the partial derivative, Represents the maximum likelihood estimation of the process noise covariance variable To find the partial derivative, assuming that the process of prior estimation of covariance is stable within a certain period of time, formula (12) can be written as follows:

[0043]

[0044] Substituting formula (13) into formula (11), we get:

[0045]

[0046] Then, substitute formula (14) into the maximum likelihood criterion to obtain:

[0047]

[0048] Where, represents the cumulative summation of the time steps from the initial time j0 to the current time k, tr(·) represents the trace of the calculation matrix, represents the covariance of the i-th joint point at time j, for The inverse matrix of The prior residual of the i-th joint point at time j, for The transposed matrix of is the measurement noise covariance of the i-th joint point at time j, is the maximum likelihood estimation variable of the i-th joint point at time j, Η j is the measurement matrix at time j, For Η j The transposed matrix of , in formula (15), the process noise is a known quantity, which is used to estimate the maximum likelihood variable The partial derivative of is 0, and formula (15) is re-expressed as:

[0049]

[0050] assumed The parameters described are matrices The elements on the main diagonal, then The result is the identity matrix, and formula (16) can be rewritten as:

[0051]

[0052] Since in the filtering process is a positive definite matrix, then the condition satisfied by formula (17) is:

[0053]

[0054] Where N is the number of time sliding windows, and the covariance of the measurement noise is The real-time adjustment strategy is:

[0055]

[0056] Furthermore, in step 5, the adjusted measurement noise covariance is obtained through step 4. And calculate the Kalman filter gain Then the posterior estimate of each joint point is and its covariance The calculation can be expressed as:

[0057]

[0058]

[0059] Where, is the prior residual covariance, are the prior estimates and their covariances, Η Τ is the transposed matrix of the measurement matrix H, and I is the identity matrix.

[0060] Compared with the prior art, the present invention has the following advantages: The present invention proposes a method for estimating the upper limb posture of a humanoid robot based on adaptive filtering. This method uses the end points of the humanoid robot's limbs as key points and constructs a priori models of the key joints of the humanoid robot by establishing a correlation between the position and velocity of each end point in the local coordinate system. By fusing acceleration and direction information with the prior model, a filtering framework is constructed that can effectively suppress the impact of instantaneous sensor errors on overall accuracy. At the same time, the prior residual distribution is used to reflect the actual measurement conditions, and an adaptive measurement noise adjustment strategy is proposed that combines the prior residual with the maximum likelihood criterion, significantly improving the robustness of the humanoid robot's upper limb posture estimation. BRIEF DESCRIPTION OF THE DRAWINGS

[0061] Figure 1 is a flow chart of the present invention;

[0062] Figure 2 It is a structural block diagram of the present invention;

[0063] Figure 3 is a schematic diagram of the initial alignment method of the present invention. DETAILED DESCRIPTION

[0064] The following will clearly and completely describe the technical solutions in the embodiments of the present invention in conjunction with the accompanying drawings. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without making creative efforts are within the scope of protection of the present invention.

[0065] Reference Figure 1 , Figure 2 and Figure 3 , a method for estimating the upper limb posture of a humanoid robot based on adaptive filtering, comprising the following steps:

[0066] Step 1: Construct the global coordinate system position of the multiple joints of the upper limbs of the humanoid robot. The joints include: hip joint, thoracic joint, left shoulder joint, left elbow joint, left wrist joint, right shoulder joint, right elbow joint and right wrist joint. Among them, the hip joint is used as the root node. The changes between joint nodes are described by the posture changes of child nodes relative to the parent node. The posture change of the parent node is the basis for calculating the posture change of the child node, which depends on the human stick model.

[0067] The global coordinate system position of each joint point is achieved through the initial alignment method, such as Figure 3 As shown, the thoracic joint is selected as the global coordinate system G, and the hip joint coordinates are assumed to be the same as the thoracic joint coordinates to provide a unified coordinate reference. The position of the thoracic joint is expressed as p 1 =(0,0,0) T , under the initial coordinate correction, the position information of each joint in the global coordinate system is expressed as:

[0068]

[0069] Where, are the position information of the left shoulder joint, left elbow joint, left wrist joint, right shoulder joint, right elbow joint and right wrist joint in the global coordinate system, (p 2 )'、(p 3 )'、(p 4 )'、(p 5 )'、(p 6 )'、(p 7 )' are the position changes of the left shoulder joint, left elbow joint, left wrist joint, right shoulder joint, right elbow joint and right wrist joint after posture transformation.

[0070] Step 2: Establish a discrete posture estimation system based on the multi-joint kinematic model. Model the motion posture of each joint of the human body as a uniform acceleration motion model. Consider the information of a joint point, and its position discrete equation is described as:

[0071]

[0072] Where, Represents the X, Y, and Z position information of the i-th joint at time k in the global coordinate system G. represents the X, Y, and Z velocity information of the i-th joint at time k in the global coordinate system G. δT is the sampling period of the sensor. The kinematic model of the joint is expressed as:

[0073]

[0074] Where, is the process noise of the i-th joint at time k. In the global coordinate system, the motion velocity of each joint is obtained by the acceleration. The velocity discrete equation of the joint can be expressed as:

[0075]

[0076] Where, Represents the X, Y, and Z acceleration information of the i-th joint at time k in the global coordinate system G;

[0077] According to formulas (1) and (4), the measurement equation is:

[0078]

[0079] Where, is the measurement information of the i-th joint at time k, is the measurement noise of the i-th joint at time k;

[0080] The state equation of the discrete posture estimation system of each joint of the humanoid robot's upper limb can be described as:

[0081]

[0082] Where x k =[(x 1 ) T (x 2 ) T …(x i ) T ] T is the posture information of each joint point, T represents the transposed matrix, i=0,1,…dd is the number of joint points, the first joint point x 1 =[(p G,1 ) T (v G,1 ) T ] T ,(p G,1 ) T and (v G,1 ) T are the position and velocity of the first joint in the global coordinate system, and the process noise ω k =[(ω 1 ) T (ω 2 ) T …(ω i ) T ] T , whose covariance is In addition, the state matrix Ψ is composed of the matrix Ψ i The constructed diagonal matrix, Ψ i is represented as:

[0083]

[0084] z in the equation of state k =[(z 1 ) T (z 2 ) T …(z i ) T ] T is the measurement information, and z 1 =[(p G,1 ) T (v G,1 ) T ] T ,υ k =[(υ 1 ) T (υ 2 ) T …(υ i ) T ] T To measure noise, the measurement matrix H is composed of multiple 6×6 identity matrices I6, expressed as:

[0085]

[0086] Step 3: Calculate the state prediction of each joint and its covariance The process is expressed as:

[0087]

[0088] Where, are the prior estimates and their covariances, Ψ Τ is the transposed matrix of the state matrix Ψ, is the process noise covariance.

[0089] Step 4: Calculate the prior residual and its covariance The process is expressed as:

[0090]

[0091] Where, is the measured value, Η Τ is the transposed matrix of the measurement matrix H, and the measurement noise covariance is adaptively adjusted by the covariance of the prior residual

[0092] Adaptive adjustment of measurement noise covariance is achieved through the covariance of the prior residual The adaptive strategy is the maximum likelihood criterion, which transforms the adaptive measurement noise problem into In the maximum likelihood estimation of variables To solve the problem under the condition of Taking partial derivatives, we can get:

[0093]

[0094] Where, Represents the state prediction covariance to the maximum likelihood estimation variable Find the partial derivative, Represents the measurement noise covariance to the maximum likelihood estimation variable To find the partial derivative, according to formula (8), Expressed as:

[0095]

[0096] Where, Represents the prior estimated covariance of the maximum likelihood estimate of the variable Find the partial derivative, Represents the maximum likelihood estimation of the process noise covariance variable To find the partial derivative, assuming that the process of prior estimation of covariance is stable within a certain period of time, formula (12) can be written as follows:

[0097]

[0098] Substituting formula (13) into formula (11), we can obtain:

[0099]

[0100] Then, substituting formula (14) into the maximum likelihood criterion, we can obtain:

[0101]

[0102] Where, represents the cumulative summation of the time steps from the initial time j0 to the current time k, tr(·) represents the trace of the calculation matrix, represents the covariance of the i-th joint point at time j, for The inverse matrix of The prior residual of the i-th joint point at time j, for The transposed matrix of is the measurement noise covariance of the i-th joint point at time j, is the maximum likelihood estimation variable of the i-th joint point at time j, Η jis the measurement matrix at time j, For Η j The transposed matrix of , in formula (15), the process noise is a known quantity, which is used to estimate the maximum likelihood variable The partial derivative of is 0, and formula (15) can be re-expressed as:

[0103]

[0104] assumed The parameters described are matrices The elements on the main diagonal, then The result is the identity matrix, and formula (16) can be rewritten as:

[0105]

[0106] Since in the filtering process is a positive definite matrix, then the condition satisfied by formula (17) is:

[0107]

[0108] Where N is the number of time sliding windows, and the covariance of the measurement noise is The real-time adjustment strategy is:

[0109]

[0110] Step 5: Update the state of each joint and calculate its posterior estimate and its covariance The adjusted measurement noise covariance is obtained through step 4 And calculate the Kalman filter gain Then the posterior estimate of each joint point is and its covariance The calculation can be expressed as:

[0111]

[0112] Where, is the prior residual covariance, are the prior estimates and their covariances, Η Τ is the transposed matrix of the measurement matrix H, and I is the identity matrix.

[0113] Step 6: Repeat steps 3 to 5 to obtain the upper limb joint posture estimation results of the humanoid robot at all times.

[0114] While embodiments of the present invention have been shown and described, it will be appreciated by those skilled in the art that various changes, modifications, substitutions, and variations may be made to these embodiments without departing from the principles and spirit of the invention, and that the scope of the invention is defined by the appended claims and their equivalents.

Claims

1. A method for estimating the upper limb posture of a humanoid robot based on adaptive filtering, characterized in that: The following steps are involved: Step 1: Construct the global coordinate system position of the humanoid robot's upper limb multi-joints; Step 2: Establish a discrete posture estimation system based on the multi-joint kinematic model; Step 3: Calculate the state prediction of each joint and its covariance Step 4: Calculate the prior residual and its covariance Adaptively adjust the measurement noise covariance through the covariance of the prior residual Step 5: Update the state of each joint and calculate its posterior estimate and its covariance Step 6: Repeat steps 3 to 5 to obtain the pose estimation results of each joint at all times.

2. The method for estimating upper limb posture of a humanoid robot based on adaptive filtering according to claim 1, characterized in that: The multiple joints of the upper limbs of the humanoid robot in step 1 include: multiple ones of the hip joint, thoracic joint, left shoulder joint, left elbow joint, left wrist joint, right shoulder joint, right elbow joint, and right wrist joint, wherein the hip joint serves as the root node, and the changes between joint nodes are described as the posture changes of the child node relative to the parent node.

3. The method for estimating upper limb posture of a humanoid robot based on adaptive filtering according to claim 2, characterized in that: The global coordinate system position of each joint point in step 1 is realized by the initial alignment method. The thoracic joint is selected as the global coordinate system G, and the hip joint coordinates and the thoracic joint coordinates are assumed to be the same to provide a unified coordinate reference. The position of the thoracic joint is expressed as p 1 =(0,0,0) T , under the initial coordinate correction, the position information of each joint in the global coordinate system is expressed as: Where, are the position information of the left shoulder joint, left elbow joint, left wrist joint, right shoulder joint, right elbow joint and right wrist joint in the global coordinate system, (p 2 )'、(p 3 )'、(p 4 )'、(p 5 )'、(p 6 )'、(p 7 )' are the position changes of the left shoulder joint, left elbow joint, left wrist joint, right shoulder joint, right elbow joint and right wrist joint after posture transformation.

4. The method for estimating upper limb posture of a humanoid robot based on adaptive filtering according to claim 1, characterized in that: In step 2, the motion posture of each joint of the human body is modeled as a uniform acceleration motion model. Considering the information of a joint point, its position discrete equation is described as: Where, Represents the X, Y, and Z position information of the i-th joint at time k in the global coordinate system G. represents the X, Y, and Z velocity information of the i-th joint at time k in the global coordinate system G. δT is the sampling period of the sensor. The kinematic model of the joint is expressed as: Where, is the process noise of the i-th joint at time k. In the global coordinate system, the motion velocity of each joint is obtained by the acceleration. The velocity discrete equation of the joint can be expressed as: Where, Represents the X, Y, and Z acceleration information of the i-th joint at time k in the global coordinate system G; According to formulas (1) and (4), the measurement equation is: Where, is the measurement information of the i-th joint at time k, is the measurement noise of the i-th joint at time k; The state equations of the discrete posture estimation system of each joint of the humanoid robot's upper limbs are described as: Where x k =[(x 1 ) T (x 2 ) T … (x i ) T ] T is the posture information of each joint point, T represents the transposed matrix, i=0,1,…dd is the number of joint points, the first joint point x 1 =[(p G,1 ) T (v G,1 ) T ] T ,(p G,1 ) T and (v G,1 ) T are the position and velocity of the first joint in the global coordinate system, and the process noise ω k =[(ω 1 ) T (ω 2 ) T … (ω i ) T ] T , whose covariance is In addition, the state matrix Ψ is composed of the matrix Ψ i The constructed diagonal matrix, Ψ i is represented as: z in the equation of state k =[(z 1 ) T (z 2 ) T … (z i ) T ] T is the measurement information, and z 1 =[(p G,1 ) T (v G,1 ) T ] T ,υ k =[(υ 1 ) T (υ 2 ) T … (υ i ) T ] T To measure noise, the measurement matrix H is composed of multiple 6×6 identity matrices I6, expressed as:

5. The method for estimating upper limb posture of a humanoid robot based on adaptive filtering according to claim 1, characterized in that: The state prediction of each joint point in step 3 and its covariance The calculation process is expressed as: Where, are the prior estimates and their covariance, Ψ Τ is the transposed matrix of the state matrix Ψ, is the process noise covariance.

6. The method for estimating upper limb posture of a humanoid robot based on adaptive filtering according to claim 5, characterized in that: The prior residual in step 4 and its covariance The calculation process is expressed as: Where, is the measured value, Η Τ is the transposed matrix of the measurement matrix H, and the measurement noise covariance is adaptively adjusted by the covariance of the prior residual 7. The method for estimating upper limb posture of a humanoid robot based on adaptive filtering according to claim 6, characterized in that: The adaptive strategy in step 4 is the maximum likelihood criterion, which transforms the adaptive measurement noise problem into In the maximum likelihood estimation of variables To solve the problem under the condition of Find the partial derivative and we get: Where, Represents the state prediction covariance to the maximum likelihood estimation variable Find the partial derivative, Represents the measurement noise covariance to the maximum likelihood estimation variable To find the partial derivative, according to formula (8), Expressed as: Where, Represents the prior estimated covariance of the maximum likelihood estimate of the variable Find the partial derivative, Represents the maximum likelihood estimation of the process noise covariance variable To find the partial derivative, assuming that the process of prior estimation of covariance is stable within a certain period of time, formula (12) can be written as follows: Substituting formula (13) into formula (11), we get: Then, substitute formula (14) into the maximum likelihood criterion to obtain: Where, represents the cumulative summation of the time steps from the initial time j0 to the current time k, tr(·) represents the trace of the calculation matrix, represents the covariance of the i-th joint point at time j, for The inverse matrix of The prior residual of the i-th joint point at time j, for The transposed matrix of is the measurement noise covariance of the i-th joint point at time j, is the maximum likelihood estimation variable of the i-th joint point at time j, Η j is the measurement matrix at time j, For Η j The transposed matrix of , in formula (15), the process noise is a known quantity, which is used to estimate the maximum likelihood variable The partial derivative of is 0, and formula (15) is re-expressed as: assumed The parameters described are matrices The elements on the main diagonal, then The result is the identity matrix, and formula (16) can be rewritten as: Since in the filtering process is a positive definite matrix, then the condition satisfied by formula (17) is: Where N is the number of time sliding windows, and the covariance of the measurement noise is The real-time adjustment strategy is:

8. The method for estimating upper limb posture of a humanoid robot based on adaptive filtering according to claim 7, characterized in that: In step 5, the adjusted measurement noise covariance is obtained through step 4. And calculate the Kalman filter gain Then the posterior estimate of each joint point is and its covariance The calculation can be expressed as: Where, is the prior residual covariance, are the prior estimates and their covariances, Η Τ is the transposed matrix of the measurement matrix H, and I is the identity matrix.

Images