A multi-source heterogeneous sensor data hierarchical preprocessing method for quadruped robots

Abstract

The application relates to the technical field of robot perception and data processing, and discloses a multi-source heterogeneous sensor data hierarchical preprocessing method for a quadruped robot. The original outputs of multi-source sensors such as an inertial measurement unit, a six-dimensional acceleration sensor and a three-dimensional acceleration sensor of a hip joint are uniformly converted into international system of units physical quantities, and are sequentially subjected to deterministic error correction, real-time temperature compensation, spatial coordinate alignment and lever arm effect compensation. Rotational correlation and leg relative motion components are stripped from each hip joint acceleration measurement value based on a rigid body kinematics equation, and multi-path equivalent mass center line acceleration is extracted. The equivalent mass center line acceleration is subjected to low-pass filtering, and impact adaptive suppression processing is performed on a shooting impact window according to a firing synchronization signal, so that data is packaged into a standard observation frame and output. Standardized observation data with a reference and low noise is provided for attitude estimation of the quadruped robot under high dynamic and strong impact working conditions.

Description

Technical Field

[0001] This invention relates to the field of robot perception and data processing technology, and in particular to a hierarchical preprocessing method for multi-source heterogeneous sensor data of quadruped robots. Background Technology

[0002] Special-purpose quadruped robots typically carry weapon payloads and must perform highly maneuverable movement and firing while on the move in complex terrain. To achieve stable control and dynamic balance during firing, the robot needs to accurately perceive its own posture and motion state in real time. Current posture perception systems mostly rely on onboard inertial measurement units (IMUs) and joint encoders, estimating posture through multi-sensor fusion algorithms. However, the perception system of special-purpose quadruped robots differs significantly from that of ordinary legged robots: in addition to traditional IMUs and joint sensors, multi-dimensional accelerometers are required to capture the center of mass motion information under highly dynamic conditions, and a firing synchronization perception module is also needed to cope with the special interference of firing impact. Faced with such diverse sensor resources, how to efficiently integrate sensor data with different physical principles, different output formats, and different installation locations into reliable observation information has become a key issue restricting the accuracy of posture estimation.

[0003] In practical engineering applications, data preprocessing of multi-source heterogeneous sensors faces numerous prominent technical challenges. First, robots are equipped with a wide variety of sensors, with output dimensions encompassing voltage, digital codes, pulses, and other forms. All of these sensors exhibit deterministic systematic errors such as zero-point bias, sensitivity error, and inter-axis non-orthogonality, which would introduce significant measurement deviations if used directly. Second, the drastic temperature variations in special operating environments cause sensor zero bias and sensitivity to drift with temperature. Fixed parameters calibrated at room temperature become ineffective over a wide temperature range, leading to gradual divergence in attitude estimation during prolonged operation. Third, sensors are dispersed across different locations such as the body and hip joints, each with its own independent sensitive axis coordinate system, lacking a unified spatial reference. Furthermore, the accelerometer at the hip joint, while sensing body motion, also couples in the relative motion of the legs and the angular motion introduced by non-center-of-mass mounting, severely interfering with the accurate extraction of center-of-mass motion. In addition, the broadband, high-amplitude impact vibration generated at the moment of load release, combined with the impact of the foot hitting the ground, severely distorts the acceleration measurement signal. Conventional filtering methods struggle to effectively suppress impact interference while preserving accurate motion information.

[0004] Existing preprocessing techniques are mostly designed for single-type sensors or general scenarios, and there is no systematic preprocessing solution specifically tailored to the sensor layout characteristics, motion coupling features, and high-impact conditions of quadruped robots in special operations. Due to the lack of a unified hierarchical processing architecture, the observation data received by the fusion estimator still suffers from problems such as dimensional differences, error drift, coordinate deviation, motion coupling, and impact noise, which restrict the accuracy and robustness of attitude perception. Therefore, a hierarchical preprocessing method is needed to systematically address the above-mentioned problems of multi-source heterogeneous data, providing standard observation data with the same reference and low noise for subsequent fusion estimation. Summary of the Invention

[0005] The purpose of this invention is to provide a hierarchical preprocessing method for multi-source heterogeneous sensor data for quadruped robots, which solves the problems in the prior art where multi-source heterogeneous data is difficult to directly fuse due to inconsistent sensor dimensions, system error coupling, temperature drift, spatial coordinate dispersion, motion entanglement, and shooting impact contamination, thus affecting the accuracy and robustness of attitude estimation.

[0006] To achieve the above objectives, this invention provides a hierarchical preprocessing method for multi-source heterogeneous sensor data of quadruped robots, comprising the following steps: Acquire multi-source heterogeneous sensing data, which includes raw measurement data output from an inertial measurement unit, a six-dimensional accelerometer, a hip joint three-dimensional accelerometer, a joint encoder, and a torque sensor, as well as a firing synchronization signal output from a firing synchronization signal sensing module; convert the multi-source heterogeneous sensing data into SI physical quantities. The SI physical quantities are subjected to deterministic error correction, which includes sensitivity scaling factor compensation, inter-axis non-orthogonal compensation, constant zero bias compensation, and linear acceleration cross-coupling compensation of the angular acceleration channel. The real-time temperature values ​​of the corresponding sensors are obtained through the built-in temperature measurement units of the inertial measurement unit, the six-dimensional accelerometer, and the hip joint three-dimensional accelerometer, respectively. For each sensor, the zero-bias parameter and sensitivity parameter are corrected online based on the temperature-zero bias curve and temperature-sensitivity curve pre-calibrated by the sensor. The calibrated sensor measurements are transformed to a unified body coordinate system through a pre-calibrated spatial rotation matrix, and lever arm effect compensation is performed on sensors mounted near the center of mass. By utilizing the robot's forward kinematics model and rigid body kinematics relationship, centripetal acceleration, tangential acceleration, and leg relative motion-induced acceleration caused by non-center-of-mass mounting are removed from the measurements of the three-dimensional acceleration sensors of each hip joint, and the multi-path equivalent body center-of-mass line acceleration is extracted. The equivalent centroid acceleration sequence is low-pass filtered, and the firing synchronization signal output by the firing synchronization signal sensing module is used to mark the firing impact window. Impact interference suppression processing is performed within the firing impact window. The processed data is packaged into standard observation frames at a uniform frequency and sent to the fusion estimation module.

[0007] In the deterministic error correction of the measured physical quantities, the accelerometer error correction model for the inertial measurement unit and the six-dimensional accelerometer sensor is as follows: ; in, The corrected acceleration vector, This is a diagonal matrix of sensitivity scaling factors. It is an inter-axis non-orthogonal compensation matrix. The vector is a constant zero bias; the gyroscope is corrected using an isomorphic error model. This is the sensor's original output vector.

[0008] The angular acceleration channel line acceleration cross-coupling compensation model is as follows: ; in, This is the corrected angular acceleration vector; This is the original output vector of the angular acceleration channel; This is the cross-coupling matrix of linear acceleration and diagonal acceleration. This is the corrected linear acceleration vector. This is the constant zero-bias vector for the angular acceleration channel.

[0009] The temperature-zero bias curve adopts the following polynomial model: ; in, is the temperature-dependent zero-bias vector; T is the current real-time temperature of the sensor; T0 is the reference temperature; b0 is the zero-bias vector at the reference temperature; This is a first-order temperature coefficient vector, representing the rate of change of zero bias with linear temperature variation; This is a second-order temperature coefficient vector, representing the rate of change of zero bias with respect to a quadratic nonlinear temperature change; The temperature-sensitivity curve is modeled using the following polynomial model: ;

[0010] in, This is the temperature-dependent sensitivity coefficient; T0 is the real-time temperature of the sensor; T0 is the reference temperature; k0 is the sensitivity coefficient at the reference temperature. The first-order sensitivity temperature coefficient represents the relative rate of change of sensitivity as a linear change with temperature. The second-order sensitivity temperature coefficient represents the relative rate of change of sensitivity with the second nonlinear variation of temperature.

