Humanoid robot attitude estimation method based on physical information constraint
By introducing physical information constraints and lightweight neural networks into the attitude estimation of humanoid robots, the problem of inaccurate joint angle parameter reconstruction in traditional methods is solved, and accurate posture estimation and generalization ability improvement in dynamic motion scenarios is achieved.
Patent Information
- Application Number
- CN202510381509.3
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2025-03-28
- Publication Date
- 2025-08-19
AI Technical Summary
In the pose estimation of humanoid robots, there is a problem of inaccurate joint angle parameter reconstruction in the sensor data noise interference and dynamic motion scenarios. In addition, deep learning methods rely on too much data, neglect the physical model of the system, resulting in insufficient generalization capabilities.
Using a humanoid robot pose estimation method based on physical information constraints, a lightweight complex value neural network and physical information constraints are introduced, combined with the Kalman filter optimal gain theorem, a hybrid model driving framework is established, and the fixed parameter defects of traditional filters are overcome, and the lightweight neural network is used to replace the gain matrix to perform pose estimation.
Accurate and real-time joint pose estimation in dynamic motion scenarios is achieved, which avoids dependence on a large amount of labeled data, and improves the generalization ability and estimation accuracy of the model.
Smart Images
Figure CN120509435A_ABST
Abstract
Description
Technical Field
[0001] The present invention relates to the field of humanoid robots, and in particular to a humanoid robot posture estimation method based on physical information constraints. Background Art
[0002] With the rapid development of humanoid robotics, achieving accurate pose estimation has become a core challenge in enhancing their autonomous perception capabilities. The key lies in accurately and in real time reconstructing joint angle parameters in dynamic motion scenarios using limited and noisy sensor data. Traditional filtering-based single-joint pose estimation methods use layered filtering to address external perturbations coupled into the measurement and employ a multiplicative extended Kalman filter for state estimation. However, the highly nonlinear joint dynamics lead to unmodeled perturbations, invalidating the filter's Gaussian assumption. To address these model deficiencies, researchers have proposed single-joint pose estimation methods based on deep learning. Deep learning leverages the hierarchical feature extraction and end-to-end nonlinear mapping capabilities of neural networks to optimize filter parameters and exploit data patterns. However, existing data-driven methods not only require large amounts of labeled data for training but also rely too heavily on the data itself, neglecting the physical model of the system, resulting in poor generalization.
[0003] To address these limitations, this application proposes a humanoid robot posture estimation method based on physical information constraints. It constructs a posture prediction layer based on the dynamic mechanism, adopts a neural network model to replace the gain matrix of the filter, and strengthens the neural network model's characterization of the real mechanism by introducing physical information constraints. Then, accurate posture estimation is achieved through the posture update layer. This hybrid model-driven framework not only ensures physical rationality, but also effectively overcomes the impact of model uncertainty through the parameter adaptation mechanism. Summary of the Invention
[0004] The purpose of the present invention is to provide a humanoid robot posture estimation method based on physical information constraints to solve the problems raised in the above background technology.
[0005] To achieve the above object, the present invention provides the following technical solutions:
[0006] A method for estimating the posture of a humanoid robot based on physical information constraints comprises the following steps:
[0007] Step 1: Establish a posture estimation network model, the structure of which includes a state prediction layer, a lightweight complex-valued neural network layer, and a state update layer;
[0008] Step 2: Introduce physical information constraints and construct the loss function;
[0009] Step 3: Train the posture estimation network model. After training, use the posture estimation network model to perform posture estimation.
[0010] Step 4: Calculate the state prediction value through the state prediction layer and its covariance matrix
[0011] Step 5: Calculate the gain matrix K of the posture estimation network model through the lightweight complex-valued neural network layer k ;
[0012] Step 6: Calculate the posterior estimate of the state through the state update layer and its covariance matrix
[0013] Step 7: Calculate the state estimation result through the posture solution algorithm to obtain the joint posture estimation of the humanoid robot at the current moment;
[0014] Step 8: Repeat steps 4 to 7 to obtain the pose estimation results at all times.
[0015] Furthermore, the parameters of the posture estimation network model in step 1 are set by the state equation and the observation equation:
[0016]
[0017] Where Φ is the state matrix, is the prior estimate, ω k is the process noise, Q is the process noise covariance, y k is the observation value, Η is the observation matrix, is the observation noise, R is the observation noise covariance, and N is the normal distribution.
[0018] Furthermore, the input of the state prediction layer of the posture estimation network model is the prior estimate The output is the state prediction value and its covariance matrix The input of the lightweight complex-valued neural network layer is the observation value ψ k and state prediction value The output is the gain matrix K k , the state update layer input is the gain matrix K k , state prediction value and its covariance matrix The output is the posterior estimate and its covariance matrix
[0019] Furthermore, in step 2, the loss function includes the state estimation loss term L state, physical information loss term L physics , observation residual loss term L obs , where the state estimation loss term L state is the posterior estimate and the true value The error function is the mean square error function, which is defined as follows:
[0020]
[0021] The physical information constraint is based on the optimal gain theorem of Kalman filtering, and the physical information loss term L of the loss function is constructed based on this. physics , the calculation process is given by the following formula:
[0022]
[0023] in, is the partial derivative, J(·) is the posterior estimate The covariance matrix of traces, that is tr(·) represents the trace of the computation matrix, Η is the observation matrix, and Η T is the transposed matrix of H, is the covariance matrix of the state prediction value, R is the observation noise covariance;
[0024] Observation residual loss term L obs From the observed value y k , the observation matrix Η and the posterior estimate The loss function is the mean square error function, which is defined as follows:
[0025]
[0026] Furthermore, the loss function L in step 2 is loss The definition of is given by:
[0027] L loss =αL state +βL physics +γL obs
[0028] Among them, α, β, and γ are all learnable weights, with values ranging from 0 to 1, and satisfying the constraint α+β+γ=1.
[0029] Furthermore, the training of step 3 uses a Bayesian optimization algorithm to optimize the three learnable weights α, β, and γ of step 2, and selects a Gaussian process as a proxy model;
[0030] In the initial stage of training, several weight combinations are randomly selected for evaluation. Subsequently, the latest evaluation results are incorporated into the training data in each iteration, and the proxy model is dynamically updated. The optimization process continues until the preset number of iterations is reached or the objective function value converges stably, and finally the weight combination with the best performance is output.
[0031] Furthermore, the state prediction value in step 4 and its covariance The calculation of is given by:
[0032]
[0033] Among them, Φ, Φ T are the state matrix and its transposed matrix respectively, is the prior estimate The covariance matrix of , Q is the process noise covariance.
[0034] Furthermore, the lightweight complex-valued neural network layer in step 5 includes an input processing module, a fast Fourier transform module, a low-pass filtering module, a complex-valued linear module, an inverse Fourier transform module, and an output processing module. The calculation process of the lightweight complex-valued neural network layer includes: normalizing the time series to zero mean, using fast Fourier transform for frequency domain projection, then processing low-frequency noise through low-pass filtering, interpolating the frequency through the complex-valued linear module, and restoring it to the time domain through inverse Fourier transform, and obtaining the final output matrix K through inverse normalization and linear processing. k , its mathematical description is given by the following formula:
[0035]
[0036] z rFFT = rFFT(z)
[0037] Z LPF =LPF(z rFFT )
[0038] Z′=W·Z LPF
[0039] Z padded =ZeroPadding(Z')
[0040] z irFFT =irFFT(Z padded )
[0041] ξ=σz irFFT +μ
[0042] K k =tanh(z irFFT )
[0043] Among them, z norm is the normalized data, z is the original data, μ, σ are the mean and variance respectively, Z' is the output of the complex-valued linear layer after interpolation, W is the complex-valued weight matrix, ξ is the output after inverse normalization, rFFT(·), LPF(·), ZeroPadding(·), irFFT(·), tanh(·) are the fast Fourier transform function, low-pass filter function, zero padding function, inverse Fourier transform function and activation function respectively, z rFFT is the output of the fast Fourier transform function, Z LPF is the output of the low-pass filter function, Z padded is the output of the zero-filling function, z irFFT is the output of the inverse Fourier transform function.
[0044] Furthermore, the posterior estimate in step 6 is and its covariance matrix The calculation is given by:
[0045]
[0046] in, are the state prediction value and its covariance matrix, K k is the gain matrix, y k is the observation value, H is the observation matrix, and I is the unit matrix.
[0047] Furthermore, the attitude calculation algorithm in step 7 is a quaternion estimator algorithm. After obtaining the gravitational acceleration and geomagnetic field estimation at the current moment, the attitude quaternion is calculated by the quaternion estimator algorithm.
[0048] Compared with the prior art, the beneficial effects of the present invention are as follows: the present invention proposes a humanoid robot posture estimation method based on physical information constraints. First, a lightweight complex-valued neural network model is used to replace the gain matrix of the Kalman filter, and physical information constraints are established based on the Kalman filter optimal gain theorem to strengthen the network model's characterization of the real mechanism. While retaining the theoretical framework of the Kalman filter, it overcomes the defects of the traditional Kalman filter due to its fixed parameters. The hybrid model-driven structure can effectively avoid the network model's large demand for labeled data and the tendency to fall into local optimality, and can provide accurate posture estimation results. BRIEF DESCRIPTION OF THE DRAWINGS
[0049] Figure 1 is a flow chart of the present invention;
[0050] Figure 2 It is the structural block diagram of the posture estimation network model;
[0051] Figure 3is a diagram of a lightweight complex-valued neural network layer. DETAILED DESCRIPTION
[0052] 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.
[0053] The elbow joint posture estimation of a humanoid robot uses the method proposed in the present invention as a posture estimation algorithm.
[0054] Reference Figure 1 , Figure 2 and Figure 3 , a humanoid robot posture estimation method based on physical information constraints, comprising the following steps:
[0055] Step 1: Construct a posture estimation network model. The structure of the posture estimation network model consists of a state prediction layer, a lightweight complex-valued neural network layer (FITS), and a state update layer. Its state variables are defined as augmented matrices of gravity acceleration and geomagnetic field. The parameters of the posture estimation network model are set through the state equation and observation equation:
[0056]
[0057] Where Φ is the state matrix, is the prior estimate, ω k is the process noise, Q is the process noise covariance, y k is the observation value, Η is the observation matrix, is the observation noise, R is the observation noise covariance, and N is the normal distribution.
[0058] In addition, the input of the state prediction layer is the prior estimate The output is the state prediction value and its covariance matrix The input of the lightweight complex-valued neural network layer is the observation value ψ k and state prediction value The output is the gain matrix K k , the state update layer input is the gain matrix K k , state prediction value and its covariance matrix The output is the posterior estimate and its covariance matrix
[0059] Step 2: Introduce physical information constraints and establish a loss function. The loss function includes the state estimation loss term L state , physical information loss term L physics , observation residual loss term L obs .
[0060] Among them, the state estimation loss term L state is the posterior estimate and the true value The error function is the mean square error function, which is defined as follows:
[0061]
[0062] The physical information constraint is based on the optimal gain theorem of Kalman filtering, and the physical information loss term L of the loss function is constructed based on this. physics , the calculation process is given by the following formula:
[0063]
[0064] in, is the partial derivative, J(·) is the posterior estimate The covariance matrix of traces, that is tr(·) represents the trace of the computation matrix, Η is the observation matrix, and Η T is the transposed matrix of H, is the covariance matrix of the state prediction value, and R is the observation noise covariance.
[0065] Observation residual loss term L obs From the observed value y k , the observation matrix Η and the posterior estimate The loss function is the mean square error function, which is defined as follows:
[0066]
[0067] Finally, the loss function is calculated, which is defined as follows:
[0068] L loss =αL state +βL physics +γL obs
[0069] Among them, α, β, and γ are all learnable weights, with values ranging from 0 to 1, and satisfying the constraint α+β+γ=1.
[0070] Step 3: Train the attitude estimation network model. After training, use the attitude estimation network model to perform attitude estimation. The mean square error function is used as the estimation error indicator. The training data consists of three-dimensional angular velocity, three-dimensional acceleration, and three-dimensional magnetic field strength collected by magnetic and inertial measurement units. The label value is the gravity acceleration and geomagnetic field strength collected by the high-precision inertial measurement unit Xsens.
[0071] The training uses the Bayesian optimization algorithm to optimize the three learnable weights described in step 2, and the Gaussian process is selected as the proxy model.
[0072] In the initial stage of training, several weight combinations are randomly selected for evaluation. Subsequently, the latest evaluation results are incorporated into the training data in each iteration, and the proxy model is dynamically updated. The optimization process continues until the preset number of iterations is reached or the objective function value converges stably, and finally the weight combination with the best performance is output.
[0073] Step 4: Calculate the state prediction value through the state prediction layer and its covariance matrix The calculation process is given by the following formula:
[0074]
[0075] Among them, Φ, Φ T are the state matrix and its transposed matrix respectively, is the prior estimate The covariance matrix of , Q is the process noise covariance;
[0076] Step 5: Calculate the gain matrix K of the posture estimation network model through the lightweight complex-valued neural network layer k ,The lightweight complex-valued neural network layer includes an input processing module, a fast Fourier transform module, a low-pass filter module, a complex-valued linear module, an inverse Fourier transform module, and an output processing module, such as Figure 3 As shown in Figure 1, the calculation process of the lightweight complex-valued neural network layer includes: normalizing the time series to zero mean, using fast Fourier transform to perform frequency domain projection, then processing low-frequency noise through low-pass filtering, interpolating the frequency through the complex-valued linear module, and restoring it to the time domain through inverse Fourier transform, and obtaining the final output matrix K through inverse normalization and linear processing. k , its mathematical description is given by the following formula:
[0077]
[0078] z rFFT = rFFT(z)
[0079] Z LPF =LPF(z rFFT )
[0080] Z′=W·Z LPF
[0081] Z padded =ZeroPadding(Z')
[0082] z irFFT =irFFT(Z padded )
[0083] ξ=σz irFFT +μ
[0084] K k =tanh(z irFFT )
[0085] Among them, z norm is the normalized data, z is the original data, μ, σ are the mean and variance respectively, Z' is the output of the complex-valued linear layer after interpolation, W is the complex-valued weight matrix, ξ is the output after inverse normalization, rFFT(·), LPF(·), ZeroPadding(·), irFFT(·), tanh(·) are the fast Fourier transform function, low-pass filter function, zero padding function, inverse Fourier transform function and activation function respectively, z rFFT is the output of the fast Fourier transform function, Z LPF is the output of the low-pass filter function, Z padded is the output of the zero-filling function, z irFFT is the output of the inverse Fourier transform function.
[0086] Step 6: Calculate the posterior estimate of the state through the state update layer and its covariance matrix The calculation process is given by the following formula:
[0087]
[0088] in, are the state prediction value and its covariance matrix, K k is the gain matrix, y k is the observation value, H is the observation matrix, and I is the unit matrix.
[0089] Step 7: After obtaining the current gravity acceleration and geomagnetic field estimation, the state estimation result is calculated by the quaternion estimator algorithm to obtain the current attitude quaternion.
[0090] Step 8: Repeat steps 4 to 7 to obtain the elbow joint posture estimation results at all times.
[0091] 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 humanoid robot posture estimation method based on physical information constraints, characterized in that: The following steps are involved: Step 1: Establish a posture estimation network model, the structure of which includes a state prediction layer, a lightweight complex-valued neural network layer, and a state update layer; Step 2: Introduce physical information constraints and construct the loss function; Step 3: Train the posture estimation network model. After training, use the posture estimation network model to perform posture estimation. Step 4: Calculate the state prediction value through the state prediction layer and its covariance matrix Step 5: Calculate the gain matrix K of the posture estimation network model through the lightweight complex-valued neural network layer k ; Step 6: Calculate the posterior estimate of the state through the state update layer and its covariance matrix Step 7: Calculate the state estimation result through the posture solution algorithm to obtain the joint posture estimation of the humanoid robot at the current moment; Step 8: Repeat steps 4 to 7 to obtain the pose estimation results at all times.
2. A humanoid robot posture estimation method based on physical information constraints according to claim 1, characterized in that: The parameters of the posture estimation network model in step 1 are set by the state equation and the observation equation: Where Φ is the state matrix, is the prior estimate, ω k is the process noise, Q is the process noise covariance, y k is the observation value, Η is the observation matrix, is the observation noise, R is the observation noise covariance, and N is the normal distribution.
3. The method for estimating the posture of a humanoid robot based on physical information constraints according to claim 2, characterized in that: The input of the state prediction layer of the posture estimation network model is the prior estimate The output is the state prediction value and its covariance matrix The input of the lightweight complex-valued neural network layer is the observation value ψ k and state prediction value The output is the gain matrix K k , the state update layer input is the gain matrix K k , state prediction value and its covariance matrix The output is the posterior estimate and its covariance matrix 4. The method for estimating the posture of a humanoid robot based on physical information constraints according to claim 1, characterized in that: In step 2, the loss function includes the state estimation loss term L state , physical information loss term L physics , observation residual loss term L obs , Among them, the state estimation loss term L state is the posterior estimate and the true value The error function is the mean square error function, which is defined as follows: The physical information constraint is based on the optimal gain theorem of Kalman filtering, and the physical information loss term L of the loss function is constructed based on this. physics , the calculation process is given by the following formula: in, is the partial derivative, J(·) is the posterior estimate The covariance matrix of traces, that is tr(·) represents the trace of the computation matrix, Η is the observation matrix, Η T is the transposed matrix of H, is the covariance matrix of the state prediction value, R is the observation noise covariance; Observation residual loss term L obs From the observed value y k , the observation matrix Η and the posterior estimate The loss function is the mean square error function, which is defined as follows:
5. The method for estimating the posture of a humanoid robot based on physical information constraints according to claim 1, characterized in that: The loss function L in step 2 loss The definition of is given by: L loss =αL state +βL physics +γL obs Among them, α, β, and γ are all learnable weights, with values ranging from 0 to 1, and satisfying the constraint α+β+γ=1.
6. The method for estimating the posture of a humanoid robot based on physical information constraints according to claim 5, characterized in that: The training of step 3 uses the Bayesian optimization algorithm to optimize the three learnable weights α, β, and γ of step 2, and selects the Gaussian process as the proxy model; In the initial stage of training, several weight combinations are randomly selected for evaluation. Subsequently, the latest evaluation results are incorporated into the training data in each iteration, and the proxy model is dynamically updated. The optimization process continues until the preset number of iterations is reached or the objective function value converges stably, and finally the weight combination with the best performance is output.
7. The method for estimating the posture of a humanoid robot based on physical information constraints according to claim 1, characterized in that: The state prediction value in step 4 and its covariance The calculation of is given by: Among them, Φ, Φ T are the state matrix and its transposed matrix respectively, is the prior estimate The covariance matrix of , Q is the process noise covariance.
8. The method for estimating the posture of a humanoid robot based on physical information constraints according to claim 1, characterized in that: The lightweight complex-valued neural network layer in step 5 includes an input processing module, a fast Fourier transform module, a low-pass filtering module, a complex-valued linear module, an inverse Fourier transform module, and an output processing module. The calculation process of the lightweight complex-valued neural network layer includes: normalizing the time series to zero mean, using fast Fourier transform for frequency domain projection, then processing low-frequency noise through low-pass filtering, interpolating the frequency through the complex-valued linear module, and restoring it to the time domain through inverse Fourier transform, and obtaining the final output matrix K through inverse normalization and linear processing. k , its mathematical description is given by the following formula: With rFFT =rFFT(z) Z LPF =LPF(z rFFT ) Z'=W·Z LPF WITH padded =ZeroPadding(Z') With irFFT =irFFT(Z padded ) ξ=σz irFFT +m K k =tanh(z irFFT ) Among them, z norm is the normalized data, z is the original data, μ, σ are the mean and variance respectively, Z' is the output of the complex-valued linear layer after interpolation, W is the complex-valued weight matrix, ξ is the output after inverse normalization, rFFT(·), LPF(·), ZeroPadding(·), irFFT(·), tanh(·) are the fast Fourier transform function, low-pass filter function, zero padding function, inverse Fourier transform function and activation function respectively, z rFFT is the output of the fast Fourier transform function, Z LPF is the output of the low-pass filter function, Z padded is the output of the zero-filling function, z irFFT is the output of the inverse Fourier transform function.
9. The method for estimating the posture of a humanoid robot based on physical information constraints according to claim 1, characterized in that: The posterior estimate in step 6 and its covariance matrix The calculation is given by: in, are the state prediction value and its covariance matrix, K k is the gain matrix, y k is the observation value, H is the observation matrix, and I is the unit matrix.
10. The method for estimating the posture of a humanoid robot based on physical information constraints according to claim 1, characterized in that: The attitude calculation algorithm in step 7 is a quaternion estimator algorithm. After obtaining the gravity acceleration and geomagnetic field estimation at the current moment, the attitude quaternion is calculated by the quaternion estimator algorithm.