A real-time online calibration system for the center of mass position of a quadruped robot
By using multi-source data real-time calibration technology, the problem of insufficient accuracy in estimating the center of mass position of quadruped robots was solved, achieving high-precision online calibration of the center of mass and gait balance control, thus improving the stability and adaptability of the robot in complex terrain.
Patent Information
- Application Number
- CN202611360463.8
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2026-09-03
- Publication Date
- 2026-11-17
AI Technical Summary
Existing technologies for estimating and calibrating the center of mass position of quadruped robots suffer from poor adaptability of static offline calibration, limited dynamic accuracy, and cumulative drift problems in inertial fusion estimation, resulting in low balance control accuracy and difficulty in meeting the stability requirements under high dynamic motion.
The system employs a multi-source sensing synchronous acquisition module, a gait support hierarchical module, a kinematic centroid prediction module, a hierarchical constraint observation module, and a two-level joint calibration module. By acquiring multi-source data in real time, hierarchically dividing the gait phase, and combining inertial data and torque information, it performs real-time online calibration of the centroid position, eliminates invalid observation data, and iteratively updates the model parameters.
Real-time online calibration of the quadruped robot's center of mass position was achieved, improving the accuracy of center of mass calculation under load changes and high dynamic motion, ensuring high precision and stability of gait balance control, and enhancing adaptability to complex terrain.
Smart Images