[0011] Specifically, lever arm effect compensation for sensors mounted near the center of mass includes: The sensor mounted near the center of mass includes a six-dimensional accelerometer and an inertial measurement unit, and the lever arm effect compensation is performed according to the following formula: ; in, The equivalent acceleration vector of the center of mass after lever arm compensation; The acceleration vector at the sensor's sensitive point. The lever arm vector pointing from the sensor's sensing center to the machine's center of mass. The body angular acceleration provided by the six-dimensional accelerometer The angular velocity of the machine body provided to the inertial measurement unit.

[0012] Among them, the extraction of multiple equivalent body center-of-mass accelerations is based on the rigid body kinematic equations: ; in, Let be the acceleration vector output by the i-th hip sensor; The true linear acceleration of the body's center of mass; This is the vector of the body's angular acceleration. The angular velocity vector of the aircraft; The position vector of the center of rotation of the hip joint; This is the vector of the entrainment acceleration generated at the hip joint point by the relative motion of the legs; Measure noise for the sensor; By inverse solving and ignoring noise terms, the equivalent centroidal acceleration derived from the i-th hip joint three-dimensional accelerometer is obtained: ; in, It is the equivalent center of mass acceleration vector; Let be the measurement value of the i-th hip joint three-dimensional accelerometer in the body coordinate system. Let be the position vector of the i-th hip joint rotation center in the body coordinate system. This is the drag acceleration vector generated by the relative motion of the legs at the hip joint. To measure noise for the sensor, Measurements taken from a six-dimensional accelerometer. Measurements were taken from the inertial measurement unit.

[0013] The relative motion acceleration vector of the legs in the rigid body kinematic equations is calculated using the following formula: ; in, This is the vector of the entrainment acceleration generated at the hip joint point by the relative motion of the legs; Let be the angle, angular velocity, and angular acceleration of the i-th hip joint, respectively. The unit vector for the hip joint rotation axis. The direction vector of the thigh rod is given, and the angle of the hip joint is measured in real time by the joint encoder. The angular velocity of the hip joint is obtained by performing first-order numerical difference on the hip joint angle sequence continuously collected by the joint encoder and then passing it through low-pass filtering. The angular acceleration of the hip joint is obtained by performing first-order numerical difference on the filtered hip joint angular velocity sequence, or by estimating it based on the hip joint torque measured by the torque sensor combined with the robot dynamics model.

[0014] Specifically, the impact interference suppression process performed within the firing impact window includes: The firing impact window is time-stamped by the rising edge of the firing synchronization signal. Mark the window length, which is 20ms to 50ms. Within the aforementioned shooting impact window, a 5-point median filter is first applied to suppress transient glitches, assuming that impact response templates for each sensor are pre-calibrated. Then, the signal in the window is subtracted from the impact response template to remove the recoil impact component. The calibration method of the impact response template is as follows: under standard firing conditions, the rising edge of the firing synchronization signal is used as the trigger reference, the impact response waveforms of each sensor channel are synchronously acquired and recorded, and the impact response waveforms recorded multiple times are time-domain aligned and averaged to obtain the impact response template. Otherwise, mark the acceleration data frames within the window as impact contamination states and set the corresponding noise covariance pre-expansion factor to 100 times.

[0015] The standard observation frame includes the angular velocity and acceleration of the inertial measurement unit in the body coordinate system, the centroidal acceleration and angular acceleration of the six-dimensional accelerometer, the equivalent centroidal acceleration of the four-channel hip joint three-dimensional accelerometer, the angle / angular velocity / torque of each joint, the firing impact flag, and the most recent firing timestamp.

[0016] The low-pass filtering of the equivalent centroid acceleration sequence specifically includes: A fourth-order Butterworth low-pass filter with a cutoff frequency of 80Hz is used to suppress noise spikes caused by high-frequency vibrations and foot impacts.

[0017] This invention discloses a hierarchical preprocessing method for multi-source heterogeneous sensor data of quadruped robots. First, the raw outputs of various sensors, including the inertial measurement unit, six-dimensional accelerometer, hip joint three-dimensional accelerometer, joint sensor, and firing synchronization signal sensing module, are uniformly converted into SI physical quantities. Then, deterministic error correction is sequentially performed to compensate for sensitivity scaling factor deviation, inter-axis non-orthogonality error, constant zero bias, and linear acceleration cross-coupling of the angular acceleration channel. Thermal drift is corrected online using the built-in temperature values ​​of each sensor based on pre-calibrated temperature characteristic curves. Next, the corrected sensor measurements are transformed to a unified body coordinate system using a spatial rotation matrix, and lever arm effect compensation is performed on the near-center-of-mass sensor. Then, based on rigid body kinematics equations and the robot's forward kinematics model, rotational entrainment components and leg relative motion components are extracted from the hip joint acceleration measurements to extract multi-path equivalent center-of-mass linear acceleration. Finally, the equivalent center-of-mass acceleration is low-pass filtered, and impact adaptive suppression processing is performed based on the firing synchronization signal marking the firing impact window. The processed data is then packaged into standard observation frames for output. This invention systematically solves technical problems such as dimension inconsistency among multi-source heterogeneous sensors, measurement deviation caused by system errors and temperature drift, lack of unified benchmark for spatial coordinate dispersion, centroid extraction due to motion coupling interference of hip sensors, and contamination of observation data by shooting impact through hierarchical pipeline processing. It provides standardized observation data with the same benchmark, low noise, and high reliability for attitude estimation and motion control of quadruped robots under high dynamic and strong impact conditions. Attached Figure Description

[0018] To more clearly illustrate the technical solutions in the embodiments of this application or the prior art, the accompanying drawings used in the description of the embodiments or the prior art will be briefly introduced below.

[0019] Figure 1 This is a flowchart of the multi-source heterogeneous sensor data hierarchical preprocessing method for quadruped robots according to the present invention.

[0020] Figure 2 This is a schematic diagram of the sensor layout and coordinate system definition of the present invention.

[0021] Figure 3 This is a schematic diagram illustrating the principle of rigid body kinematics compensation and equivalent center of mass extraction of the present invention.

[0022] Figure 4 This is a comparison diagram of the firing impact window and processing effect of the present invention.

[0023] Figure 5 This is a flowchart of the steps of the hierarchical preprocessing method for multi-source heterogeneous sensor data for quadruped robots according to the present invention. Detailed Implementation

[0024] The embodiments of the present invention are described in detail below. Examples of the embodiments are shown in the accompanying drawings. The embodiments described below with reference to the accompanying drawings are exemplary and intended to explain the present invention, but should not be construed as limiting the present invention.

[0025] Please refer to Figures 1 to 5 , Figure 1 This is a flowchart of the multi-source heterogeneous sensor data hierarchical preprocessing method for quadruped robots according to the present invention.

[0026] Figure 2 This is a schematic diagram of the sensor layout and coordinate system definition of the present invention. In the diagram, 1-4 represent three-dimensional hip joint accelerometers respectively mounted on the hip joint housings of the four legs; 5 represents a six-dimensional accelerometer; 6 represents an inertial measurement unit; 7 represents a firing synchronization signal sensing module; 8 represents a load platform; 9 represents a firing mechanism; 10 represents the quadruped robot body; 11 represents a mounting bracket; 12 represents a hip joint; 13 represents a knee joint; and 14 represents an ankle joint.

[0027] Figure 3 This is a schematic diagram illustrating the principle of rigid body kinematics compensation and equivalent center of mass extraction of the present invention.

[0028] Figure 4 This is a comparison diagram of the firing impact window and processing effect of the present invention.

[0029] Figure 5 This is a flowchart of the steps of the hierarchical preprocessing method for multi-source heterogeneous sensor data for quadruped robots according to the present invention.

[0030] This invention provides a hierarchical preprocessing method for multi-source heterogeneous sensor data of quadruped robots, comprising the following steps: S101: Acquire multi-source heterogeneous sensing data, which includes raw measurement data output by the inertial measurement unit, the six-dimensional accelerometer, the hip joint three-dimensional accelerometer, the joint encoder, and the torque sensor, as well as the firing synchronization signal output by the firing synchronization signal sensing module; and convert the multi-source heterogeneous sensing data into SI physical quantities. S102: Perform deterministic error correction on the measured value of the physical quantity. The deterministic error correction includes sensitivity scaling factor compensation, inter-axis non-orthogonal compensation, constant zero bias compensation, and linear acceleration cross-coupling compensation of the angular acceleration channel. S103: The real-time temperature values ​​of the corresponding sensors are obtained through the built-in temperature measurement units of the inertial measurement unit, the six-dimensional accelerometer, and the hip joint three-dimensional accelerometer, respectively; for each sensor, the zero-bias parameter and sensitivity parameter are corrected online based on the temperature-zero bias curve and temperature-sensitivity curve pre-calibrated by the sensor. S104: Transform the calibrated sensor measurements to a unified body coordinate system using a pre-calibrated spatial rotation matrix, and compensate for lever arm effect for sensors mounted near the center of mass. S105: Using the robot's forward kinematics model and rigid body kinematics relationship, the centripetal acceleration, tangential acceleration and leg relative motion-induced acceleration caused by non-center-of-mass mounting are removed from the measurements of the three-dimensional acceleration sensors of each hip joint, and the multi-path equivalent body center-of-mass line acceleration is extracted. S106: Perform low-pass filtering on the equivalent centroid line acceleration sequence, and use the firing synchronization signal output by the firing synchronization signal sensing module to mark the firing impact window, and perform impact interference suppression processing within the firing impact window; S107: Pack the processed data into standard observation frames at a uniform frequency and send them to the fusion estimation module.

[0031] Specifically, using a quadruped robot with a payload as a platform, a six-dimensional accelerometer (including three-axis acceleration and three-axis angular acceleration) is installed at the robot's center of mass, and a three-dimensional accelerometer is installed at the hip joint shell of each of the four legs. Simultaneously, the robot's built-in inertial measurement unit, joint encoders, and torque sensors are utilized. The joint encoders are installed at the rotation axes of each joint to measure the angle of the corresponding joint. The torque sensors are installed at the joint drive ends to measure the joint torque of the corresponding joint. The joint angular velocity is obtained by performing first-order numerical difference and low-pass filtering on the joint angle sequence continuously collected by the joint encoders. The joint angular acceleration is obtained by performing first-order numerical difference on the filtered joint angular velocity sequence, or by estimating the joint torque measured by the torque sensors combined with the robot's dynamics model. A firing synchronization signal sensing module is installed on the payload. The body coordinate system {B} is defined: the origin is located at the robot's center of mass, the X-axis points forward along the longitudinal axis of the body, the Y-axis points to the left along the transverse axis of the body, and the Z-axis is perpendicular to the XY plane and points upward, forming a right-handed rectangular coordinate system. The body coordinate system is rigidly connected to the robot and moves and rotates with the robot, serving as a unified reference for describing the robot's own motion and attitude. The world coordinate system {W} is the local horizontal coordinate system. For example... Figure 1 As shown, this method is performed according to the following steps: Dimensional unification and unit standardization of physical quantities: To convert the raw electrical quantities (voltage values ​​or analog-to-digital converter (ADC) digital code values) output by each sensor into SI physical quantities, it is necessary to obtain the zero bias voltage of each sensor channel in advance. And the sensitivity coefficient S. These two parameters are accurately measured using the following offline calibration method and pre-written into the system configuration file or the sensor's built-in memory for real-time access in this step.

[0032] (a) Zero bias voltage Calibration: For the accelerometer channel, the sensor is mounted on a precision horizontal vibration isolation platform, and a high-precision indexing head is used to align the sensing axis sequentially to six orthogonal directions in space. The output voltage is collected for at least 60 seconds in each direction, and the average value of the output voltage in all directions is taken as the zero bias voltage of that channel. Alternatively, the symmetrical position method can be used: measure the output voltage with the sensitive axis pointing vertically upwards. Then measure vertically downwards. Then the zero bias voltage is: (1) For the gyroscope channel, the sensor is fixed on a precision rate turntable, keeping the turntable absolutely stationary. Multiple segments of stationary data are collected, and the average output voltage is used as the zero bias voltage. All of the above processes were completed at a standard temperature (25°C).

[0033] (b) Calibration of the sensitivity coefficient S: The accelerometer's sensitivity coefficient was obtained using the gravitational field tumbling calibration method. The sensor was placed on the indexing head, and the angle between the sensitive axis and the direction of gravity was adjusted. The voltage is varied in increments (e.g., 10°) within the range of 0° to 360°, and the steady-state output voltage at each position is recorded. Based on the accelerometer output model: (2) The measurement sequence was sinusoidally fitted using the least squares method, and the sensitivity coefficient was calculated simultaneously. (unit: or ). Gyroscope sensitivity coefficient The calibration is then performed using the rate turntable method: the turntable is operated at different constant angular velocities. Rotate and record the steady-state output voltage. Based on the linear model: (3) Obtained through linear regression (unit: or ).

[0034] (c) Parameter solidification and online conversion: The zero bias voltage obtained during calibration The sensitivity coefficient S is permanently written into the system configuration file or stored on the sensor chip. During real-time operation, the main control acquisition unit obtains the sensor's raw output voltage or the equivalent voltage value converted by the ADC. The dimensions are unified according to the following formula: Acceleration physical value: (4) in, This is the current channel's output voltage. This is the zero-bias voltage for acceleration of this channel. This is the acceleration sensitivity coefficient.

[0035] Angular velocity physical value: (5) in This is the zero bias voltage of the gyroscope. This represents the sensitivity coefficient of the gyroscope.

[0036] For IMU sensors that directly output digital code values, the above conversion is equivalent to linear mapping using the scaling factor and zero-bias code value provided in the datasheet. Joint angle values ​​are uniformly converted from the encoder's original pulse count or degree to radians (rad), and joint torques are uniformly converted to Newton-meters (N·m).

[0037] At this point, all sensor data are expressed in SI units, but remain in their original sensing axis coordinate systems, providing standardized input for subsequent system error correction and coordinate alignment.

[0038] Sensor system error correction: Due to factors such as manufacturing processes, packaging stress, and circuit characteristics, each accelerometer, gyroscope, and angular accelerometer has deterministic systematic errors at the time of manufacture. These mainly include constant zero bias, sensitivity scaling factor deviation, inter-axis non-orthogonality error, and the cross-coupling error between the linear acceleration and angular acceleration channels unique to six-dimensional sensors. Using error parameters obtained from offline precision calibration, frame-by-frame deterministic error compensation is performed on each sensor channel to eliminate the aforementioned systematic biases.

[0039] (a) Accelerometer error correction model: For the three-axis acceleration channel and the four hip joint three-dimensional acceleration sensors in the six-dimensional accelerometer, the following error correction model is uniformly adopted: (6) The meanings of each parameter and their acquisition methods are as follows: : The sensor's original output vector (converted to physical units m / s², but uncorrected), with the superscript S indicating that it is in the sensor's own sensitive axis coordinate system.

[0040] The constant zero-bias vector, measured in m / s², represents the equivalent acceleration bias of the sensor output under conditions of no input excitation (zero acceleration). It originates from static factors such as internal circuit misalignment and mechanical stress. Acquisition method: At standard temperature (25℃), the sensor is placed on a precision vibration isolation platform. The zero-bias voltage is measured using the symmetrical position method or the six-position averaging method, and then the components are calculated from the sensitivity coefficient. The output voltage is collected vertically upwards and downwards along the sensitive axis, and the average value is taken as the zero-bias voltage for that axis. This value is then converted into an acceleration value. .