Figure CN123103652A_ABST
Abstract
Description
Technical Field
[0001] This invention belongs to the field of quadruped robot dynamics and intelligent motion control technology, specifically a real-time online calibration system for the center of mass position of a quadruped robot. Background Technology
[0002] Quadruped robots possess multi-degree-of-freedom motion capabilities and strong adaptability to unstructured terrain. They can replace human labor in high-risk operations in complex scenarios such as disaster relief, industrial inspection, field exploration, and special transportation, and are one of the core research directions in the field of legged robots.
[0003] The center of mass position is a core state parameter for gait planning, balance control, and stability margin calculation of quadruped robots. Its measurement accuracy directly determines the robot's dynamic walking performance, load-bearing capacity, and reliability in traversing complex terrain. Currently, the acquisition of the center of mass position of quadruped robots mainly relies on two technical paths. The first is the forward kinematics method based on the body structure model. The center of mass parameters of each link and drive component are obtained through static calibration before leaving the factory. Combined with the joint angles collected in real time by the joint encoder, the theoretical position of the center of mass of the whole machine is obtained through rigid body kinematics calculation. The second is the multi-sensor fusion estimation method. With the torso inertial measurement unit as the core, it integrates data from joint encoders, foot force sensors, etc., and solves the motion state and position information of the center of mass through the extended Kalman filter algorithm.
[0004] However, existing robot center of mass estimation and calibration technologies have many shortcomings. First, the adaptability of static offline calibration conditions is poor. The factory-fixed calibration parameters cannot be dynamically updated with changes in load, component wear and deformation, and external impacts, which can easily cause mismatch between the control model and the actual working conditions, reduce the accuracy of balance control, and lead to the risk of equipment instability. Second, the dynamic accuracy of pure kinematic calculation is limited. It does not consider joint clearance, link deformation, and nonlinear errors of drive hysteresis. Under high dynamic motion conditions, the deviation of center of mass calculation increases significantly, making it difficult to meet the requirements of high-precision balance control. Third, inertial fusion estimation has the problem of cumulative drift. The IMU quadratic integral estimation method is susceptible to sensor zero bias and noise interference, and lacks a multi-constraint closed-loop calibration mechanism. After long-term operation, the estimation error continues to accumulate. Therefore, this invention provides a real-time online calibration system for the center of mass position of a quadruped robot. Summary of the Invention
[0005] In order to overcome the shortcomings of the prior art, at least one technical problem raised in the background art is solved.
[0006] The technical solution adopted by this invention to solve its technical problem is: a real-time online calibration system for the center of mass position of a quadruped robot, comprising: The multi-source sensing synchronous acquisition module is used to synchronously acquire the six-axis inertial data of the torso inertial measurement unit, the real-time angle data of each joint encoder, and the contact force and torque data of each foot three-dimensional force sensor, and to complete the timestamp alignment and noise preprocessing. The gait support classification module is used to divide the robot's motion cycle into four phase states: swing phase, single support phase, double support phase and three-point support phase, based on the foot contact force threshold and joint pose calculation results, and output the corresponding phase identifier and the number of supporting feet. The kinematic center of mass prediction module has a built-in rigid body kinematic model with link elastic deformation compensation. It combines real-time joint angle data to calculate the kinematic prediction position of the whole machine's center of mass and outputs the deformation estimate of the center of mass position. The graded constraint observation module is used to match the corresponding level of centroid constraint observation equations based on the current phase identifier and the number of support feet. The module has a built-in observation validity verification unit, which determines whether the current observation value is valid based on the consistency of the overall torque balance, and removes invalid observation data under foot slippage and impact disturbance. The dual-level joint calibration module incorporates a position-level rolling filter unit and a parameter-level trigger update unit. The position-level rolling filter unit uses the centroid motion state obtained by quadratic integration of six-axis inertial data as the prediction term and the effective observation value output by the hierarchical constraint observation module as the measurement correction term, combining the deformation prediction value to complete the instantaneous position calibration of the centroid in each cycle. The parameter-level trigger update unit has a preset deviation threshold. When the continuous deviation between the kinematic predicted position and the calibrated centroid position exceeds the threshold, the rigid body load three-dimensional offset of the whole machine centroid, the elastic deformation compensation coefficient of the connecting rod, and the zero bias of the acceleration of the inertial measurement unit are used as the parameter set to be estimated. Parameter iterative identification is performed within the multi-support phase time window, and the model parameters of the kinematic centroid prediction module are updated synchronously. The data output module is used to output the calibrated real-time position of the centroid and the updated model parameters to the quadruped robot gait balance control system.
[0007] Preferably, the gait support grading module divides the phase states, determines the contact / leave state of a single foot one by one according to the contact force of each foot and the preset contact threshold, counts the number of feet in the contact state at the current moment, and verifies the geometric validity of the support polygon by combining the joint pose calculation results of each supporting foot. Finally, based on the number of effective supporting feet, it is divided into four phase states: swing phase, single support phase, double support phase and three-point support phase.
[0008] Preferably, the observation validity verification unit is as follows: based on the current kinematic model and trunk posture data, it calculates the theoretical torque vector of the whole machine weight relative to each supporting foot end point, performs a difference calculation between the torque vector measured by the foot end three-dimensional force sensor and the theoretical torque vector to obtain the torque deviation amplitude, and when the deviation amplitude exceeds the allowable range, it determines that the current observation value is invalid and discards it; if it does not exceed the allowable range, it determines that it is valid and outputs it to the dual-level joint calibration module.
[0009] Preferably, the hierarchical constraint observation module matches the constraint observation equation as follows: it pre-configures observation equations and initial weight coefficients of corresponding levels for the four types of phase states. When a phase state switch is detected, it does not directly jump to the weight coefficient of the target phase, but uses a fixed proportion of the current gait cycle as a transition interval to linearly adjust the weight coefficient to the target value, thus completing the smooth switching of the observation equation.
[0010] Preferably, the position-level rolling filter unit includes: using a single complete gait cycle as the rolling window length, triggering a measurement correction update only once at the middle moment of the support phase within each window, maintaining inertial integral recursion at other times, and sliding the window backward by one cycle step after each gait cycle is completed, discarding the historical data of the earliest cycle, and retaining the state data of the latest cycle for the next cycle recursion.
[0011] Preferably, the triggering determination process of the parameter-level triggering update unit is as follows: calculate the average deviation between the kinematic prediction position and the calibrated centroid position in each gait cycle in real time, accumulate and statistically analyze the average deviation for multiple consecutive gait cycles, and trigger the parameter iteration identification process when the average deviation for multiple consecutive cycles exceeds the preset deviation threshold. If the condition of continuous exceeding the threshold is not met, keep the current model parameters unchanged.
[0012] Preferably, the kinematic center of mass prediction module calculates the deformation estimate as follows: based on the output torque of each joint and the link structure parameters, the bending load borne by each link is inferred, and combined with the stiffness characteristics of the link material, the elastic deformation of each link under the load is calculated. The deformation of each link is substituted into the rigid body kinematic model for pose correction, and finally the kinematic prediction position of the center of mass with deformation compensation and the deformation estimate are obtained.
[0013] Preferably, the multi-source sensing synchronous acquisition module completes timestamp alignment, using the sampling clock of the torso inertial measurement unit as the reference time axis, and resamples the joint encoder data and foot force sensor data with lower sampling rates to the same sampling frequency as the inertial data using interpolation, and assigns a timestamp consistent with the reference clock to each group of resampled data, thereby completing the time synchronization of multi-source data.
[0014] Preferably, the parameter-level trigger update unit performs parameter iterative identification as follows: after triggering the identification process, selects multiple consecutive gait cycles containing double support phases and three-point support phases as identification windows, accumulates effective constraint observation data and kinematic prediction data cycle by cycle within the window, iteratively calculates the parameter set to be estimated using a recursive solution method, and updates the solved parameters to the kinematic centroid prediction module after the identification window ends, while resetting the deviation statistics count.
[0015] Preferably, before the data output module outputs data, it uses a low-pass filter to smooth the real-time position of the calibrated centroid. When the model parameters are detected to be updated, the new parameters are gradually substituted into the kinematic prediction calculation over several subsequent gait cycles to reduce the jump in output position caused by parameter mutations. The smoothed centroid position and the updated model parameters are then output.
[0016] The beneficial effects of this invention are as follows: 1. The present invention discloses a real-time online calibration system for the center of mass position of a quadruped robot, which combines gait hierarchical constraint observation with rolling time-domain filtering to achieve real-time online calibration of the center of mass position throughout the gait cycle. It does not require the robot to stop and enter the calibration posture, and simultaneously identifies rigid body load offset, link deformation compensation coefficient and IMU zero bias, effectively corrects model parameter mismatch and sensor integral drift, and significantly improves the accuracy of center of mass measurement under load changes and high dynamic motion.
[0017] 2. The quadruped robot's real-time online calibration system for center of mass position described in this invention, through a built-in observation validity verification and phase smoothing switching mechanism, can actively eliminate invalid observation data under foot slippage and impact disturbances, reduce filtering oscillations, and, in conjunction with parameter gradual change and output smoothing processing, ensure continuous and stable calibration results. It can provide high-precision center of mass feedback for gait balance control, effectively improving the walking stability and working condition adaptability of quadruped robots in complex terrain. Attached Figure Description
[0018] The invention will now be further described with reference to the accompanying drawings.
[0019] Figure 1 This is a flowchart illustrating the overall system architecture in this invention; Figure 2 This is a schematic diagram of the calibration method in this invention. Detailed Implementation
[0020] To make the technical means, creative features, objectives and effects of this invention easier to understand, the invention will be further described below in conjunction with specific embodiments.
[0021] like Figure 1-2As shown in the embodiment of the present invention, a real-time online calibration system for the center of mass position of a quadruped robot includes: The multi-source sensing synchronous acquisition module is used to synchronously acquire the six-axis inertial data of the torso inertial measurement unit, the real-time angle data of each joint encoder, and the contact force and torque data of each foot three-dimensional force sensor, and to complete the timestamp alignment and noise preprocessing. The gait support classification module is used to divide the robot's motion cycle into four phase states: swing phase, single support phase, double support phase and three-point support phase, based on the foot contact force threshold and joint pose calculation results, and output the corresponding phase identifier and the number of supporting feet. The kinematic center of mass prediction module has a built-in rigid body kinematic model with link elastic deformation compensation. It combines real-time joint angle data to calculate the kinematic prediction position of the whole machine's center of mass and outputs the deformation estimate of the center of mass position. The graded constraint observation module is used to match the corresponding level of centroid constraint observation equations based on the current phase identifier and the number of support feet. The module has a built-in observation validity verification unit, which determines whether the current observation value is valid based on the consistency of the overall torque balance, and removes invalid observation data under foot slippage and impact disturbance. The dual-level joint calibration module incorporates a position-level rolling filter unit and a parameter-level trigger update unit. The position-level rolling filter unit uses the centroid motion state obtained by quadratic integration of six-axis inertial data as the prediction term and the effective observation value output by the hierarchical constraint observation module as the measurement correction term, combining the deformation prediction value to complete the instantaneous position calibration of the centroid in each cycle. The parameter-level trigger update unit has a preset deviation threshold. When the continuous deviation between the kinematic predicted position and the calibrated centroid position exceeds the threshold, the rigid body load three-dimensional offset of the whole machine centroid, the elastic deformation compensation coefficient of the connecting rod, and the zero bias of the acceleration of the inertial measurement unit are used as the parameter set to be estimated. Parameter iterative identification is performed within the multi-support phase time window, and the model parameters of the kinematic centroid prediction module are updated synchronously. The data output module is used to output the calibrated real-time position of the centroid and the updated model parameters to the quadruped robot gait balance control system. By adopting a data flow-driven layered architecture, from bottom to top, there are six functional modules: perception layer, phase determination layer, prediction layer, observation layer, calibration layer and output layer. The overall architecture adopts a producer-consumer data interaction mode. During system operation, the multi-source sensing synchronous acquisition module, acting as a data producer, continuously collects raw data from three types of sensors. After time synchronization, outlier removal, and noise preprocessing, it writes the data to a shared memory data pool. The gait support hierarchical module and the kinematic centroid prediction module, acting as primary consumers, subscribe to the synchronized sensing data and output gait phase identifiers and centroid kinematic prediction values, respectively. The hierarchical constraint observation module, acting as a secondary consumer, combines the phase identifiers and kinematic prediction results to construct and output centroid constraint observation values that have been validated. The dual-level joint calibration module, acting as the core computing layer, synchronously receives inertial data, kinematic prediction values, and constraint observation values, and executes position-level real-time data in parallel. The system employs time-based calibration and parameter-level triggered identification. Position calibration is updated every cycle, while parameter identification is initiated only when the deviation exceeds a threshold. After identification, the built-in parameters of the kinematic model are updated in reverse. Finally, the data output module smooths the calibration results before outputting them to the quadruped robot's gait balance control system, forming a complete closed-loop calibration link of perception, prediction, observation, calibration, and feedback. The entire process does not require the robot to stop or enter a specific calibration posture. The system's built-in parameter storage area is located in non-volatile flash memory, storing the factory-calibrated link mass, relative position of the center of mass, stiffness coefficient, and initial parameters of the IMU mounting matrix. The stored parameters are automatically overwritten after each parameter update to ensure that the parameters are not lost after power failure.
[0022] like Figure 1-2 As shown, the gait support grading module divides the phase states, determines the contact / lift-off state of a single foot one by one according to the contact force of each foot end and the preset contact threshold, counts the number of feet in the contact state at the current moment, and verifies the geometric validity of the support polygon by combining the joint pose calculation results of each supporting foot. Finally, based on the number of effective supporting feet, it is divided into four phase states: swing phase, single support phase, double support phase and three-point support phase. The gait support grading module uses foot force contact determination as the basis and support polygon geometry verification as the correction, and performs four-level phase division. The complete process is as follows: Single-foot contact state anti-shake judgment: preset vertical contact force base threshold The threshold is set to 5% of the robot's unloaded weight; in actual operation, the threshold is dynamically adjusted according to the estimated weight of the entire robot. ,in For no-load calibration quality, Estimate the current overall quality of the machine; To avoid momentary misjudgments caused by foot impact and vibration, an anti-shake mechanism is added: for the first... One leg, only when the foot tip is in direct contact with the force for three consecutive sampling cycles. All meet When the foot is in contact, a contact marker is set. Only when all three consecutive sampling periods satisfy the condition. At that time, it is determined to be in an off-ground state, and set... The intermediate transition state retains the judgment result of the previous cycle. Support polygon validity verification: Calculate the horizontal coordinates of the plantar center points of all contacting feet in the world coordinate system using the forward kinematics solution of DH. The Graham scan method was used to construct the supporting convex hull polygons for all contact feet, and the convex hull area was calculated based on the shoelace formula. In the formula For the number of contacts, and Preset minimum effective area threshold ,like If all contact feet are deemed valid support feet, then the corresponding validity indicator will be displayed. If the convex hull area is too small, such as when the two feet almost overlap or a single foot makes virtual contact, then the foot with the smallest contact force is removed and the convex hull is recalculated until the area meets the requirements or the number of remaining feet is 0. Fourth-level phase state mapping output: Statistical count of the final effective support levels. The phase state is mapped according to the following rules: : Full swing phase, all feet are off the ground, corresponding to the jumping and airborne conditions; : Single support phase, with only one foot in effective contact, corresponding to a fast running gait; : Double support phase, with effective contact between both feet, corresponds to the transition phase between walking and jogging; Three-point support phase, with three or more feet in effective contact, corresponding to slow walking and standing stability conditions; The module output includes phase identification signals, a list of valid support foot numbers, coordinates of the vertex of the support polygon, and the duration of the current phase, which are simultaneously sent to the hierarchical constraint observation module and the dual-level joint calibration module.
[0023] like Figure 1-2 As shown, the observation validity verification unit is as follows: based on the current kinematic model and trunk posture data, it calculates the theoretical torque vector of the whole machine weight relative to each supporting foot end point, performs difference calculation between the torque vector measured by the foot end three-dimensional force sensor and the theoretical torque vector to obtain the torque deviation amplitude. When the deviation amplitude exceeds the allowable range, the current observation value is determined to be invalid and discarded; if it does not exceed the allowable range, it is determined to be valid and output to the dual-level joint calibration module. The observation validity verification unit, based on the principle of static torque balance, verifies the consistency between foot force data and kinematic model, and eliminates invalid observation data caused by slippage, impact, or sensor malfunction. The complete process is as follows: Coordinate system and basic parameter preparation: The world coordinate system adopts the Northeast-East-South (ENU) coordinate system, and the origin of the body coordinate system is located at the geometric center of the torso, with the x-axis forward, the y-axis to the left, and the z-axis upward; the rotation matrix from the body coordinate system to the world coordinate system is obtained by solving the quaternions output by the IMU. This is used for coordinate transformation of all vectors; the overall gravity vector in the world coordinate system is... ,in It is the acceleration due to gravity; Theoretical torque vector calculation: Based on the current kinematic model, calculate the position of the machine's center of mass in the world coordinate system. ; for the first For each effective supporting foot, calculate the world coordinates of its foot endpoint. ; According to the lever arm cross product principle, the total weight of the machine relative to the first lever arm cross product is... The theoretical moment vector at each supporting foot endpoint is: In the formula For three-dimensional vector cross product operations, the theoretical torque is generated only by gravity, reflecting the expected torque value under static equilibrium; Preprocessing and coordinate transformation of measured torque: The torque output by the foot-end three-dimensional force sensor is the original value in the sensor coordinate system. First subtract the zero bias value of the static calibration upon power-on. Then, by using the sensor mounting matrix and the foot posture rotation matrix, the values are gradually transformed to the world coordinate system to obtain the measured torque. ; Deviation Judgment and Data Screening: Calculating the L2 deviation between theoretical torque and measured torque: Preset allowable deviation threshold The threshold is dynamically adjusted according to the robot's movement speed: the nominal value is taken when stationary, it is amplified by 1.5 times when walking, and amplified by 2 times when running, to adapt to torque fluctuations under dynamic working conditions. Judgment rule: If all valid support feet satisfy... If the current observation frame is deemed to be valid as a whole, the centroid observation value is output to the dual-level joint calibration module; if any foot deviation exceeds the threshold, the current frame is deemed to have foot slippage, impact disturbance or sensor abnormality, and the observation data of that frame is discarded and not included in this measurement update. The module simultaneously counts the number of consecutive invalid frames. If more than 10 consecutive frames are deemed invalid, a sensor abnormality alarm is triggered, and the main control system is notified to perform fault diagnosis.
[0024] like Figure 1-2As shown, the hierarchical constraint observation module matches the constraint observation equation as follows: the observation equation and initial weight coefficients of the corresponding levels are pre-configured for the four types of phase states respectively. When a phase state switch is detected, it does not directly jump to the weight coefficient of the target phase, but uses the fixed proportion of the current gait cycle as the transition interval and linearly adjusts the weight coefficient to the target value to complete the smooth switch of the observation equation. The hierarchical constraint observation module pre-configures observation equations and initial weights for four types of phases, dynamically matches them based on real-time phase identifiers, and reduces weight jumps through a smooth transition mechanism, as detailed below: Hierarchical observation equations and weight configuration: Based on the strength of the support constraints, centroid position observation equations with different accuracies are constructed for the four types of phases, and corresponding initial weight coefficients are configured: Three-point support phase: This provides the strongest support constraint. A high-precision horizontal position observation equation is constructed based on the zero-moment point (ZMP). The ZMP coordinates on the horizontal plane are calculated from the foot force and moment. Combining the linear inverted pendulum model, the horizontal projection of the center of mass and ZMP satisfy the dynamic constraints, constructing the horizontal position observation equation of the center of mass. The vertical direction is supplemented by the kinematic height constraint, and the initial weighting coefficient of this phase is... The observation noise covariance is minimized; Dual-support phase: Support constraint is secondary, also using ZMP observation equations, but due to the reduction in support polygon size, observation robustness decreases, and the initial weighting coefficients are adjusted. ; Single-support phase: Only a single point of support exists, and the ZMP constraint degenerates into a foot endpoint constraint, significantly reducing the observation accuracy. Only the torso attitude angle geometric constraint is retained to construct a low-precision observation equation, with initial weighting coefficients... ; Oscillating phase: No effective foot contact constraint, force observation channel closed, weighting coefficient It relies entirely on inertial integrals and kinematic recursion; Observation noise covariance It is inversely proportional to the weighting coefficient, satisfying the following relationship: In the formula The reference observation noise covariance matrix is used. The higher the weight, the smaller the measurement noise and the higher the confidence of the filter in the observation. Phase smoothing switching mechanism: When a phase state transition is detected, triggered by the rising / falling edge of the contact flag, the switch does not directly switch to the target weight. Instead, a transition interval is set to achieve a linear and gradual transition. The duration of the transition interval is... ,in Estimating the duration of the current gait cycle using a scaling factor. Take 0.15; Let the phase switching time be The original phase weight is The target phase weight is Then at any time within the transition interval The actual weights are: After the transition interval ends, the weight remains at the target value. This mechanism can effectively reduce the oscillation of the filter output caused by the sudden change in the observation weight during phase switching and improve the smoothness of the calibration results.
[0025] like Figure 1-2 As shown, the position-level rolling filter unit includes: using a single complete gait cycle as the rolling window length, triggering a measurement correction update only once at the middle moment of the support phase within each window, and maintaining inertial integral recursion at other times. After each gait cycle is completed, the window slides backward by one cycle step, discarding the historical data of the earliest cycle and retaining the state data of the latest cycle for the next cycle recursion. The position-level rolling filter unit adopts a rolling time-domain extended Kalman filter architecture, using a single gait cycle as the window length, to achieve high-frequency real-time calibration of the instantaneous position of the centroid. The specific implementation process is as follows: State variables and state equation construction: Nine state variables, including the three-dimensional position, velocity, and acceleration of the center of mass, are selected to form the state vector. A constant acceleration (CA) motion model is used to construct the state transition equations, and the discretized state transition matrix is then used. It is a 9th-order block diagonal matrix, and the submatrix for each axis is: in Given the filter sampling period (1ms), the overall state transition matrix is... ; Input matrix The corresponding IMU acceleration input will be converted to world coordinates. As the control input, the state prediction equation is: Process noise covariance matrix IMU-based angular velocity random walk and acceleration random walk parameter tuning reflects the inherent noise level of the inertial integral; Scrolling window mechanism and measurement update timing: Set a single complete gait cycle as the length of the scrolling window, which contains... One sampling point, The measurement update is dynamically adjusted according to the gait frequency. Unlike the traditional Kalman filter that performs measurement updates every frame, this scheme only performs a measurement update once in each window. The trigger time is the middle of the support phase: the timing starts from the phase switch to the support phase. When the duration of the support phase reaches 50% of the estimated total support duration, a measurement update is triggered. At other times, only the state prediction recursion is performed. Before updating the measurement, first introduce the estimated value of the linkage deformation. The observations are corrected, and the corrected observation vector is obtained. To compensate for the displacement of the center of mass caused by the elastic deformation of the connecting rod; Measurement update execution standard extended Kalman filter update formula: In the formula For the observation matrix, The observation noise covariance matrix corresponding to the current phase. Kalman gain; Window scrolling update: After each complete gait cycle, the scrolling window slides backward by one cycle step, discarding the historical state and covariance data of the earliest cycle, and retaining the state estimate of the latest cycle as the initial state of the next window. This mechanism limits the influence range of historical data, reduces the accumulation of errors caused by long-term integration, and reduces the amount of computation, adapting to the computing power level of embedded processors.
[0026] like Figure 1-2 As shown, the triggering determination process of the parameter-level triggering update unit is as follows: calculate the average deviation between the kinematic prediction position and the calibrated centroid position in each gait cycle in real time, accumulate and statistically analyze the average deviation for multiple consecutive gait cycles, and trigger the parameter iteration identification process when the average deviation for multiple consecutive cycles exceeds the preset deviation threshold. If the condition of continuous exceeding the threshold is not met, the current model parameters remain unchanged. The parameter-level trigger update unit identifies the degree of mismatch between model parameters and actual state through periodic deviation statistics and continuous threshold judgment. Parameter identification is triggered only when the deviation continues to exceed the limit, reducing frequent parameter fluctuations. The specific process is as follows: Single-cycle average deviation calculation: After each gait cycle, the kinematic predicted centroid positions at all sampling times within that cycle are calculated. With the calibrated centroid position Calculate the three-dimensional Euclidean distance deviation and take the average value over the entire period: In the formula For the first One gait cycle, The number of sampling points per cycle is given, and the average deviation per cycle for the x, y, and z axes is calculated separately. Used for axis determination; Continuous Deviation Accumulation and Triggering Logic: Set a continuous over-threshold count variable. The initial value is 0, and the triggering judgment rules are as follows: If the overall average deviation of the current period The overall deviation threshold is set at 15mm, or if any axial deviation exceeds the corresponding sub-axis threshold, then... ; If the current cycle deviation does not exceed the threshold, then Reset to zero and start accumulating again; when When the continuous period threshold is 4, it is determined that there is a persistent mismatch in the kinematic model parameters, triggering the parameter iterative identification process; If the triggering condition is not met, keep the current kinematic model parameters unchanged and only perform position-level real-time calibration; In addition, when the robot first enters a stationary standing state after being powered on, it automatically triggers a parameter identification to complete the initial calibration; when the robot remains stationary for more than 5 seconds, it will also periodically trigger a lightweight identification to correct sensor zero drift.
[0027] like Figure 1-2 As shown, the kinematic center of mass prediction module calculates the deformation estimate as follows: based on the output torque of each joint and the link structure parameters, the bending load borne by each link is inferred, and combined with the stiffness characteristics of the link material, the elastic deformation of each link under the load is calculated. The deformation of each link is substituted into the rigid body kinematic model for pose correction, and finally the kinematic prediction position of the center of mass with deformation compensation and the deformation estimate are obtained. The kinematic center of mass prediction module incorporates a rigid body kinematic model with link elastic deformation compensation. It estimates the link bending deformation using a cantilever beam mechanics model and corrects the predicted center of mass position. The specific process is as follows: Link bending load feedback: Each leg of the quadruped robot consists of three links: hip lateral swing, thigh, and calf, for a total of 12 links. The thigh and calf are the main load-bearing components, accounting for over 90% of the deformation, while the hip link has relatively small deformation. The output torque is based on feedback from the servo actuators of each joint. ( (Number the joints), and in conjunction with the lever arm relationship of the link, the joint torque is equivalent to the bending load at the root of the link. Taking the lower leg link as an example, the output torque of the knee joint is equivalent to the bending torque of the lower leg link around the knee joint, which corresponds to the bending moment at the fixed end of the cantilever beam. Single link deformation calculation: The thigh and calf links are equivalent to a cantilever beam structure with one end fixed and the other end free. The link uses a hollow circular tube section, and the equivalent length of the link is assumed to be... The outer diameter of the cross section is The inner diameter is Then the moment of inertia of the cross section is: Let the elastic modulus of the connecting rod material be... (Aluminum alloy, 71 GPa), bending moment at the root Under the action, the bending deflection (deformation) at the end of the connecting rod is: This deformation is the elastic displacement of the end of the link relative to the rigid body model, and its direction is perpendicular to the neutral axis of the link; Center-of-mass shape mapping and kinematic compensation: Constructing the deformation Jacobian matrix This matrix describes the mapping relationship between the local deformation of each link and the position offset of the center of mass in the world coordinate system of the whole machine. It is derived from the kinematic Jacobian of the current joint angle and is updated in real time with the joint pose. The estimated deformation of the center of gravity of the entire machine is as follows: Kinematic prediction of the center of mass with deformation compensation: First, the standard DH parameter method is used to perform forward kinematics of the rigid body to calculate the position of the center of mass of each link in the world coordinate system. The position of the rigid body's center of mass is obtained by mass-weighted summation. In the formula For the first The rated mass of the connecting rod, For the first The world coordinates of the center of mass of the root link; By superimposing the estimated elastic deformation, the kinematic prediction of the center of mass with deformation compensation is obtained: The module will simultaneously The output is sent to a dual-level joint calibration module for deformation correction of the measured observations. After static load testing, it was verified that the prediction error of the centroid position under full load conditions can be reduced by about 45% after introducing deformation compensation.
[0028] like Figure 1-2 As shown, the multi-source sensing synchronous acquisition module completes timestamp alignment. Using the sampling clock of the torso inertial measurement unit as the reference time axis, it resamples the joint encoder data and foot force sensor data with lower sampling rates to the same sampling frequency as the inertial data using interpolation. It assigns a timestamp consistent with the reference clock to each group of resampled data, thus completing the time synchronization of multi-source data. The multi-source sensing synchronous acquisition module uses the IMU sampling clock as a global reference. It achieves timestamp alignment of multi-source data through transmission delay compensation and resampling interpolation, and also performs noise preprocessing. The specific process is as follows: Reference Time Axis and Transmission Delay Compensation: The global reference time axis is based on a 1000Hz sampled IMU clock, and the reference time sequence is as follows: Sampling interval Pre-calibrate the signal transmission delay of three types of sensors: IMU data is transmitted via the SPI interface, with a delay of... The joint encoder is read via the encoder interface, with a delay. The foot force sensor transmits data via CAN bus, with a delay. For the raw data of each type of sensor, the corresponding transmission delay is first added to correct the actual sampling time of the data and eliminate the time deviation of the hardware transmission link. Multi-rate data linear resampling: The original sampling rate of the joint encoder and foot force sensor is 500Hz, and the original time sequence is... The sampling interval is 2ms; to align with a 1ms reference time axis, linear interpolation is used for resampling: for any reference time... Locate two adjacent sampling points before and after it in the original time sequence of the sensor. and ,satisfy Calculate the resampled value using the following formula: In the formula The original sensor measurements are used, and the linear interpolation has low computational load and high real-time performance. It is compatible with embedded processors, and the interpolation accuracy of the slowly changing signals of joint angle and foot force meets the fusion requirements. Noise preprocessing: After time synchronization is complete, two-stage noise preprocessing is performed on all data: Outlier removal: The 3σ criterion is used. Based on the mean and standard deviation of the most recent 20 sampling points, data exceeding three times the standard deviation are judged as outliers and replaced with the valid values of the previous time step. Low-pass filtering: A first-order Butterworth low-pass filter with a cutoff frequency of 50Hz is used to filter out high-frequency noise caused by mechanical vibration and electromagnetic interference from motors; the final output synchronization data uniformly carries a reference timestamp and is written to shared memory for downstream modules to call, and the time synchronization error is controlled within 0.1ms.
[0029] like Figure 1-2As shown, the parameter-level trigger update unit performs parameter iteration identification as follows: after triggering the identification process, selects multiple consecutive gait cycles containing double support phases and three-point support phases as identification windows, accumulates effective constraint observation data and kinematic prediction data cycle by cycle within the window, and iteratively calculates the parameter set to be estimated using a recursive solution method. After the identification window ends, updates the solved parameters to the kinematic centroid prediction module, and resets the deviation statistics count. After the parameter-level trigger update unit triggers the identification process, a multi-period observation window is selected, and the recursive least squares method is used to achieve joint identification of multiple parameters. The specific process is as follows: Identification window and parameter set construction: After triggering identification, select continuous... gait cycles ( Take 6) as the identification window to ensure that the window contains effective observation data of at least 3 double-support phases and 2 three-point support phases, so as to ensure the identifiability of the observation equation; The set of parameters to be estimated contains 7 parameters, defined as follows: in: : Rigid body load three-dimensional offset, reflecting the overall displacement of the center of mass caused by load changes and component wear; : Linkage elastic deformation compensation coefficient, used to correct the deviation between theoretical deformation calculation and actual deformation; IMU acceleration zero bias reflects the zero bias change caused by sensor temperature drift and aging; To reduce the number of physically unreasonable parameters identified, upper and lower limits are set for each parameter: rigid body offset is limited to ±0.1m, deformation coefficient is limited to 0.5~1.5, and zero acceleration offset is limited to ±0.5m / s². If the range is exceeded, the parameter is truncated to the boundary value. Recursive least squares iterative solution: Construct a parameterized centroid position prediction model, express the kinematic prediction position as a linear function of the parameters to be estimated, and obtain the observation equation: In the formula For the parameter observation matrix, To observe the residuals, recursive least squares (RLS) with a forgetting factor is used for iterative solution, and the update formula is as follows: In the formula To identify the gain matrix, Here is the parametric covariance matrix, and the forgetting factor is... The value is set to 0.98 to give higher weight to recent data, adapting to the slow time-varying characteristics of the parameters; Convergence Criteria and Parameter Update: During the identification process, parameter changes are monitored in real time. If the relative change of the parameter at 10 consecutive sampling points is less than 0.1%, the parameters are considered to have converged, and the identification can be terminated early. If convergence is not achieved by the end of the window, the final parameter values of the window are used. After identification is completed, the converged parameter set is updated. The following steps are implemented in the kinematic center of mass prediction module: rigid body offsets are superimposed onto the initial coordinates of the link's center of mass; deformation compensation coefficients correct the deformation calculation formula; zero acceleration bias is used for IMU data preprocessing; online updates of model parameters are completed; and continuous deviation count variables are also included. Reset to 0 and restart deviation monitoring; If the amount of valid observation data in the identification window is less than 60% of the preset value, the identification is deemed invalid, the parameter update is abandoned, the original parameters are retained, and the process waits for the next trigger.
[0030] like Figure 1-2 As shown, before the data output module outputs data to the outside, it uses a low-pass filter to smooth the real-time position of the calibrated centroid. When the model parameters are detected to be updated, the new parameters are gradually substituted into the kinematic prediction calculation in the subsequent gait cycles to reduce the jump in output position caused by parameter mutation. The smoothed centroid position and the updated model parameters are then output to the outside. Before outputting data, the data output module performs position smoothing and parameter gradual change processing to reduce the impact of output jumps on gait control stability. The specific process is as follows: Dynamic smoothing filter for centroid position: The real-time position of the calibrated centroid is smoothed using a first-order low-pass filter. The filter equation is as follows: In the formula The filter coefficients are dynamically adjusted using a mechanism: when the robot is stationary or moving slowly... Improve smoothness; when running or making quick turns. This improves response speed; the filter coefficients are linearly interpolated with the motion speed, balancing smoothness and dynamic response. Model parameter gradual transition mechanism: When a model parameter update event is detected, to reduce the step jump in kinematic prediction position caused by abrupt parameter changes, a gradual transition of parameters is enabled, with a set number of gradual transition periods. The old parameters before the update were: The updated parameters are as follows: Then the first A gradual cycle ( The actual effective parameters are: The parameters are gradually transitioned from old values to new values over four gait cycles to ensure continuous change in kinematic prediction values and reduce spike pulses in the filtered output. Output data encapsulation and transmission: The final output data frame includes: the calibrated centroid 3D position, 3D velocity, model parameter update flag, data validity flag, and current phase identifier; the data is transmitted to the gait balance control system via shared memory, and can also be sent to the servo drive layer via EtherCAT bus for feedforward control compensation; The output data update frequency is consistent with the IMU sampling frequency, which is 1000Hz, ensuring that the gait control system can obtain the real-time continuous center of mass state and support high dynamic balance control.
[0031] The foregoing has shown and described the basic principles, main features, and advantages of the present invention. Those skilled in the art should understand that the present invention is not limited to the above embodiments. The embodiments and descriptions in the specification are merely illustrative of the principles of the invention. Various changes and modifications can be made to the invention without departing from its spirit and scope, and all such changes and modifications fall within the scope of the present invention as claimed. The scope of protection of the present invention is defined by the appended claims and their equivalents.
Claims
1. A real-time online calibration system for the center of mass position of a quadruped robot, characterized in that, include: The multi-source sensing synchronous acquisition module is used to synchronously acquire the six-axis inertial data of the torso inertial measurement unit, the real-time angle data of each joint encoder, and the contact force and torque data of each foot three-dimensional force sensor, and to complete the timestamp alignment and noise preprocessing. The gait support classification module is used to divide the robot's motion cycle into four phase states: swing phase, single support phase, double support phase and three-point support phase, based on the foot contact force threshold and joint pose calculation results, and output the corresponding phase identifier and the number of supporting feet. The kinematic center of mass prediction module has a built-in rigid body kinematic model with link elastic deformation compensation. It combines real-time joint angle data to calculate the kinematic prediction position of the whole machine's center of mass and outputs the deformation estimate of the center of mass position. The graded constraint observation module is used to match the corresponding level of centroid constraint observation equations based on the current phase identifier and the number of support feet. The module has a built-in observation validity verification unit, which determines whether the current observation value is valid based on the consistency of the overall torque balance, and removes invalid observation data under foot slippage and impact disturbance. The dual-level joint calibration module incorporates a position-level rolling filter unit and a parameter-level trigger update unit. The position-level rolling filter unit uses the centroid motion state obtained by quadratic integration of six-axis inertial data as the prediction term and the effective observation value output by the hierarchical constraint observation module as the measurement correction term, combining the deformation prediction value to complete the instantaneous position calibration of the centroid in each cycle. The parameter-level trigger update unit has a preset deviation threshold. When the continuous deviation between the kinematic predicted position and the calibrated centroid position exceeds the threshold, the rigid body load three-dimensional offset of the whole machine centroid, the elastic deformation compensation coefficient of the connecting rod, and the zero bias of the acceleration of the inertial measurement unit are used as the parameter set to be estimated. Parameter iterative identification is performed within the multi-support phase time window, and the model parameters of the kinematic centroid prediction module are updated synchronously. The data output module is used to output the calibrated real-time position of the centroid and the updated model parameters to the quadruped robot gait balance control system.
2. The real-time online calibration system for the center of mass position of a quadruped robot according to claim 1, characterized in that: The gait support grading module divides the phase states, determines the contact / leave state of a single foot one by one according to the contact force of each foot and the preset contact threshold, counts the number of feet in the contact state at the current moment, and verifies the geometric validity of the support polygon by combining the joint pose calculation results of each supporting foot. Finally, based on the number of effective supporting feet, it is divided into four phase states: swing phase, single support phase, double support phase and three-point support phase.
3. The real-time online calibration system for the center of mass position of a quadruped robot according to claim 1, characterized in that: The observation validity verification unit is as follows: based on the current kinematic model and trunk posture data, it calculates the theoretical torque vector of the whole machine weight relative to each supporting foot end point, performs difference calculation between the torque vector measured by the foot end three-dimensional force sensor and the theoretical torque vector to obtain the torque deviation amplitude. When the deviation amplitude exceeds the allowable range, the current observation value is determined to be invalid and discarded; if it does not exceed the allowable range, it is determined to be valid and output to the dual-level joint calibration module.
4. The real-time online calibration system for the center of mass position of a quadruped robot according to claim 1, characterized in that: The hierarchical constraint observation module matches the constraint observation equation as follows: it pre-configures the corresponding level of observation equation and initial weight coefficient for each of the four phase states. When a phase state switch is detected, it does not directly jump to the weight coefficient of the target phase, but uses a fixed proportion of the current gait cycle as a transition interval to linearly adjust the weight coefficient to the target value, thus completing the smooth switch of the observation equation.
5. The real-time online calibration system for the center of mass position of a quadruped robot according to claim 1, characterized in that: The position-level rolling filter unit includes: using a single complete gait cycle as the rolling window length, triggering a measurement correction update only once at the middle moment of the support phase within each window, and maintaining inertial integral recursion at other times. After each gait cycle is completed, the window slides backward by one cycle step, discarding the historical data of the earliest cycle and retaining the state data of the latest cycle for the next cycle recursion.
6. The real-time online calibration system for the center of mass position of a quadruped robot according to claim 1, characterized in that: The triggering determination process of the parameter-level triggering update unit is as follows: calculate the average deviation between the kinematic prediction position and the calibrated centroid position in each gait cycle in real time, accumulate and statistically analyze the average deviation for multiple consecutive gait cycles, and trigger the parameter iteration identification process when the average deviation for multiple consecutive cycles exceeds the preset deviation threshold. If the condition of continuous exceeding the threshold is not met, keep the current model parameters unchanged.
7. The real-time online calibration system for the center of mass position of a quadruped robot according to claim 1, characterized in that: The kinematic center of mass prediction module calculates the deformation estimate as follows: based on the output torque of each joint and the link structure parameters, the bending load borne by each link is inferred, and combined with the stiffness characteristics of the link material, the elastic deformation of each link under the load is calculated. The deformation of each link is substituted into the rigid body kinematic model for pose correction, and finally the kinematic prediction position of the center of mass with deformation compensation and the deformation estimate are obtained.
8. The real-time online calibration system for the center of mass position of a quadruped robot according to claim 1, characterized in that: The multi-source sensing synchronous acquisition module completes timestamp alignment. Using the sampling clock of the torso inertial measurement unit as the reference time axis, it resamples the joint encoder data and foot force sensor data with lower sampling rates to the same sampling frequency as the inertial data using interpolation. It assigns a timestamp consistent with the reference clock to each group of resampled data, thus completing the time synchronization of multi-source data.
9. A real-time online calibration system for the center of mass position of a quadruped robot according to claim 1, characterized in that: The parameter-level trigger update unit performs parameter iteration identification as follows: After triggering the identification process, multiple consecutive gait cycles containing double support phases and three-point support phases are selected as identification windows. Within the window, effective constraint observation data and kinematic prediction data are accumulated cycle by cycle. The set of parameters to be estimated is iteratively calculated using a recursive solution method. After the identification window ends, the solved parameters are updated to the kinematic centroid prediction module, and the deviation statistics count is reset.
10. A real-time online calibration system for the center of mass position of a quadruped robot according to claim 1, characterized in that: Before outputting the data, the real-time position of the calibrated centroid is smoothed by low-pass filtering. When the model parameters are detected to be updated, the new parameters are gradually substituted into the kinematic prediction calculation over several subsequent gait cycles to reduce the jump in output position caused by parameter mutations. The smoothed centroid position and the updated model parameters are then output.