[0041] : Sensitivity scaling factor diagonal matrix. Ideally, it is an identity matrix; in practice, the sensitivity of each axis deviates slightly from the nominal value. These represent the ratio of the true sensitivity to the nominal sensitivity for each axis. Acquisition method: Using gravity field tumbling calibration, align the sensor's sensitive axis with the opposite direction of gravity, measure the output change, and calculate the scaling factor. x-axis scaling factor. The y and z axes are operated in the same way as the x axis.

[0042] The inter-axis non-orthogonal compensation matrix is ​​a 3x3 matrix that describes the error caused by the non-perpendicularity of three sensitive axes due to manufacturing and packaging. Its elements are small angles. (in radians), representing the non-orthogonal angle coupled from the j-axis to the i-axis: (7) Table 1 provides the symbols, positions, and meanings of the non-orthogonal angle parameters in the interaxial non-orthogonal compensation matrix: Table 1: Meaning of Parameters in Interaxial Non-Orthogonal Compensation Matrix

[0043] The triaxial acceleration output under different attitudes was recorded by 12-position precision indexing head roll calibration. With the local gravity vector as a reference, all non-orthogonal angle parameters were solved by nonlinear least squares optimization.

[0044] (b) IMU error correction model For the error model of triaxial accelerometer and three-dimensional accelerometer isomorphic in airborne IMU: (8) The meanings and acquisition methods of the parameters are analogous to those of an accelerometer, as detailed below: This is the corrected angular velocity vector; The raw output vector of the gyroscope (converted to physical units rad / s).

[0045] : Gyroscope constant zero bias vector, in rad / s. Represents the static bias output when there is no angular velocity input. Multiple data points are collected on a precision rate turntable while the turntable is kept absolutely stationary, and the average of the angular velocity outputs is taken as the zero bias.

[0046] This is a diagonal matrix representing the gyroscope sensitivity scaling factor, where each element is the ratio of the actual sensitivity to the nominal sensitivity for each axis. The scaling factor is determined by a rate turntable operating at multiple constant angular velocities. The output voltage is collected, and the sensitivity coefficient is obtained by linear regression. The proportional factor is obtained by comparing it with the nominal value.

[0047] : Gyroscope inter-axis non-orthogonal compensation matrix, structure and Similarly, non-orthogonal angles are obtained through joint calibration using a multi-axis rate turntable. The turntable rotates around each axis sequentially according to a preset rotation order, collects gyroscope outputs, and identifies non-orthogonal angles using angular velocity vector transformation relationships.

[0048] (c) Error correction model for six-dimensional sensor angular acceleration channel In addition to the linear error mentioned above, the three-axis angular acceleration channel in the six-dimensional sensor also needs to compensate for the cross-coupling effect of linear acceleration on angular acceleration measurement. The correction model is as follows: (9) in: This is the corrected angular acceleration vector; : Raw output vector of angular acceleration channel (converted to physical units rad / s²).

[0049] The linear acceleration-diagonal acceleration cross-coupling matrix is ​​a 3x3 matrix with elements... This represents the parasitic coupling coefficient between the j-axis acceleration and the i-axis angular acceleration channel. Ideally, it is a zero matrix, but due to the non-ideal decoupling of the six-dimensional sensor's elastic structure, linear acceleration can cause spurious outputs in the angular acceleration channel. On a precision linear vibration table, sinusoidal vibrations of known amplitude and frequency are applied along the x, y, and z directions, respectively, while simultaneously recording the outputs of the three angular acceleration channels. Since there is no real angular motion, the output of the angular acceleration channel is the linear acceleration coupling response. All coupling coefficients are identified using frequency domain transfer functions or least-squares fitting.

[0050] Angular acceleration channel constant zero bias vector, unit: rad / s². Represents the static bias when there is no angular motion input. The six-dimensional sensor is mounted on a vibration isolation platform, kept absolutely stationary, and the angular acceleration channel output is collected for at least 120 seconds. The time average is taken, and the known linear acceleration cross-coupling component is subtracted to obtain the value. .

[0051] After the above corrections, the calibrated measured values ​​under the respective sensor sensing axis systems are obtained. and The deterministic systematic error is compensated, but it is still in the original sensitive coordinate system and needs to be sent to subsequent steps for temperature compensation and spatial coordinate alignment.

[0052] Real-time temperature compensation: The working environment of the quadruped robot and its payload experiences drastic temperature changes (e.g., moving from a cold outdoor environment to a hot indoor environment, or prolonged high-intensity operation causing self-heating of components). The mechanical elastic modulus, circuit bias, and scaling factor within the sensors all drift significantly with temperature, causing the fixed error parameters calibrated at room temperature in the aforementioned sensor system error correction to fail over a wide temperature range. To eliminate the impact of temperature changes on measurement accuracy, this step acquires the temperature values ​​of the corresponding sensors in real time through the built-in temperature measurement units of the inertial measurement unit, the six-dimensional accelerometer, and each hip joint three-dimensional accelerometer. By using temperature-zero bias curves and temperature-sensitivity curves pre-calibrated at multiple temperature points, the zero bias vector and sensitivity scaling factor matrix of the corresponding sensors are corrected online, achieving frame-by-frame thermal drift compensation.

[0053] (a) Temperature-zero bias model: For any accelerometer channel, the zero bias at temperature T Described using a quadratic polynomial model: (10) The meanings of each parameter and the testing methods are as follows: Temperature-dependent zero bias vector (m / s² or rad / s). It is the actual zero bias value of the sensor at the current temperature T, calculated using the temperature drift model. Physically, it represents the equivalent physical quantity that the sensor should output at the current temperature without external input excitation.

[0054] T: The sensor's current real-time temperature, measured in real time by a digital temperature sensor integrated inside the sensor, and returned via a digital interface at a frequency of no less than 100Hz, in degrees Celsius (°C).

[0055] T 0： The reference temperature, usually taken as the standard room temperature of the calibration environment, is 25°C, and is used as the base point for the polynomial expansion. The unit is °C.

[0056] The zero bias vector at the reference temperature is the constant zero bias obtained during calibration at 25°C in the aforementioned sensor system error correction. or The unit is m / s² or rad / s.

[0057] : First-order temperature coefficient vector, representing the rate of change of zero bias with linear temperature change, in units of (m / s²) / ℃ or (rad / s) / ℃.

[0058] : Second-order temperature coefficient vector, representing the rate of change of zero bias with the second nonlinear change of temperature, with units of (m / s²) / ℃² or (rad / s) / ℃².

[0059] The sensor was placed in a precision temperature-controlled chamber with temperature stability and uniformity better than ±0.5℃. A series of temperature points were set (-20℃, -10℃, 0℃, 10℃, 25℃, 40℃, 55℃). After holding the sensor at each temperature point for at least 30 minutes to allow it to reach thermal equilibrium, the zero-bias value at that temperature was obtained according to the zero-bias calibration method in the aforementioned physical dimension unification and unit standardization (multi-position averaging method for accelerometers, static data averaging method for gyroscopes). After obtaining the discrete temperature point sequence and its corresponding zero-bias sequence, least squares polynomial fitting is used to solve for the values ​​of each channel. and Fitting time In accordance with the constant zero bias in the aforementioned sensor system error correction, the reference temperature The temperature is fixed at 25℃.

[0060] (b) Temperature-sensitivity model: The change in sensitivity coefficient with temperature is also modeled using a quadratic polynomial: (11) This model applies independently to the sensitivity scaling factor for each sensitive axis, i.e., a diagonal matrix. The meanings of each parameter and the testing methods are as follows: Temperature-dependent sensitivity coefficient (dimensionless or dimensionally matched to the corresponding physical quantity). It is determined by the current temperature. The ratio of the actual sensitivity to the nominal sensitivity of a certain sensitive axis at a given temperature, calculated using the temperature-sensitivity model. This value is then substituted into the sensitivity scaling factor diagonal matrix. In this process, temperature-adaptive compensation for sensitivity is achieved.

[0061] The sensitivity coefficient at the reference temperature, i.e., the sensitivity scaling factor obtained by calibration at 25°C in step "Sensor System Error Correction". Dimensionless (or may contain physical units).

[0062] The first-order sensitivity temperature coefficient represents the relative rate of change of sensitivity as a linear relationship with temperature, and its unit is °C. -1 .

[0063] The second-order sensitivity temperature coefficient represents the relative rate of change of sensitivity with respect to a quadratic nonlinear change in temperature, and its unit is °C. -2 .

[0064] Under the same temperature sequence of the temperature control chamber as the zero-bias temperature drift calibration, after sufficient heat preservation at each temperature point, the sensitivity coefficients of each axis at that temperature are obtained according to the sensitivity calibration method in the aforementioned physical dimension unification and unit standardization (accelerometers use gravitational field sine fitting, gyroscopes use rate turntable linear regression). The ratio of sensitivity at each temperature point to the reference temperature of 25℃ was calculated to obtain the temperature-sensitivity scaling factor sequence. A quadratic polynomial fitting was then performed on this sequence using the least squares method to identify the values ​​of each axis. , and .

[0065] (c) Online temperature compensation execution: During real-time system operation, each frame of data simultaneously carries the sensor temperature T. For each channel of the accelerometer and gyroscope, the fixed constant zero bias is used according to the error correction model described in the aforementioned sensor system error correction section. (or Replace with temperature-related The sensitivity scaling factor matrix (or Replace with temperature-related The original measurements are corrected frame by frame online. Temperature adaptive correction model: Accelerometer online temperature compensation: (12) Online temperature compensation for gyroscopes: (13) This is the temperature-compensated acceleration vector (unit: m / s²), still within the sensor's own sensing axis S. The subscript "corr,TC" indicates that deterministic error correction and temperature drift compensation have been completed simultaneously, and it is the direct input for subsequent spatial coordinate alignment.

[0066] This is the inter-axis non-orthogonality compensation matrix (dimensionless, 3×3 matrix), obtained through offline calibration and independent of temperature. Its function is to correct for the non-perfect orthogonality of the three axes caused by manufacturing errors.

[0067] The temperature-dependent sensitivity scaling factor is a diagonal matrix (dimensionless), in the form of: Each diagonal element The temperature-sensitivity model in equation (11) calculates in real time the ratio of the actual sensitivity of each sensitive axis to the nominal sensitivity at the current temperature T, which is used to compensate for the change in sensitivity with temperature.

[0068] The original output vector of the accelerometer (unit: m / s²) has been dimensionally unified, but has not yet undergone any error correction or temperature compensation.

[0069] The temperature-dependent zero bias vector (unit: m / s²) is derived from the temperature-zero bias model in equation (10). Real-time computing (written here as) (This is used to indicate that it is the zero bias of the accelerometer). It represents the equivalent acceleration value that the sensor should output under the current temperature T and no input excitation conditions, used to subtract the zero bias and its temperature drift.

[0070] The temperature-compensated angular velocity vector (unit: rad / s) remains within the sensor's own sensing axis S. Deterministic error correction and temperature drift compensation have also been completed.

[0071] The non-orthogonal compensation matrix between gyroscope axes (dimensionless, 3×3 matrix) is obtained from offline calibration and does not change with temperature.

[0072] This is a dimensionless diagonal matrix of temperature-dependent gyroscope sensitivity scaling factors, with diagonal elements... .

[0073] The temperature-sensitivity model of the gyroscope is used to calculate in real time to compensate for the drift of the gyroscope's sensitivity with temperature.

[0074] This is the original output vector of the gyroscope (unit: rad / s), and the dimensions have been standardized.

[0075] This is the temperature-dependent gyroscope zero-bias vector (unit: rad / s), calculated in real-time by the gyroscope's temperature-zero bias model. It represents the static bias value that the gyroscope should output at the current temperature T and with no angular velocity input.

[0076] The above online compensation output and Both deterministic systematic errors caused by the manufacturing process and gradual drift caused by ambient temperature have been eliminated, providing stable sensor data for subsequent spatial coordinate alignment.

[0077] Spatial coordinate alignment and near-centroid sensor arm compensation: The calibrated measurements, which remain in their respective sensitive axis coordinate systems after temperature compensation, are uniformly transformed to the body coordinate system {B} using a pre-calibrated rotation matrix. For sensors such as six-dimensional accelerometers and airborne IMUs that are installed close to the center of mass, if their sensitive center does not coincide with the body's center of mass, preliminary lever compensation is required to reduce the acceleration at the sensitive point to the equivalent value at the center of mass.

[0078] (a) Spatial coordinate rotation transformation: Each sensor obtains its overall rotation matrix from its own sensing axis S to the body coordinate system {B} through offline calibration. For the hip joint three-dimensional accelerometer, the cascaded rotation matrix from the sensor housing reference plane through the hip joint coordinate system to the {B} system is denoted as... Then the hip sensor measurement value is transformed as follows: (14) The original acceleration vector of the hip sensor after transformation to the body coordinate system (unit: m / s²).

[0079] This vector is the first The acceleration output by each hip sensor is expressed in the body coordinate system {B}. The superscript B indicates that the system has been transformed to the body coordinate system; the superscript "raw" indicates that lever arm compensation and kinematic compensation have not yet been performed, only coordinate rotation has been completed. The subscript... Indicates the hip joint. Leg part number (FL / FR / HL / HR).

[0080] For from the first Hip sensor sensitive axis The overall rotation matrix (3×3 unit orthogonal matrix, dimensionless) to the body coordinate system {B}. This matrix rotates the measurement vector in the hip sensor's own sensitive axis system to be expressed in the body coordinate system.

[0081] Composition and Acquisition Method: Since the hip sensor is first fixed to the hip joint housing and then connected to the machine body through the hip joint, this rotation matrix is ​​actually a cascaded transformation, containing the rotational relationships of the following two components: sensor sensing axis system To the hip joint shell reference coordinate system The rotation reflects the sensor's installation posture deviation on the housing; the hip joint housing reference coordinate system The rotation to the body coordinate system {B} is determined by the mounting geometry of the hip joint on the fuselage. Using an optical coordinate measuring machine or laser tracker, feature points are selected on the sensor and fuselage reference surfaces. The two-step rotation matrix is ​​measured and calculated, and finally merged into a single overall matrix. It is also embedded in the system configuration.

[0082] For the first The corrected acceleration vector (in m / s²) of each hip sensor in its own sensitive axis system.

[0083] The vector has undergone dimension unification (converted to m / s²), systematic error correction (subtraction of zero bias, sensitivity deviation, and non-orthogonality error), and real-time temperature compensation in sequence. (Superscript) This indicates that the vector is still in the sensor's own sensing axis coordinate system, and the three components correspond to the acceleration values ​​in the three sensing axis directions of the sensor.

[0084] The same applies to airborne IMUs and six-dimensional accelerometers: (15) This is the original acceleration vector of the IMU after transformation to the body coordinate system (unit: m / s²).

[0085] This represents the expression of the specific force measured by the airborne IMU in the body coordinate system {B}, without lever arm compensation.

[0086] This is the IMU angular velocity vector after transformation to the body coordinate system (unit: rad / s).

[0087] This represents the body angular velocity measured by the airborne IMU gyroscope in the body coordinate system {B}. This quantity will serve as the core input for subsequent kinematic compensation and state prediction.

[0088] For the IMU sensitive axis system Rotation matrix to body coordinate system {B} (3×3 identity orthogonal matrix, dimensionless).

[0089] The IMU is usually fixed inside the body by bolts or adhesive, and there is an installation deviation between its sensing axis and the axis of the body coordinate system {B}.

[0090] Acquisition method: After assembly, use a coordinate measuring machine to measure the attitude deviation of the IMU shell reference plane relative to the fuselage reference plane, and calculate the rotation matrix; alternatively, online calibration can be performed by using the projection relationship of the gravity vector between the IMU and the fuselage reference plane through multi-position static experiments.

[0091] This is the corrected acceleration vector of the IMU in its own sensitive axis system (unit: m / s²).

[0092] Dimensional unification, systematic error correction, and temperature compensation have been completed.

[0093] This is the corrected angular velocity vector of the IMU in its own sensitive axis system (unit: rad / s).

[0094] Dimensional unification, systematic error correction, and temperature compensation have been completed.

[0095] (16) The original linear acceleration vector of the six-dimensional sensor after transformation to the body coordinate system (unit: m / s²). This represents the output of the three-axis acceleration channels in the six-dimensional accelerometer in the body coordinate system {B}, before lever arm compensation (if the sensing center and the center of mass do not coincide).

[0096] The six-dimensional sensor angular acceleration vector (unit: rad / s²) after transformation to the body coordinate system.

[0097] This represents the output of the three-axis angular acceleration channel in the six-dimensional sensor, expressed in the body coordinate system {B}. This quantity will be used for calculating tangential entrainment acceleration in lever compensation and kinematic compensation.

[0098] For the sensing axis system of a six-dimensional sensor Rotation matrix to body coordinate system {B} (3×3 identity orthogonal matrix, dimensionless).

[0099] The six-dimensional sensor is mounted near the fuselage's center of mass on a rigid base, but there may still be slight deviations between its sensing axis and the fuselage coordinate system. Similar to an IMU, the relative attitude between the sensor's reference plane and the fuselage's reference plane is measured using a coordinate measuring machine or a laser tracker, and the rotation matrix is ​​calculated and written into the system configuration.

[0100] The calibrated linear acceleration vector (unit: m / s²) of the six-dimensional sensor in its own sensitive axis system.

[0101] Dimensional unification, systematic error correction (including inter-axis non-orthogonality and sensitivity compensation), and temperature compensation have been completed.

[0102] This is the corrected angular acceleration vector (unit: rad / s²) of the six-dimensional sensor in its own sensing axis system.

[0103] Dimensional unification, system error correction (including linear acceleration cross-coupling compensation and constant zero bias subtraction) and temperature compensation have been completed.

[0104] The above three rotation matrices All are 3×3 identity orthogonal matrices, satisfying ,and Its function is to rigidly rotate a three-dimensional vector from one coordinate system to another, keeping the vector's magnitude unchanged and only changing the projection components of its direction on each coordinate axis.

[0105] After completing this step, all inertial measurements (acceleration, angular velocity, angular acceleration) are uniformly expressed in the body coordinate system {B}, providing a unified spatial reference for subsequent lever compensation and kinematic decoupling.

[0106] (b) Near-center-of-mass sensor arm compensation: There is a known constant lever arm vector between the sensing center of the six-dimensional accelerometer and the airborne IMU and the center of mass C of the aircraft. This vector is expressed in the {B} frame. Due to the angular motion of the body, the lever arm will introduce additional components of centripetal and tangential acceleration. The angular velocity of the body is obtained using the aforementioned rigid body kinematics compensation and equivalent center of mass acceleration extraction. (or the filtered value) and the angular acceleration provided by the six-dimensional sensor. The acceleration at the sensitive point is reduced to the equivalent acceleration of the center of mass using the following formula: (17) Meaning and testing methods of lever arm compensation related parameters: Let $\frac{B}{B}$ be the equivalent acceleration vector of the center of mass after lever arm compensation (unit: m / s²), expressed in the body coordinate system $B$. This vector is the estimated value of the true acceleration of the body's center of mass after subtracting the lever arm effect from the measurements at the sensitive point. For a six-dimensional sensor, this value is denoted as $\frac{B}{B}$. For IMUs, this value is denoted as .

[0107] This is the original acceleration measurement value (unit: m / s²) at the sensor's sensitive point. It has undergone dimensional unification, systematic error correction, temperature compensation, and rotational transformation through preliminary steps and is expressed in the body coordinate system {B}, but still includes the angular motion coupling component caused by the lever arm. For a six-dimensional sensor, this is the linear acceleration after rotational transformation. For IMU, .

[0108] The body angular acceleration vector (unit: rad / s²) is taken from the angular acceleration channel of the six-dimensional accelerometer, and is the output value after step system error correction, temperature compensation, and rotational transformation to the {B} frame. This quantity describes the rate of change of the angular velocity of the body rotating about each axis and is a key input for calculating tangential entrainment acceleration.

[0109] The lever arm vector (unit: m) points from the sensor's sensing center to the robot's center of mass C (or vice versa, the sign and direction must be consistent with the formula), and is a constant three-dimensional vector in the robot's coordinate system {B}. It is obtained as follows: After mechanical assembly, using a 3D coordinate measuring machine or laser tracker, accurately measure the relative coordinate difference between the geometric center of the sensor's sensing element and the robot's center of mass marker point, and then write it into the system configuration after coordinate system transformation. If the sensor is precisely installed at the center of mass position, then... At this point, the lever arm compensation automatically degenerates into an identity transformation. .

[0110] The angular velocity vector (unit: rad / s) is taken from the gyroscope of the airborne IMU, and is the output value after system error correction, temperature compensation, and rotational transformation to the {B} frame. This quantity describes the instantaneous angular rate of the airframe's rotation about each axis.

[0111] This is the tangential entrainment acceleration term (unit: m / s²), the tangential acceleration at the end of the lever caused by the angular acceleration of the body. Its direction is perpendicular to the plane formed by the angular acceleration vector and the lever vector, and its magnitude is equal to the product of the angular acceleration amplitude and the lever length.

[0112] This is the centripetal acceleration term (unit: m / s²), the centripetal acceleration at the end of the lever arm caused by the angular velocity of the machine body. Its direction is perpendicular to the angular velocity vector and points towards the rotation axis, and its magnitude is equal to the square of the angular velocity amplitude multiplied by the projected length of the lever arm in the plane perpendicular to the rotation axis. This compensation is applicable to six-dimensional linear acceleration and IMU acceleration. After compensation, the six-dimensional sensor outputs the centroidal acceleration. IMU provides a comparison of the centroid. If the six-dimensional accelerometer is precisely installed at the center of mass (with the lever arm at zero), then this compensation step automatically degenerates into an identity transformation.

[0113] At this point, all acceleration measurements have been expressed in the body coordinate system {B}. The accelerations of the six-dimensional sensor and IMU have been reduced to the equivalent value of the center of mass. The acceleration of the hip sensor is still the acceleration of the hip joint (the relative motion of the leg and the distal lever arm effect have not yet been removed) and needs further processing.

[0114] Rigid body kinematic compensation and equivalent center of mass acceleration extraction: Four three-dimensional accelerometers are mounted at the hip joints of the legs, away from the center of mass, and their outputs are aligned accelerations. (i.e., the transformed) This includes not only the true linear acceleration of the body's center of mass, but also the acceleration caused by the angular motion of the body due to non-center-of-mass mounting and the acceleration component caused by the relative motion of the legs themselves. Based on the principles of rigid body kinematics and the robot's forward kinematics model, these coupling terms are precisely removed, and independent equivalent observations of the linear acceleration of the body's center of mass are extracted from each hip sensor, achieving decoupling and extraction of multi-path redundant center-of-mass motion information.

[0115] According to the theorem of acceleration composition at any point on a rigid body, the acceleration of hip joint point i in the body coordinate system {B} can be decomposed as follows: (18) In the formula, all vectors are expressed in the {B} system, and the meanings and methods for obtaining the parameters are as follows: : No. The acceleration vector (m / s²) output by each hip sensor.

[0116] The true linear acceleration of the body's center of mass, i.e., the final amount to be extracted, is expressed in m / s².

[0117] The body angular acceleration vector is directly taken from the angular acceleration measurement value of the six-dimensional sensor after calibration, temperature compensation, and rotation transformation. , in rad / s².

[0118] The body angular velocity vector is directly taken from the angular velocity measurement value of the IMU after calibration, temperature compensation, and rotational transformation. , in rad / s.

[0119] : The position vector of the hip joint rotation center, pointing from the body's center of mass C to the i-th hip joint rotation center, is a constant in the {B} system, and its unit is meters (m).

[0120] During the mechanical design phase, theoretical values ​​are obtained through 3D CAD models. After assembly, the precise coordinates of each hip rotation center relative to the centroid marker point are measured using a coordinate measuring machine or laser tracker. After coordinate transformation, the coordinates are written into the system configuration.

[0121] The drag acceleration vector generated by the relative motion of the leg at the hip joint is caused by the rotation of the thigh bar around the hip joint and includes tangential and centripetal components, measured in m / s². Its calculation formula is: (19) The sub-parameters are explained and the measurement methods are as follows: : Hip joint angle, angular velocity, and angular acceleration. The hip joint angle is measured in real time by the joint encoder; the hip joint angular velocity is obtained by performing first-order numerical difference on the hip joint angle sequence continuously collected by the joint encoder and then passing it through low-pass filtering; the hip joint angular acceleration is obtained by performing first-order numerical difference on the filtered hip joint angular velocity sequence, or by estimating it based on the hip joint torque measured by the torque sensor combined with the robot dynamics model.

[0122] The unit vector of the hip joint rotation axis, expressed in the {B} system, is determined by the robot's leg structure and is typically along the axis of rotation for simplified planar legs. Direction (horizontal axis). Determined through mechanical design drawings and assembly relationships, and verified during calibration using the zero position of an angle encoder and geometric relationships.

[0123] The vector from the center of mass of the thigh bar to the center of hip rotation, or the length direction vector of the thigh bar (simplified model), varies with the joint angle in the {B} system and can be calculated in real time by forward kinematics. Mechanical parameters are provided by the CAD model, and the length and initial direction are verified by assembly measurements.

[0124] Sensor measurement noise is considered as zero-mean white noise and is not explicitly calculated in the compensation formula, but its statistical characteristics are used in subsequent filter design.

[0125] Using the known quantities mentioned above, the hip sensor acceleration is... Subtracting the coupling terms yields the equivalent centroidal acceleration derived from this hip sensor: (20) This is the equivalent center-of-mass acceleration vector (unit: m / s²). From the first... The equivalent observable acceleration along the body's center of mass is derived from the measurements of a hip joint accelerometer after kinematic compensation. The subscript 'c' denotes "center of mass"; the subscript... The legs are numbered with values ​​FL (front left), FR (front right), HL (rear left), and HR (rear right); the superscript B indicates that the vector is represented in the body coordinate system {B}; wavy lines This indicates that the value is an indirect estimate derived through a compensation algorithm, rather than a direct measurement. This vector is one of the core outputs of the preprocessing layer. Each of the four hip sensors independently generates a set of... The acceleration of the center of mass directly output by the six-dimensional sensor Together, they form five redundant acceleration observations, providing redundant information for subsequent fusion estimation.

[0126] Acceleration vector (unit: m / s²) directly measured by the hip sensor. Measured by the sensor installed at the... The output of the three-dimensional accelerometer from the hip joint shell has undergone dimensional unification, system error correction, temperature compensation, and spatial coordinate alignment to the body coordinate system {B}. The measured value contains the superposition of four motion components: the true acceleration of the body's center of mass, the entrainment acceleration (centripetal term + tangential term) caused by the body's angular motion at the hip joint, and the relative motion acceleration generated by the leg's rotation around the hip joint.

[0127] This is the body angular acceleration vector (unit: rad / s²). It is taken from the angular acceleration channel of a six-dimensional accelerometer and has been calibrated, temperature-compensated, and aligned. It describes the rate of change of the body's angular velocity around each axis and is used to calculate the tangential drag acceleration at the hip joint.

[0128] This is the body angular velocity vector (unit: rad / s). Taken from the gyroscope of the airborne IMU, it has been calibrated, temperature-compensated, and coordinate-aligned. It describes the instantaneous angular rate of the body rotation about each axis and is used to calculate the centripetal pull acceleration at the hip joint.

[0129] This is the position vector (in meters) of the hip joint rotation center. It points from the body's center of mass C to the... The rotation center of each hip joint is a constant three-dimensional vector in the body coordinate system {B}. The theoretical value is obtained through a mechanical three-dimensional CAD model. After assembly, the precise relative coordinates between the rotation center of each hip and the centroid marker point are measured using a coordinate measuring machine or laser tracker. After coordinate transformation, the coordinates are fixed in the system configuration.

[0130] Tangential entrainment acceleration term (unit: m / s²). Derived from body angular acceleration. The resulting tangential acceleration at the hip joint. Direction: perpendicular to the angular acceleration vector. With lever arm vector The plane formed follows the right-hand rule; its size is equal to the product of the magnitude of the angular acceleration and the projection length of the arm vector in the direction perpendicular to the angular acceleration.

[0131] Centripetal acceleration term (unit: m / s²).

[0132] From the angular velocity of the body The resulting centripetal acceleration (also known as normal acceleration) at the hip joint.

[0133] Direction: Perpendicular to the angular velocity vector And point to the axis of rotation (i.e., the direction of the foot perpendicular from the hip joint point to the axis of rotation); Magnitude: equal to the square of the amplitude of angular velocity Multiply by the vertical distance from the hip joint point to the instantaneous axis of rotation.

[0134] The vector of acceleration due to the relative motion of the legs (unit: m / s²).

[0135] The four hip sensors output four independent equivalent center of mass acceleration estimates. The acceleration of the center of mass directly output by the six-dimensional sensor Together, they form five redundant centroid line acceleration observations, providing redundant information for subsequent multi-source fusion attitude estimation.

[0136] Filtering and Adaptive Preprocessing for Impact: A fourth-order Butterworth low-pass filter with a cutoff frequency of 80Hz is used on the equivalent centroid acceleration sequence to filter out noise spikes caused by high-frequency vibrations and foot impacts. For firing impacts, the firing time is obtained from the rising edge of the firing synchronization pulse. Define the impact duration window Within this window, the following processing is employed: First, transient glitches are suppressed using a 5-point median filter; if impact response templates for each sensor are calibrated... If the signal is positive, the signal is subtracted; otherwise, the acceleration data frames within the window are marked as "impact contamination," and their corresponding noise covariance pre-inflation factor is set to 100 times for subsequent fusion algorithms to adaptively reduce weights. The processing effect is as follows: Figure 4 As shown, the instantaneous oscillation caused by firing is weakened, but the motion waveform still retains the main trend of change.

[0137] Standard observation frame output: All processed data were aligned to a 500Hz frequency and packaged into standard observation frames, each containing: IMU angular velocity in body coordinate system and acceleration ; Six-dimensional center of mass acceleration and angular acceleration ; Four-way hip equivalent center of mass acceleration ; Angles of each joint angular velocity and torque ; Activation impact flag And the most recently fired timestamp.

[0138] This observation frame is transmitted to the attitude estimation solver in real time via shared memory.

[0139] At this point, the multi-source heterogeneous sensor data has undergone hierarchical preprocessing, addressing issues such as dimensional differences, systematic errors, temperature drift, coordinate inconsistencies, motion coupling, and impact contamination, and can be used as standard observation input for subsequent fusion algorithms.

[0140] The above-disclosed embodiments are merely one or more preferred embodiments of this application and should not be construed as limiting the scope of this application. Those skilled in the art can understand that all or part of the processes for implementing the above embodiments and equivalent changes made in accordance with the claims of this application still fall within the scope of this application.

Claims

1. A hierarchical preprocessing method for multi-source heterogeneous sensor data of quadruped robots, characterized in that, Includes the following steps: Acquire multi-source heterogeneous sensing data, which includes raw measurement data output from an inertial measurement unit, a six-dimensional accelerometer, a hip joint three-dimensional accelerometer, a joint encoder, and a torque sensor, as well as a firing synchronization signal output from a firing synchronization signal sensing module; convert the multi-source heterogeneous sensing data into SI physical quantities. The SI physical quantities are subjected to deterministic error correction, which includes sensitivity scaling factor compensation, inter-axis non-orthogonality compensation, constant zero bias compensation, and linear acceleration cross-coupling compensation for the angular acceleration channel; wherein, the linear acceleration cross-coupling compensation model for the angular acceleration channel is as follows: ;in, This is the corrected angular acceleration vector; This is the original output vector of the angular acceleration channel; This is the cross-coupling matrix of linear acceleration and diagonal acceleration. This is the corrected linear acceleration vector. This is the constant zero-bias vector for the angular acceleration channel; The real-time temperature values ​​of the corresponding sensors are obtained through the built-in temperature measurement units of the inertial measurement unit, the six-dimensional accelerometer, and the hip joint three-dimensional accelerometer, respectively. For each sensor, the zero-bias parameter and sensitivity parameter are corrected online based on the temperature-zero bias curve and temperature-sensitivity curve pre-calibrated by the sensor. The calibrated sensor measurements are transformed to a unified body coordinate system through a pre-calibrated spatial rotation matrix, and lever arm effect compensation is performed on sensors mounted near the center of mass. By utilizing the robot's forward kinematics model and rigid body kinematics relationship, centripetal acceleration, tangential acceleration, and leg relative motion-induced acceleration caused by non-center-of-mass mounting are removed from the measurements of the three-dimensional acceleration sensors of each hip joint, and the multi-path equivalent body center-of-mass line acceleration is extracted. The equivalent body center of mass acceleration sequence is low-pass filtered, and the firing synchronization signal output by the firing synchronization signal sensing module is used to mark the firing impact window. Impact interference suppression processing is performed within the firing impact window. The processed data is packaged into standard observation frames at a uniform frequency and sent to the fusion estimation module.

2. The hierarchical preprocessing method for multi-source heterogeneous sensor data of quadruped robots as described in claim 1, characterized in that, In the deterministic error correction of the measured physical quantities, the accelerometer error correction model for the inertial measurement unit and the six-dimensional accelerometer sensor is as follows: ； in, The corrected acceleration vector, This is a diagonal matrix of sensitivity scaling factors. It is an inter-axis non-orthogonal compensation matrix. The vector is a constant zero bias; the gyroscope is corrected using an isomorphic error model. This is the sensor's original output vector.

3. The hierarchical preprocessing method for multi-source heterogeneous sensor data of quadruped robots as described in claim 1, characterized in that, The temperature-zero bias curve adopts the following polynomial model: ； in, This is the temperature-dependent zero-bias vector; T is the current real-time temperature of the sensor. For reference temperature; This is the zero bias vector at the reference temperature; This is a first-order temperature coefficient vector, representing the rate of change of zero bias with linear temperature variation; This is a second-order temperature coefficient vector, representing the rate of change of zero bias with respect to a quadratic nonlinear temperature change; The temperature-sensitivity curve is modeled using the following polynomial model: ； in, This is the temperature-dependent sensitivity coefficient; The sensor provides real-time temperature readings. For reference temperature; The sensitivity coefficient at the reference temperature. The first-order sensitivity temperature coefficient represents the relative rate of change of sensitivity as a linear change with temperature. The second-order sensitivity temperature coefficient represents the relative rate of change of sensitivity with the second-order nonlinear variation of temperature.

4. The hierarchical preprocessing method for multi-source heterogeneous sensor data of quadruped robots as described in claim 1, characterized in that, The lever arm effect compensation for sensors mounted near the center of mass specifically includes: The sensor mounted near the center of mass includes a six-dimensional accelerometer and an inertial measurement unit, and the lever arm effect compensation is performed according to the following formula: ； in, The acceleration vector along the center of mass of the machine after lever arm compensation; The acceleration vector at the sensor's sensitive point. The lever arm vector pointing from the sensor's sensing center to the machine's center of mass. The body angular acceleration provided by the six-dimensional accelerometer The angular velocity of the machine body provided to the inertial measurement unit.

5. The hierarchical preprocessing method for multi-source heterogeneous sensor data of quadruped robots as described in claim 1, characterized in that, Among the extracted equivalent body center-of-mass accelerations, the extraction of these equivalent accelerations is based on the rigid body kinematics equations: ； in, Let be the acceleration vector output by the i-th hip sensor; The true linear acceleration of the body's center of mass; This is the vector of the body's angular acceleration. The angular velocity vector of the aircraft; The position vector of the center of rotation of the hip joint; This is the vector of the entrainment acceleration generated at the hip joint point by the relative motion of the legs; Measure noise for the sensor; By inverse solving and ignoring noise terms, the equivalent body center-of-mass acceleration derived from the i-th hip joint three-dimensional accelerometer is obtained: ； in, This is the equivalent acceleration vector along the center of mass of the organism; Let be the measurement value of the i-th hip joint three-dimensional accelerometer in the body coordinate system. Let be the position vector of the i-th hip joint rotation center in the body coordinate system. This is the drag acceleration vector generated by the relative motion of the legs at the hip joint. To measure noise for the sensor, Measurements taken from a six-dimensional accelerometer. Measurements were taken from the inertial measurement unit.

6. The hierarchical preprocessing method for multi-source heterogeneous sensor data of quadruped robots as described in claim 5, characterized in that, The leg-related acceleration vector in the rigid body kinematic equations is calculated using the following formula: ； in, This is the vector of the entrainment acceleration generated at the hip joint point by the relative motion of the legs; Let be the angle, angular velocity, and angular acceleration of the i-th hip joint, respectively. The unit vector for the hip joint rotation axis. The direction vector of the thigh rod is given, and the angle of the hip joint is measured in real time by the joint encoder. The angular velocity of the hip joint is obtained by performing first-order numerical difference on the hip joint angle sequence continuously collected by the joint encoder and then passing it through low-pass filtering. The angular acceleration of the hip joint is obtained by performing first-order numerical difference on the filtered hip joint angular velocity sequence, or by estimating it based on the hip joint torque measured by the torque sensor combined with the robot dynamics model.

7. The hierarchical preprocessing method for multi-source heterogeneous sensor data of quadruped robots as described in claim 1, characterized in that, The impact interference suppression process performed within the firing impact window specifically includes: The firing impact window is time-stamped by the rising edge of the firing synchronization signal. Mark the window length, which is 20ms to 50ms. Within the aforementioned shooting impact window, a 5-point median filter is first applied to suppress transient glitches, assuming that impact response templates for each sensor are pre-calibrated. Then, the signal in the window is subtracted from the impact response template to remove the recoil impact component. The calibration method of the impact response template is as follows: under standard firing conditions, the rising edge of the firing synchronization signal is used as the trigger reference, the impact response waveforms of each sensor channel are synchronously acquired and recorded, and the impact response waveforms recorded multiple times are time-domain aligned and averaged to obtain the impact response template. Otherwise, mark the acceleration data frames within the window as impact contamination states and set the corresponding noise covariance pre-expansion factor to 100 times.

8. The hierarchical preprocessing method for multi-source heterogeneous sensor data of quadruped robots as described in claim 1, characterized in that, The standard observation frame includes the angular velocity and acceleration of the inertial measurement unit in the body coordinate system, the centroidal acceleration and angular acceleration of the six-dimensional accelerometer, the equivalent centroidal acceleration of the body from the four-channel hip joint three-dimensional accelerometer, the angle / angular velocity / torque of each joint, the firing impact flag, and the most recent firing timestamp.

9. The hierarchical preprocessing method for multi-source heterogeneous sensor data of quadruped robots as described in claim 1, characterized in that, The low-pass filtering of the equivalent body centroid acceleration sequence specifically includes: A fourth-order Butterworth low-pass filter with a cutoff frequency of 80Hz is used to suppress noise spikes caused by high-frequency vibrations and foot impacts.

Images