Array MEMS inertial navigation error adaptive suppression method

Abstract

The application relates to the technical field of inertial navigation, in particular to an array type MEMS inertial navigation error adaptive suppression method. The method comprises the following steps: acquiring a plurality of MEMS inertial sensor data, performing clock pulse signal control, collecting in the same sampling period, extracting a preset period, arranging an accelerometer mean value and performing initial alignment processing to obtain an initial alignment result; performing attitude angle conversion, accelerometer data projection and navigation coordinate system velocity calculation based on the initial alignment result to generate a navigation coordinate system velocity result; sequentially reconstructing the navigation coordinate system velocity result, dividing a sliding window, calculating a range value, and arranging the range value in ascending order to generate a range value discrimination result; and finally performing adaptive factor coefficient distribution, comprehensive velocity calculation, position updating and attitude updating based on the range value discrimination result. The application can make the array type MEMS comprehensive velocity calculation basis clearer, improve navigation precision and stability, get rid of the limitation of a high-precision turntable, and reduce cost.

Description

Technical Field

[0001] This invention relates to the field of inertial navigation technology, and in particular to an adaptive method for suppressing errors in array-type MEMS inertial navigation systems. Background Technology

[0002] In the field of inertial navigation technology, existing solutions typically revolve around data acquisition, initial alignment, navigation calculation, and fusion output from multiple MEMS inertial sensors. This presents a problem of strong dependence on sensor-level data fusion. Because MEMS inertial sensors are soldered onto circuit boards, assembly processes prevent the coordinate system of N MEMS inertial sensors from being unified during installation. High-precision turntables are required for calibration, and specific rotation and positioning experiments are used to calculate the relative relationships of each MEMS inertial sensor. The data is then averaged to obtain the fused gyroscope and accelerometer data, thereby achieving the goal of suppressing inertial navigation errors through array-based fusion. However, high-precision turntables are expensive, and the calibration process is complex, increasing the development cost of array-based MEMS inertial navigation systems and failing to meet the product design goals and the characteristics of low-cost, miniaturized products. Summary of the Invention

[0003] To address the aforementioned technical problems, this invention provides an adaptive error suppression method for array-type MEMS inertial navigation, comprising:

[0004] S100: Acquire data from multiple MEMS inertial sensors, perform pitch angle calculation, roll angle calculation, and initial heading setting respectively, and obtain the initial alignment result;

[0005] S200. Based on the initial alignment results, perform attitude angle transformation, accelerometer data projection and navigation coordinate system velocity calculation on each MEMS to generate multiple navigation coordinate system velocity results.

[0006] S300. Obtain the velocity results of the navigation coordinate system, sort them in descending order to obtain the sequence reconstruction results; based on the sequence reconstruction results, perform a preset length sliding window division process to generate local velocity groups; based on the local velocity groups, perform range calculation and range ascending order sorting process to generate range discrimination results.

[0007] S400: Obtain the range value discrimination result, perform adaptive factor coefficient allocation processing to obtain adaptive factor coefficients; based on the adaptive factor coefficients, perform comprehensive velocity calculation processing to generate comprehensive velocity; based on the comprehensive velocity, perform position update and attitude update processing to generate update results.

[0008] Furthermore, the process of acquiring data from multiple MEMS inertial sensors includes:

[0009] The acquisition of multiple MEMS inertial sensor data includes: based on clock pulse signal control, performing the same sampling period on each MEMS inertial sensor to generate MEMS inertial sensor data with sensor serial number and sampling time mark;

[0010] Furthermore, the process of calculating pitch angle, roll angle, and initial heading settings includes:

[0011] Based on the extraction of the pre-set time period, continuous records of the stationary phase after startup are extracted from the MEMS inertial sensor data, and stationarity checks and accelerometer mean calculations are performed to generate the triaxial accelerometer mean results of each MEMS inertial sensor within the pre-set time period.

[0012] The staticity check includes judging the fluctuation range of angular velocity measurements from multiple MEMS inertial sensors and the continuous changes in accelerometer measurements. When there are instantaneous jumps or missing measurement markers, the channel is marked as an abnormal channel and excluded during the mean averaging process.

[0013] The accelerometer mean processing includes grouping by sensor number, verifying the integrity of records, removing abnormal records, and calculating the mean. Based on the triaxial accelerometer mean results, pitch angle and roll angle are calculated by sensor number, and a unified preset initial heading is written to each MEMS inertial sensor to generate an initial alignment result.

[0014] A consistency check is performed on the initial alignment results. When the initial attitude record of a certain MEMS inertial sensor deviates significantly from the other records, an abnormal attitude marker is written.

[0015] Furthermore, the process of projecting the accelerometer data includes:

[0016] The accelerometer data projection processing includes: performing sequence number matching and time matching based on the attitude transfer matrix and the accelerometer measurement values ​​in the MEMS inertial sensor data; performing coordinate system transformation on the accelerometer measurement values ​​of each MEMS inertial sensor; and generating the navigation coordinate system acceleration corresponding to each sensor.

[0017] Furthermore, the process of calculating and processing the velocity in the navigation coordinate system includes:

[0018] The navigation coordinate system velocity calculation process includes: based on the navigation coordinate system acceleration and the initial alignment result, performing independent recursive calculations according to the sensor number, continuously updating the navigation coordinate system acceleration of the current sampling period with the navigation coordinate system velocity record of the previous sampling period, generating the navigation coordinate system velocity record of each MEMS inertial sensor in the current sampling period, and synchronously writing a validity mark; when entering pure inertial calculation for the first time, the navigation coordinate system velocity record of the previous sampling period adopts zero velocity as the starting record.

[0019] Furthermore, the process of descending order sorting includes:

[0020] The descending sorting process includes: reading validity markers from the velocity results of the navigation coordinate system, sending only velocity records with valid validity markers into the sorting buffer, and performing descending sorting according to the size of the velocity records to generate the order reconstruction result, while retaining the sensor number and sampling time marker corresponding to each velocity record.

[0021] Furthermore, the process of dividing the preset length sliding window includes:

[0022] The preset length sliding window division process includes: based on the sequential reconstruction result, according to the preset fixed window length and adjacent advancement method, sequentially extracting adjacent velocity records starting from the sorting head to generate multiple local velocity groups. Each local velocity group includes velocity records within the group, sensor serial numbers within the group, and sampling time markers within the group, and synchronously writing the local group serial number, the start position within the group, and the end position.

[0023] Furthermore, the process of calculating the range and sorting the ranges in ascending order includes:

[0024] The range calculation and range ascending sorting process includes: performing an intra-group integrity check on each local velocity group, confirming that the number of records, sensor serial numbers, and sampling time markers within the group meet the requirements, and then calculating the difference between the maximum and minimum speed records within the group as the range value of that group; after completing the range calculation for all local velocity groups, arranging each range value and its corresponding local group serial number and sensor serial number within the group in ascending order according to the range value from smallest to largest to generate a range value discrimination result, which includes the local group serial number, the range value order, and the corresponding sensor serial number within the group.

[0025] Furthermore, the adaptive factor coefficient allocation process includes:

[0026] The adaptive factor coefficient allocation process includes: based on the range discrimination results, performing hierarchical processing on each local velocity group according to the range order; marking the navigation coordinate system velocity results in the local velocity group with smaller range values ​​as priority participation records; marking the navigation coordinate system velocity results in the local velocity group with larger range values ​​as subsequent participation records; and assigning corresponding adaptive factor coefficients to each navigation coordinate system velocity result accordingly, so that priority participation records have larger coefficients and subsequent participation records have smaller coefficients; when the same navigation coordinate system velocity result appears in multiple local velocity groups, the final adaptive factor coefficient is generated after cumulative discrimination according to the order position of the local velocity group in which the velocity result is located; when a certain navigation coordinate system velocity result only appears in the local velocity group with discrimination anomaly markers, it is first written into the low priority record area, and then participates in the coefficient merging process at the end of the current sampling period.

[0027] Furthermore, the process of calculating and processing the overall speed includes:

[0028] The comprehensive velocity calculation process includes: based on the adaptive factor coefficients and the velocity results of the navigation coordinate system, performing sensor number matching and sampling time matching, and weighting the velocity results of each navigation coordinate system and their corresponding adaptive factor coefficients according to the current sampling period to generate the comprehensive velocity of the current sampling period.

[0029] The key innovations of this invention include:

[0030] (1) Based on the speed results of the navigation coordinate system, perform descending sorting, preset length sliding window division, range calculation and range ascending sorting to generate range discrimination results, and convert the speed results of the navigation coordinate system from a single result set into a discriminable local speed group processing link.

[0031] (2) Based on the range discrimination result, perform adaptive factor coefficient allocation processing so that the adaptive factor coefficient corresponds to the local consistency discrimination result of the navigation coordinate system velocity result, and transform the range discrimination result into the control basis for comprehensive velocity calculation.

[0032] (3) The initial alignment result, the navigation coordinate system velocity result, the range discrimination result, the adaptive factor coefficient and the comprehensive velocity calculation are sequentially connected, and the comprehensive velocity is directly used for position update and attitude update to form a continuous operation link of data acquisition, navigation solution, sequential discrimination, comprehensive velocity calculation and update processing.

[0033] The following are its main beneficial effects:

[0034] (1) In view of the problem that the sensor layer data fusion in the existing scheme is highly dependent, the initial alignment result is obtained first, the navigation coordinate system velocity result is generated, and the sequence discrimination and comprehensive velocity calculation are completed in the navigation coordinate system velocity result layer. This shifts the processing position from the original measurement data layer to the navigation coordinate system velocity result layer, thereby reducing the probability of error divergence. This method is superior to the traditional method of directly taking the average of the original data.

[0035] (2) The calibration step was eliminated, reducing production costs. Attached Figure Description

[0036] Figure 1 This is a flowchart illustrating an adaptive error suppression method for array-type MEMS inertial navigation provided in an embodiment of this application. Detailed Implementation

[0037] Example 1: Refer to Figure 1 This is a flowchart illustrating an adaptive error suppression method for array-type MEMS inertial navigation provided in an embodiment of the present invention. The flowchart may include at least steps S100-S400:

[0038] S100: Acquire data from multiple MEMS inertial sensors, perform pitch angle calculation, roll angle calculation, and initial heading setting to obtain the initial alignment result;

[0039] S200. Based on the initial alignment result, perform attitude angle transformation, accelerometer data projection and navigation coordinate system velocity calculation to generate navigation coordinate system velocity results;

[0040] S300. Obtain the velocity results of the navigation coordinate system, sort them in descending order to obtain the sequence reconstruction results; based on the sequence reconstruction results, perform a preset length sliding window division process to generate local velocity groups; based on the local velocity groups, perform range calculation and range ascending order sorting process to generate range discrimination results.

[0041] S400: Obtain the range value discrimination result, perform adaptive factor coefficient allocation processing to obtain adaptive factor coefficients; based on the adaptive factor coefficients, perform comprehensive velocity calculation processing to generate comprehensive velocity; based on the comprehensive velocity, perform position update and attitude update processing to generate update results.

[0042] Step S100 includes at least steps S110-S130:

[0043] S110. Acquire data from multiple MEMS inertial sensors, perform clock pulse signal control and same sampling period acquisition processing to obtain MEMS inertial sensor data.

[0044] Specifically, this section corresponds to the initial execution segment of the main step "Synchronous Acquisition and Initial Alignment of MEMS Inertial Sensor Data". The input sources are the power-on startup status of the array-type MEMS inertial navigation device and the real-time measurement outputs of multiple MEMS inertial sensors. The MEMS is a Micro-Electro-Mechanical System (MEMS), and the multiple MEMS inertial sensors are multiple inertial measurement units mounted on the same circuit board. Each inertial measurement unit includes at least a gyroscope and an accelerometer, where the gyroscope is used to output angular velocity measurements and the accelerometer is used to output specific force measurements.

[0045] The clock pulse signal control refers to the control signal sent by the acquisition control unit to each MEMS inertial sensor at a unified pace, enabling multiple MEMS inertial sensors to complete a synchronous reading within the same sampling period. Acquisition within the same sampling period means that angular velocity measurements and force measurements from the same batch have consistent sampling time markers, avoiding timing misalignment caused by sequential readings. In practice, a fixed sampling pace can be preset within the acquisition control unit, and a one-to-one corresponding reading channel can be established for each MEMS inertial sensor. When each sampling pace arrives, a reading trigger is first issued, and then the buffer reading and time marker writing for each channel are completed in a preset order. After reading, the gyroscope triaxial measurement values, accelerometer triaxial measurement values, sampling time markers, and sensor serial numbers corresponding to each MEMS inertial sensor are combined and organized to form the original record of the current sampling period.

[0046] Furthermore, when a MEMS inertial sensor fails to return a complete record within the current sampling period, the acquisition control unit writes a missing measurement marker to that channel and retains the valid time marker from the previous sampling period for identifying abnormal channels during the subsequent initial alignment stage. When the returned records show channel disorder, the order is restored according to the sensor number before being written into the current sampling period record. Understandably, the MEMS inertial sensor data is not a single numerical value, but rather multiple sets of synchronous measurement records organized according to sensor number, including gyroscope measurements, accelerometer measurements, and the time correspondence within the same sampling period.

[0047] For example, during the startup and standby preparation phase of the mobile robot, multiple MEMS inertial sensors simultaneously enter the power-on acquisition state. The acquisition control unit continuously sends a unified clock pulse signal to read the angular velocity and specific force measurements of each MEMS inertial sensor according to the same sampling period, and writes the results of this round of readings into the buffer area. Each record in the buffer area carries a sensor serial number and a sampling time marker, and then the entire record is output for the next subsection to call. After the above processing, the resulting output field is named "MEMS Inertial Sensor Data". This output field serves as the direct input of S120 and is also used as the measurement source for the subsequent accelerometer data projection processing in S220, thus forming a stable data connection between this main step and S200.

[0048] S120. Based on the MEMS inertial sensor data, perform preliminary time period extraction and accelerometer mean processing to generate initial alignment data.

[0049] Specifically, this section uses the "MEMS inertial sensor data" output by S110 as the sole input, without rereading external measurement records. Instead, it extracts static phase data for initial alignment from the synchronized data that has already been processed for the same sampling period. The aforementioned pre-set time period refers to a fixed acquisition time after the array-type MEMS inertial navigation device enters the startup state but before pure inertial calculation begins. This time period corresponds to the stage where the carrier remains stationary or nearly stationary. The extraction of the pre-set time period refers to extracting sampling records that meet the static condition from the MEMS inertial sensor data of multiple consecutive sampling periods within the initial segment. In specific implementation, the startup time marker and sampling time marker can be read first, and continuous records can be extracted from the "MEMS inertial sensor data" in chronological order. Then, a static check is performed on the extracted continuous records. The staticity check can be judged based on the fluctuation range of angular velocity measurements from multiple MEMS inertial sensors and the continuous changes in accelerometer measurements. When multiple MEMS inertial sensors do not show significant jumps within several consecutive sampling periods, the recorded segment is confirmed as the pre-defined time period. When a channel has an instantaneous jump or missing measurement marker, that channel is marked as an abnormal channel within that time period, and the corresponding records of the abnormal channel are excluded during averaging. The accelerometer averaging refers to summarizing the triaxial accelerometer measurements of each MEMS inertial sensor within the pre-defined time period according to the sensor number, removing abnormal records, performing coaxial accumulation, and outputting the triaxial average result of each MEMS inertial sensor within the pre-defined time period. The "averaging" here is not a simple addition, but rather first grouping by sensor number, then checking the completeness of the records according to the sampling time order, then removing missing measurement markers, out-of-order records, and records that failed the staticity check, and finally calculating the average of the remaining valid records to obtain a small batch of stable input suitable for initial alignment. Furthermore, to ensure a unified input format for subsequent pitch and roll angle calculations, the initial alignment data can also simultaneously include a pre-set time period marker, a set of sensor serial numbers involved in the data processing, and a set of abnormal channel markers. This allows S130 to directly access the already processed stable data during pitch, roll, and initial heading settings, without needing to revisit the original sampling records. Understandably, the minimum set of initial alignment data includes the average accelerometer values ​​of each MEMS inertial sensor within the pre-set time period and the corresponding sensor serial numbers; the pre-set time period marker and abnormal channel marker are preferred extensions used to improve the traceability of the operation process.In a specific engineering embodiment, after the mobile robot is placed on a flat surface and powered on, the data acquisition and control unit first continuously reads "MEMS inertial sensor data" for multiple sampling periods, then extracts the records during the stationary phase after startup. Subsequently, the triaxial accelerometer measurements of each MEMS inertial sensor are grouped and organized, and records with instantaneous fluctuations and missing measurement markers are removed, finally generating a stable data set for initial alignment. After processing in this section, the resulting output field is named "Initial Alignment Data". This output field is directly input into S130 and serves as the basic data source for pitch angle calculation, roll angle calculation, and initial heading setting in S130.

[0050] S130. Based on the initial alignment data, perform pitch angle calculation, roll angle calculation and initial heading setting to generate the initial alignment result.

[0051] Specifically, this section uses the "initial alignment data" output by S120 as input to perform the complete landing process of initial alignment. The pitch angle calculation refers to determining the forward / backward tilt state of each MEMS inertial sensor relative to the navigation coordinate system based on the average accelerometer values ​​obtained from each MEMS inertial sensor within a pre-defined time period. The roll angle calculation refers to determining the left / right tilt state of each MEMS inertial sensor relative to the navigation coordinate system based on the same set of average accelerometer values. The initial heading setting refers to writing a unified preset value to the initial heading of each MEMS inertial sensor under the condition that no external orientation information is introduced during the initial stationary phase. In specific implementation, the average accelerometer values ​​in the "initial alignment data" are read sequentially according to the sensor number. Then, the three-axis average values ​​corresponding to each MEMS inertial sensor are sent to the pitch angle calculation unit and the roll angle calculation unit to generate a set of pitch angle results and a set of roll angle results, respectively.

[0052] Specifically, the pitch and roll angles are calculated based on the average accelerometer readings obtained over a pre-defined time period. Let the... The average values ​​of the triaxial accelerometers of the MEMS inertial sensors over the predefined time period are as follows: , , Then the pitch angle With roll angle Calculate using the following formula:

[0053]

[0054]

[0055] in:

[0056] : No. The pitch angle obtained from the initial alignment of a MEMS inertial sensor;

[0057] Arcsine function, used to calculate angles from accelerometer ratios;

[0058] : No. The data collected by the MEMS within the pre-defined time period (10 seconds) The mean of the shaft accelerometer data;

[0059] : No. Data collected by MEMS within a pre-defined time period The mean of the shaft accelerometer data;

[0060] : No. Data collected by MEMS within a pre-defined time period The mean of the shaft accelerometer data;

[0061] : No. The roll angle obtained from the initial alignment of the MEMS;

[0062] The four-quadrant arctangent function is used to determine the... shaft and The average value of the shaft accelerometer determines the sign and quadrant of the roll angle.

[0063] Subsequently, the initial heading corresponding to each MEMS inertial sensor is written into the initial heading field, forming an initial attitude record that corresponds one-to-one with the sensor number. The initial heading setting here uses a uniform preset value because this main step is responsible for the initial attitude establishment task before pure inertial calculation. Its core content is to first establish an initial attitude reference that can enter the S210 attitude angle transformation processing.

[0064] Furthermore, after the pitch and roll angle calculations are completed, a consistency check can be performed on the initial attitude records of each MEMS inertial sensor. This consistency check compares the degree of convergence of the pitch and roll angle results among multiple MEMS inertial sensors. When the initial attitude record of a certain MEMS inertial sensor deviates significantly from the others, this record is marked as an abnormal attitude, and this mark is retained in the "Initial Alignment Result" for identification during subsequent attitude angle conversion in S210. Understandably, the "Initial Alignment Result" is not a single angle value, but rather a set of initial attitude records organized by sensor number, which includes at least pitch angle results, roll angle results, and an initial heading field, and optionally includes an abnormal attitude mark and a pre-set time period mark.

[0065] The minimum set of core parameters for the initial alignment result consists of the pitch angle, roll angle, and initial heading fields necessary for entering S210. Abnormal attitude marking is a preferred extension, used to isolate abnormal records before pure inertial calculation begins. In a directly implementable engineering scenario, after the mobile robot completes the pre-set time period of data acquisition while remaining stationary, the initial alignment processing unit reads the average accelerometer value for each MEMS inertial sensor, generates pitch and roll angle results for each sensor according to their sequence number, and then writes a unified initial heading to all MEMS inertial sensors, forming a complete "initial alignment result." This result is then sent to the "initial alignment result" in S210 as input for attitude angle conversion processing and continues to be used as the attitude initiation basis in the subsequent navigation coordinate system velocity calculation in S230. After processing in this section, the output field is named "initial alignment result," and its subsequent input position is first the "initial alignment result" in S210, and simultaneously participates in the navigation coordinate system velocity calculation in S230 along with the navigation coordinate system acceleration generated in S220.

[0066] In summary, this step achieves the following technical benefits: By acquiring data within the same sampling period, extracting data from a pre-defined time period, and generating initial alignment results, it organizes the start-up measurement records from multiple MEMS inertial sensors into a unified initial record that can be directly used for pure inertial calculations, avoiding the need for subsequent steps to repeatedly trace back to the original records. Compared to the processing route that directly fuses the original measurement data, this step first establishes an independent initial attitude reference for each MEMS inertial sensor, providing a clear and continuous input chain for subsequent generation of navigation coordinate system velocity results and sequential reconstruction processing.

[0067] Step S200 includes at least steps S210-S230:

[0068] S210. Obtain the initial alignment result, perform attitude angle transformation processing, and obtain the attitude transfer matrix.

[0069] Specifically, this section corresponds to the initial execution segment of the main step "Navigation Coordinate System Velocity Generation," with the input source being the initial alignment result output from preceding step S130. The initial alignment result is an initial attitude record organized by sensor number, containing at least pitch angle, roll angle, and initial heading. The pitch angle characterizes the forward and backward tilt state, the roll angle characterizes the left and right tilt state, and the initial heading characterizes the initial orientation state. The attitude angle transformation processing refers to converting the initial attitude record corresponding to each MEMS inertial sensor into a directional correspondence between the navigation coordinate system and the sensor coordinate system; the attitude transfer matrix refers to the direction transformation record used for subsequent accelerometer data projection processing, and its content corresponds one-to-one with the sensor number.

[0070] In practice, the calculation unit reads the initial alignment result, establishes attitude angle conversion tasks one by one according to the sensor number, first checks the integrity of pitch angle, roll angle and initial heading, then performs the conversion operation in the same order, and outputs the corresponding attitude transfer matrix.

[0071] Specifically, the attitude transfer matrix is ​​based on the pitch angle of each MEMS inertial sensor. Roll angle and initial heading Calculate using the following formula:

[0072]

[0073] in:

[0074] : No. The attitude transfer matrix of a MEMS will be transferred from the carrier coordinate system ( Transform to navigation coordinate system );

[0075] : No. The heading angle of each MEMS (the initial heading is set to a uniform preset value);

[0076] : No. The pitch angle of a MEMS;

[0077] : No. The roll angle of a MEMS.

[0078] Furthermore, when the initial attitude record of a certain MEMS inertial sensor contains missing, repeated, or abnormal attitude markers, the calculation unit writes a conversion abnormal marker to the sensor and calls the valid initial attitude record of the sensor in the previous stable sampling period to perform a replacement conversion; when there is no valid initial attitude record in the previous stable sampling period, the sensor is temporarily isolated from the navigation coordinate system velocity generation chain of the current sampling period, and the isolation state record is retained until the end of the current sampling period.

[0079] Understandably, the attitude angle conversion process is not a uniform conversion for all MEMS inertial sensors, but rather an independent conversion performed on each MEMS inertial sensor. This corresponds to the independent establishment of the initial alignment results mentioned above. The reason for this approach is that the present invention does not subsequently employ a direct averaging fusion route at the sensor layer, but instead retains an independent pure inertial calculation branch for each MEMS inertial sensor, thus ensuring that each MEMS inertial sensor has its own navigation coordinate system velocity source within the current sampling period. For example, after the mobile robot starts and completes the initial alignment during the stationary phase, the calculation unit first reads the pitch angle, roll angle, and initial heading corresponding to each MEMS inertial sensor, then generates an attitude transfer matrix for each sensor according to its sequence number, and writes the conversion results into the matrix buffer. Each record in the matrix buffer carries a sensor sequence number, a sampling time marker, and a conversion anomaly marker. After processing in this section, the output field is named "Attitude Transfer Matrix". This output field is directly called by the "Attitude Transfer Matrix" of S220. At the same time, this output field maintains the sequence number correspondence with the MEMS inertial sensor data within this main step, thereby establishing the input basis for the subsequent generation of navigation coordinate system acceleration.

[0080] S220. Based on the attitude transfer matrix and the MEMS inertial sensor data, perform accelerometer data projection processing to generate navigation coordinate system acceleration.

[0081] Specifically, the input sources for this section include the attitude transfer matrix output from S210 and the MEMS inertial sensor data generated and retained in the preceding main step S100. The MEMS inertial sensor data includes at least the accelerometer measurements, gyroscope measurements, sampling time markers, and sensor serial numbers for each MEMS inertial sensor during the same sampling period. This section actually uses the accelerometer measurements corresponding to the same sensor serial number and sampling period as the attitude transfer matrix. The accelerometer data projection processing refers to transforming the accelerometer measurements of each MEMS inertial sensor in the sensor coordinate system to the navigation coordinate system via the attitude transfer matrix, obtaining a unified directional result that can be processed for velocity calculation in the navigation coordinate system. The navigation coordinate system acceleration refers to the navigation direction acceleration records organized by sensor serial number.

[0082] In practice, the calculation unit first performs sequence number matching and time matching. Sequence number matching is used to find the accelerometer measurement value corresponding to each attitude transfer matrix, and time matching is used to confirm that the sampling time corresponding to the attitude transfer matrix is ​​consistent with the sampling time of the accelerometer measurement value. After matching is completed, the corresponding attitude transfer matrix is ​​called for each MEMS inertial sensor, and coordinate system transformation is performed on its accelerometer measurement value to output the corresponding navigation coordinate system acceleration.

[0083] The mathematical expression for this coordinate system transformation is: for the first... A MEMS, and its accelerometer measurements in the sensor coordinate system. Form column vectors, then multiply by the attitude transition matrix on the left. Obtain the acceleration projection in the navigation coordinate system. ,Right now:

[0084]

[0085] in:

[0086] : No. The MEMS accelerometer measurements are in the navigation coordinate system. Projection (3D vector) under )

[0087] : No. MEMS Axis accelerometer measurement data;

[0088] : Transpose operation of a matrix or vector.

[0089] Furthermore, when a MEMS inertial sensor experiences a serial number matching failure, time inconsistency, or missing accelerometer measurement, the calculation unit first records the cause of the anomaly and then performs processing according to the anomaly type: for short-term missing measurements, it reads the valid accelerometer measurement value from the previous sampling period of the sensor and retains the current sampling time marker; for time inconsistency, it discards the late-arriving record and uses the earliest valid record that has completed matching within the same sampling period; for continuous anomalies, it directly outputs a navigation coordinate system acceleration masking marker for the current sampling period of the sensor, preventing it from participating in the navigation coordinate system velocity calculation processing of this sampling period.

[0090] Understandably, the minimum set of accelerations in the navigation coordinate system includes the sensor number, sampling time marker, and projected accelerometer results. These three items are essential for S230 to perform navigation coordinate system velocity calculation. Anomaly markers, matching status, and sources of missing measurements are preferred extended content used for operation recording and subsequent troubleshooting. In a practical engineering scenario, when the mobile robot enters a low-speed straight-line phase, multiple MEMS inertial sensors continuously output synchronously acquired accelerometer measurements. The calculation unit reads the attitude transfer matrix generated in the previous section, projects the accelerometer measurements of each MEMS inertial sensor, obtains multiple sets of navigation coordinate system accelerations in the same navigation coordinate system, and writes the results into the navigation acceleration buffer.

[0091] Each record in the navigation acceleration buffer can be directly accessed by subsequent steps S230. After processing in this section, the output field is named "Navigation Coordinate System Acceleration". This output field is directly input into the "Navigation Coordinate System Acceleration" of S230 and together with the initial alignment result, constitutes the pre-input for the navigation coordinate system velocity calculation and processing in the current sampling period.

[0092] S230. Based on the acceleration of the navigation coordinate system and the initial alignment result, perform navigation coordinate system velocity calculation processing to generate navigation coordinate system velocity results.

[0093] Specifically, this section corresponds to the closing execution segment of the main step "Navigation Coordinate System Velocity Generation". The input sources include the navigation coordinate system acceleration output in S220 and the initial alignment result output in the preceding S130, which has been continuously referenced in this main step. The navigation coordinate system velocity calculation process refers to independently recursively calculating the navigation coordinate system acceleration of each MEMS inertial sensor in the current sampling period to obtain the velocity record of that MEMS inertial sensor in the navigation coordinate system. The navigation coordinate system velocity result refers to multiple sets of velocity outputs organized according to sensor number, sampling time marker, and velocity record. In specific implementation, the calculation unit first reads the navigation coordinate system acceleration and then calls the initial alignment result of the corresponding MEMS inertial sensor to confirm the initial attitude state and sensor validity state of the current velocity recursion.

[0094] Subsequently, the navigation coordinate system velocity calculation process is performed one by one according to the sensor number. The navigation coordinate system acceleration of the current sampling period is continuously updated with the navigation coordinate system velocity record of the previous sampling period to generate the navigation coordinate system velocity result of the current sampling period.

[0095] The recursive formula for continuous updates is:

[0096]

[0097] in:

[0098] Current sampling period (the first sampling period) (the cycle) The navigation coordinate system calculated by MEMS ( Velocity (three-dimensional vector);

[0099] Previous sampling period (the first sampling period) (the cycle) The navigation coordinate system velocity calculated by MEMS;

[0100] Sampling period;

[0101] Earth's rotational angular velocity in the navigation coordinate system ( Projection (3D vector) under )

[0102] The angular velocity of the navigation system caused by the motion of the carrier in the navigation coordinate system ( Projection (3D vector) under )

[0103] Local gravitational acceleration in the navigation coordinate system ( Projection (3D vector) under )

[0104] : Discrete-time index, representing the current sampling period and the previous sampling period.

[0105] This recursion is based on the standard inertial navigation velocity update equation and is applied independently to each MEMS inertial sensor. Upon first entering the pure inertial solution, Recording begins at zero velocity.

[0106] The navigation coordinate system velocity record of the previous sampling period can be continuously maintained by the calculation unit in the local main memory area according to the sensor number; when entering the pure inertial calculation stage for the first time, the navigation coordinate system velocity record of the previous sampling period adopts zero velocity as the starting record.

[0107] Furthermore, to ensure that the subsequent descending order processing in S310 has a directly callable input format, this section writes a unified record structure for each MEMS inertial sensor when generating the navigation coordinate system velocity results. This record structure includes at least the sensor number, sampling time marker, current sampling period velocity record, and validity marker. The validity marker indicates whether the record can enter the sequential reconstruction process; if a MEMS inertial sensor has continuous anomalies, projection failures, or unrecovered initial attitude anomalies in the current sampling period, the sensor's velocity record is marked as invalid and removed from the sorted input set before entering S310. Understandably, the navigation coordinate system velocity results are not a unified velocity obtained by averaging the velocities of multiple MEMS inertial sensors, but rather an independent set of velocities obtained after each MEMS inertial sensor completes pure inertial calculations. This is because subsequent S300 needs to perform sequential reconstruction, preset length sliding window division, and range discrimination on the multiple navigation coordinate system velocity results; only by retaining the independent velocity results of each MEMS inertial sensor can the subsequent local velocity groups and range discrimination results have a practical input basis.

[0108] In a directly implementable engineering example, after the mobile robot enters the low-speed start-up phase from a standstill, the calculation unit reads the navigation coordinate system acceleration and initial alignment results corresponding to each MEMS inertial sensor in each sampling cycle, updates the velocity record one by one according to the sensor number, and sends the navigation coordinate system velocity results generated in this cycle to the sorting buffer for S310 to read. After processing in this section, the output field is named "Navigation Coordinate System Velocity Result". This output field is directly called by S310's "Navigation Coordinate System Velocity Result" and is also one of the basic inputs for S420 to perform comprehensive velocity calculation processing in the subsequent process.

[0109] In summary, this step transforms the initial alignment results and synchronously acquired MEMS inertial sensor data into independently comparable navigation coordinate system velocity results through attitude angle conversion, accelerometer data projection, and navigation coordinate system velocity calculation. Compared to the previous approach of first correcting and fusing the initial measurement data before unified calculation, this step preserves the independent pure inertial calculation branches of each MEMS inertial sensor. This ensures stable input for subsequent sequential reconstruction, local velocity group division, and range discrimination, thus establishing the comprehensive velocity formation process on a locally consistent discrimination chain.

[0110] Step S300 includes at least steps S310-S330:

[0111] S310. Obtain the velocity results of the navigation coordinate system, sort them in descending order, and obtain the sequence reconstruction results.

[0112] Specifically, this section corresponds to the initial execution segment of the main step "Reconstruction of Navigation Coordinate System Velocity Result Order and Range Discrimination," with the input source being the navigation coordinate system velocity results output from the preceding S230. The navigation coordinate system velocity results are multiple independent velocity records organized by sensor number, sampling time marker, and velocity record. These velocity records are the current sampling period velocity records obtained by each MEMS inertial sensor after completing pure inertial calculations within the same navigation coordinate system. The descending order sorting process refers to arranging the navigation coordinate system velocity results participating in this round of discrimination in descending order according to a preset sorting rule within the same sampling period, forming a sequential result that can be directly called upon by subsequent preset length sliding window division processing. The preset sorting rule here adopts a unified order rule, and the processing object is not the original gyroscope measurement value or accelerometer measurement value, but rather the velocity record after attitude angle transformation, accelerometer data projection, and navigation coordinate system velocity calculation processing. In practice, the sorting control unit first reads the validity markers in the speed results of the navigation coordinate system, sends only the speed records with normal validity markers into the sorting buffer, then completes the verification of the same sampling period according to the sampling time marker, and then performs descending sorting according to the size of the speed records. After the sorting is completed, the sensor number and sampling time marker corresponding to each speed record are retained.

[0113] Furthermore, when a speed record from a MEMS inertial sensor contains invalid, missing, or inconsistent time markers, the sorting control unit does not directly delete the sensor's serial number. Instead, it first writes the record to the abnormal record area and then removes it from the current sampling period's sorting buffer, ensuring that the sequential result of the current sampling period consists only of valid speed records. Understandably, the minimum set of the sequential reconstruction results includes the sorted speed records, the corresponding sensor serial numbers, and the corresponding sampling time markers. The sorted speed records are used for the preset-length sliding window division process in S320, the corresponding sensor serial numbers are used to trace the source of each speed record within a local speed group, and the corresponding sampling time markers are used to maintain temporal consistency throughout the entire navigation calculation cycle.

[0114] In a directly implementable engineering scenario, during the low-speed movement phase of a mobile robot, multiple MEMS inertial sensors output navigation coordinate system velocity results within the same sampling period. After reading each velocity record, the sorting control unit first removes individual sensor records with continuous anomalies in the current sampling period, then sorts the remaining valid velocity records in descending order, and writes the sorted complete records into the sorting buffer. After processing in this section, the output field is named "Sequence Reconstruction Result." This output field is directly called by the "Sequence Reconstruction Result" function of S320 and serves as the sole sequential input source for the subsequent generation of local velocity groups. Simultaneously, this output field continuously retains its correspondence with the navigation coordinate system velocity results within this main step, allowing subsequent range judgment results to still be traced back to the specific MEMS inertial sensor.

[0115] S320. Based on the sequence reconstruction result, perform a preset length sliding window division process to generate local velocity groups.

[0116] Specifically, the input source for this section is the sequential reconstruction result output by S310 mentioned above. The preset-length sliding window partitioning process refers to sequentially extracting adjacent velocity records according to a fixed window length and a fixed advancement method from the already sorted descending sequence of velocity records, forming multiple comparable local velocity groups. The local velocity group refers to a small group of records consisting of several sequentially adjacent navigation coordinate system velocity results within the same sampling period. Each group retains the velocity record within the group, the sensor number within the group, and the sampling time marker within the group.

[0117] In practice, the local group generation unit first reads all valid velocity records from the sequential reconstruction result, and then sets the window range according to a preset length. This preset length is one of the smallest parameter sets in the core processing chain of this invention, and its function is to convert the overall sorted velocity sequence into multiple locally comparable units. In this embodiment, the preset length adopts the same window length as the preceding scheme, and it advances gradually from the beginning of the sorting process to the end in an adjacent manner. Each advancement forms a new local velocity group until the end of the sorting process no longer satisfies the complete window range.

[0118] Furthermore, when generating each local velocity group, the local group generation unit not only extracts the velocity records within the group but also simultaneously writes the local group number, the start position within the group, and the end position within the group. This allows S330 to directly perform discrimination based on local velocity groups when calculating and sorting the range in ascending order, without needing to return to the sequential reconstruction results to retrieve data again. To maintain the stability of the entire operation, this section also sets boundary constraints and exception handling logic. The boundary constraints refer to the fact that when the number of valid velocity records in the current sampling period is insufficient to form at least one complete local velocity group, the local group generation unit stops the current round of local group division and writes a window insufficiency flag to the current sampling period. The exception handling logic refers to the fact that when duplicate sensor numbers, conflicting time markers, or missing records occur within a local velocity group, the local group generation unit writes an exception flag for that local velocity group and simultaneously retains this flag when passing it to S330, which then decides whether to participate in the range calculation.

[0119] Understandably, the local velocity group is not a re-averaging of the velocity results for all navigation coordinate systems, but rather a segmented organization of sequentially adjacent velocity records, with the aim of providing local comparison objects for subsequent range calculations.

[0120] In a real-world engineering scenario, during the continuous turning phase of a mobile robot, after the sorting control unit outputs the sequence reconstruction result of the current sampling period, the local group generation unit starts from the head of the sorting and sequentially extracts adjacent velocity records according to a preset length to form the first local velocity group, the second local velocity group, and subsequent local velocity groups. The velocity records, sensor numbers, and intra-group positions of each group are then written into the local group buffer. After processing in this section, the output field is named "Local Velocity Group." This output field is directly called by the "Local Velocity Group" function of S330 and serves as a pre-input for range value discrimination between this main step and S400. This ensures that the subsequent adaptive factor coefficient allocation processing no longer deals with the disordered overall velocity records but instead focuses on the intra-group records that have already been locally divided.

[0121] S330. Based on the local velocity group, perform range calculation and range ascending order sorting to generate range discrimination results.

[0122] Specifically, the input source for this section is the local velocity group output by S320 mentioned above. The range calculation refers to reading all velocity records within each local velocity group, determining the maximum and minimum velocity records within the group, and generating the range value corresponding to that local velocity group based on the difference between the two records. The ascending range sorting process refers to rearranging the range values ​​of all local velocity groups within the current sampling period in ascending order, while simultaneously retaining the local group number, sensor number within the group, and position record corresponding to each range value. Here, the range value reflects the dispersion of velocity records within the same local velocity group. The smaller the range value, the more concentrated the velocity records within the local velocity group; the larger the range value, the more significant the differences in velocity records within the local velocity group. In practice, the discrimination processing unit first reads each local velocity group, performs an intra-group integrity check on each local velocity group, and confirms that the number of records, sensor number, and sampling time marker within the group meet the requirements of the current sampling period before performing the range calculation. After completing the range calculation for all local velocity groups, the discrimination processing unit sends each range value and its corresponding relationship into the ascending sort buffer, and outputs the range value results in ascending order.

[0123] Furthermore, during the range calculation process, when a local velocity group has anomaly markers, missing records, or conflicting time markers, the discrimination processing unit first writes anomaly markers to the local velocity group and then places it in the post-sorting area. When the anomaly level reaches a preset threshold, the local velocity group does not participate in the generation of valid range order results within the current sampling period. The reason for this processing is that the adaptive factor coefficient allocation processing in S410 of this invention does not directly assign fixed weights to individual velocity records, but rather identifies local velocity groups with higher local consistency based on the range discrimination results, and then uses this local consistency as the input basis for the formation of the comprehensive velocity.

[0124] Understandably, the minimum set of the range discrimination results includes the local group number, the range order result, the corresponding sensor number within the group, and the corresponding sampling time mark. The local group number and the range order result are core inputs necessary for S410 to perform adaptive factor coefficient allocation processing, and the corresponding sensor number within the group is a necessary record for subsequently mapping the local group discrimination results back to the navigation coordinate system velocity results.

[0125] In a directly implementable engineering scenario, when a mobile robot operates on uneven ground, multiple MEMS inertial sensors will exhibit certain differences due to local disturbances. The discrimination processing unit calculates the range value for each of the multiple local velocity groups generated within the current sampling period, then forms a range value order result from smallest to largest, recording the local velocity groups with smaller range values ​​first. This allows subsequent adaptive factor coefficient allocation processing to prioritize reading these locally consistent local velocity groups. After processing in this section, the output field is named "Range Value Discrimination Result." This output field is directly used by S410's "Range Value Discrimination Result," and it maintains a traceable correspondence with the navigation coordinate system velocity results. Furthermore, during the subsequent comprehensive velocity calculation processing in S420, it indirectly affects the adaptive factor coefficients corresponding to the velocity results of each navigation coordinate system.

[0126] To more clearly describe the mathematical logic behind the above-mentioned sequence reconstruction, sliding window partitioning, and range determination, the data processing procedure for this step is summarized as follows:

[0127] Let the number of valid MEMS inertial sensors participating in the discrimination during the current sampling period be . The velocity results in the navigation coordinate system calculated by each sensor are denoted as... First, these velocity results are sorted in descending order of numerical value, resulting in a new ordered sequence. Then, a sliding window with a step size of 3 is used to divide this ordered sequence, yielding a total of... Group of local velocity data, denoted as Each of them This corresponds to a set of velocity records within a window. The difference between the maximum and minimum velocity records within each window is calculated to obtain the range of each window, denoted as . Finally, these windows are sorted in ascending order according to their range values, resulting in the reordered window data. That is, the range value discrimination result.

[0128] In the above process:

[0129] Indicates will The velocity calculation results for each MEMS were sorted in descending order and then divided using a sliding window with a step size of 3. Group local velocity data;

[0130] This represents the range value corresponding to each sliding window, that is, the difference between the maximum speed and the minimum speed within the window;

[0131] This represents the sliding window data that has been rearranged after being sorted in ascending order of range.

[0132] Through the above processing, the original disordered velocity results are transformed into range value order results that can be judged by local consistency, providing a clear quantitative basis for subsequent adaptive factor coefficient allocation.

[0133] In summary, this step reconstructs the velocity results in the navigation coordinate system sequentially, then performs pre-defined length sliding window division and range value discrimination processing. This transforms multiple disordered velocity records into local velocity groups that can be used for sequential discrimination. Compared to processing methods that directly average all velocity records or directly weight them according to overall statistics, this step identifies local velocity groups with higher local consistency and passes this identification result to the subsequent adaptive factor coefficient allocation processing, ensuring that the comprehensive velocity formation process is based on the discrimination of local dispersion.

[0134] Step S400 includes at least steps S410-S430:

[0135] S410. Obtain the range value discrimination result, perform adaptive factor coefficient allocation processing, and obtain adaptive factor coefficients.

[0136] Specifically, this section corresponds to the initial execution segment of the main step "Adaptive Factor Coefficient Determination and Comprehensive Velocity Update," with the input source being the range value discrimination result output by the preceding S330. The range value discrimination result is a discrimination record organized by local velocity group number, containing at least the range value order corresponding to the local velocity group, the sensor number within the group, and the sampling time marker within the group. The adaptive factor coefficient allocation process refers to assigning coefficient records of different sizes to the navigation coordinate system velocity results participating in the comprehensive velocity calculation of this sampling period based on the sequential position of each local velocity group in the range value discrimination result; the adaptive factor coefficient refers to the set of coefficients that corresponds one-to-one with the navigation coordinate system velocity results, which is subsequently called by S420.

[0137] In practice, the coefficient allocation unit first reads the range value discrimination result and performs layered processing on all local velocity groups according to the range value order. For local velocity groups that are earlier in the order, the sensor number within the group is extracted, and the navigation coordinate system velocity result corresponding to the sensor number within the group is marked as a priority to participate in the recording; for local velocity groups that are later in the order, the corresponding navigation coordinate system velocity result is marked as a subsequent participation in the recording.

[0138] The specific values ​​of the adaptive factor coefficients are determined based on the order of the range values. Let the number of valid MEMS inertial sensors in the current sampling period be... After the sliding window is divided, the result is The local velocity data, sorted in ascending order by range, are denoted as follows: The adaptive factor coefficients assigned to each group of data. Calculate using the following formula:

[0139]

[0140] in, This allocation method ensures that the local velocity group with the smallest range (i.e., the group with the highest local consistency) receives the largest coefficient. The second smallest coefficient is the second largest. The remaining groups receive equal small coefficients, thus achieving adaptive weighting.

[0141] Subsequently, the coefficient allocation unit completes the writing of adaptive factor coefficients based on the priority participation records and the subsequent participation records. This results in navigation coordinate system velocity results in local velocity groups with higher local consistency having larger adaptive factor coefficients, while navigation coordinate system velocity results in local velocity groups with lower local consistency have smaller adaptive factor coefficients. Furthermore, when the same navigation coordinate system velocity result appears in multiple local velocity groups simultaneously, the coefficient allocation unit performs cumulative discrimination according to the sequential position of the local velocity group in which the navigation coordinate system velocity result is located, and then generates the final adaptive factor coefficient for that navigation coordinate system velocity result. When a navigation coordinate system velocity result only appears in a local velocity group with an anomaly discrimination marker, the navigation coordinate system velocity result is first written to the low-priority recording area, and then participates in coefficient merging processing at the end of the current sampling period.

[0142] Understandably, the minimum set of adaptive factor coefficients includes sensor serial numbers, sampling time markers, and coefficient size records. The coefficient size records are essential inputs for the S420's comprehensive velocity calculation. Sensor serial numbers and sampling time markers are used to accurately map the coefficient results back to the corresponding navigation coordinate system velocity results. In specific engineering implementations, when the mobile robot traverses uneven surfaces, the coefficient allocation unit first reads the range discrimination result of the current sampling period, then marks the navigation coordinate system velocity results corresponding to the local velocity groups with smaller range values ​​as priority recordings. Subsequently, the adaptive factor coefficients are written, and the coefficient results are written to the coefficient buffer. After processing in this section, the output field is named "Adaptive Factor Coefficient." This output field is directly used by the S420's "Adaptive Factor Coefficient" function. Simultaneously, this output field maintains a sensor serial number correspondence with the preceding "Navigation Coordinate System Velocity Results," thus forming a complete input mapping chain within this main step.

[0143] S420. Based on the adaptive factor coefficients and the velocity results of the navigation coordinate system, perform comprehensive velocity calculation processing to generate a comprehensive velocity.

[0144] Specifically, the input sources for this section include the adaptive factor coefficients output by S410 and the navigation coordinate system velocity results output and continuously retained by S230. The comprehensive velocity calculation process refers to pairing each navigation coordinate system velocity result with its corresponding adaptive factor coefficient according to the sensor number correspondence, and then generating the comprehensive velocity within the same sampling period. The comprehensive velocity refers to a unified velocity record used for subsequent position and attitude updates. In specific implementation, the comprehensive calculation unit first performs bidirectional matching on the adaptive factor coefficients and the navigation coordinate system velocity results. The bidirectional matching includes sensor number matching and sampling time matching. Sensor number matching is used to find the navigation coordinate system velocity result corresponding to each adaptive factor coefficient, and sampling time matching is used to confirm that both come from the same sampling period.

[0145] The specific calculation method for the comprehensive velocity is as follows: The local velocity group data is sorted in ascending order of the range values. Multiply by the corresponding adaptive factor coefficient respectively The average speed is calculated by averaging the values ​​with a window step size of 3 and then summing the results to obtain the final overall speed. :

[0146]

[0147] In the formula, For the first A sliding window of data sorted in ascending order of range. The window step size (i.e., each window contains 3 speed records). This represents the total number of MEMS inertial sensors. The adaptive factor coefficients assigned in step S410 satisfy the following conditions: This calculation is performed once per navigation solution cycle, outputting the overall speed. Used for subsequent position and attitude updates.

[0148] After matching is completed, the integrated calculation unit reads the velocity results of each navigation coordinate system and their corresponding adaptive factor coefficients in sequence according to the valid participation records of the current sampling period, completes the integrated velocity calculation process, and writes the unified velocity record output in the current sampling period into the integrated velocity buffer.

[0149] Furthermore, when an invalidity marker appears in the velocity result of a certain navigation coordinate system, or when a missing record exists in its corresponding adaptive factor coefficient, the comprehensive calculation unit first transfers the record to the delayed processing area, and then completes the comprehensive velocity generation for the current sampling period for the remaining valid records. When the records in the delayed processing area return to normal in subsequent sampling periods, they re-enter the comprehensive velocity calculation chain. The comprehensive velocity buffer here, in addition to writing the comprehensive velocity of the current sampling period, can also synchronously retain the comprehensive velocity record of the previous sampling period for continuous use in subsequent position and attitude updates. Understandably, the minimum set of comprehensive velocities includes sampling time markers and comprehensive velocity records, both of which are required inputs of S430; sensor participation records and anomaly handling records are preferred extensions used for operation log storage and review / traceability.

[0150] In a directly implementable engineering scenario, during the continuous curve operation phase of the mobile robot, the integrated computing unit reads the adaptive factor coefficients obtained by mapping the range discrimination results, then reads the corresponding navigation coordinate system velocity results according to the sensor serial number, generates the integrated velocity for the current sampling period, and writes this integrated velocity into the integrated velocity buffer for subsequent updates. After processing in this section, the output field is named "Integrated Velocity". This output field is directly used by the S430's "Integrated Velocity" function. At the same time, this output field is also the common input for position and attitude updates within the current sampling period, thus connecting the preceding local velocity group discrimination chain with the subsequent navigation closed-loop update chain.

[0151] S430. Based on the comprehensive velocity, perform position and attitude update processing to generate update results.

[0152] Specifically, this section corresponds to the final execution segment of the main step "Adaptive Factor Coefficient Determination and Comprehensive Velocity Update," with the input source being the comprehensive velocity output from S420. The position and attitude update processing refers to continuously updating the position and attitude records in the navigation state by using the comprehensive velocity as a unified velocity input within the current sampling period; the update result refers to the navigation state record formed after the position and attitude updates are completed in the current sampling period. In specific implementation, the update unit first reads the comprehensive velocity of the current sampling period from the comprehensive velocity buffer, and then reads the navigation state record retained from the previous sampling period. The navigation state record includes at least a position record, an attitude record, and a sampling time marker.

[0153] The update unit first performs position update processing, continuously updating the position record of the previous sampling period in the current sampling period, then performs attitude update processing, continuously updating the attitude record of the previous sampling period in the current sampling period, and finally merges the two and writes them into the navigation state buffer of the current sampling period to form the update result.

[0154] Furthermore, this section can also set automatic triggering conditions and operating boundaries. The automatic triggering condition refers to the update unit automatically entering position and attitude update processing when the S420 outputs the comprehensive velocity and the current sampling period is fully marked. The operating boundary refers to the update unit first writing an update abort flag and transferring the current sampling period record to the waiting recovery area when the comprehensive velocity is missing, the sampling time is discontinuous, or the navigation state record of the previous sampling period does not exist. The update chain will then resume after the next valid sampling period arrives. For continuous operation scenarios, after the update result is written to the navigation state buffer of the current sampling period, it is also simultaneously written to the starting state area of ​​the next sampling period for continuous use by subsequent navigation coordinate system velocity calculation processing and subsequent sampling period position and attitude update processing.

[0155] Understandably, the minimum set of update results includes the current position record, the current attitude record, and the current sampling time marker. The current position record and current attitude record are direct outputs of this main step, while the current sampling time marker is used to maintain continuous operation in subsequent sampling cycles. In a complete and operable engineering embodiment, when the mobile robot is continuously running in the warehouse aisle, each sampling cycle first generates a comprehensive velocity from the preceding steps, then the update unit continuously updates the position and attitude records according to the current sampling time, outputting the update result for this sampling cycle. This update result is written to the navigation state buffer, and then called again in the next sampling cycle, thus forming a continuously running navigation closed loop. After processing in this section, the output field is named "Update Result." This output field is written to the navigation state buffer as the final output of this main step, providing preceding records for continuous navigation state updates in subsequent sampling cycles, and maintaining a traceable correspondence with the preceding "Navigation Coordinate System Velocity Result," "Range Value Discrimination Result," "Adaptive Factor Coefficient," and "Comprehensive Velocity" throughout the entire method chain.

[0156] In summary, this step does not directly and fixedly weight the velocity results from multiple navigation coordinate systems. Instead, it first assigns adaptive factor coefficients based on the range discrimination results, then generates the composite velocity, and directly sends the composite velocity into the position and attitude update processing. Compared to existing technologies that fuse the first measurement data for unified calculation, or directly weight the attitude results according to variance, this step actually transmits the sequential discrimination results of the local velocity group to the navigation state update chain, making the composite velocity formation process continuously connected with position and attitude updates.

Claims

1. An adaptive error suppression method for array-type MEMS inertial navigation, characterized in that, include: S100: Acquire data from multiple MEMS inertial sensors, perform pitch angle calculation, roll angle calculation, and initial heading setting to obtain the initial alignment result; S200. Based on the initial alignment result, perform attitude angle transformation, accelerometer data projection and navigation coordinate system velocity calculation to generate navigation coordinate system velocity results; S300: Obtain the velocity results of the navigation coordinate system, sort them in descending order to obtain the sequence reconstruction results; based on the sequence reconstruction results, perform a preset length sliding window division process to generate local velocity groups. Based on the local velocity group, the range value is calculated and sorted in ascending order to generate the range value discrimination result. S400: Obtain the range value discrimination result, perform adaptive factor coefficient allocation processing, and obtain adaptive factor coefficients; Based on the adaptive factor coefficients, a comprehensive speed calculation is performed to generate a comprehensive speed. Based on the comprehensive velocity, position and attitude updates are performed to generate update results.

2. The method according to claim 1, characterized in that, The process of acquiring data from multiple MEMS inertial sensors includes: The acquisition of multiple MEMS inertial sensor data includes: based on clock pulse signal control, performing the same sampling period on each MEMS inertial sensor to generate MEMS inertial sensor data with sensor serial number and sampling time mark; When a MEMS inertial sensor fails to return a complete record within the current sampling period, a missing measurement marker is written and the valid time marker of the previous sampling period is retained. When the returned records contain out-of-order channels, restore the sorting order according to the sensor number before writing them into the current sampling period record.

3. The method according to claim 1, characterized in that, The process of calculating pitch angle, roll angle, and initial heading includes: Based on the extraction of the pre-defined time period, continuous records of the stationary phase after startup are extracted from the MEMS inertial sensor data. Staticity checks and accelerometer mean values ​​are then processed to generate the triaxial accelerometer mean values ​​of each MEMS inertial sensor within the pre-defined time period. The staticity check includes judging the fluctuation range of angular velocity measurements from multiple MEMS inertial sensors and the continuous changes in accelerometer measurements. When there are instantaneous jumps or missing measurement markers, the channel is marked as an abnormal channel and excluded during the mean averaging process. The accelerometer mean processing includes grouping by sensor number, verifying the integrity of records, removing abnormal records, and calculating the mean. Based on the triaxial accelerometer mean results, pitch angle and roll angle are calculated by sensor number, and a unified preset initial heading is written to each MEMS inertial sensor to generate an initial alignment result. A consistency check is performed on the initial alignment results. When the initial attitude record of a certain MEMS inertial sensor deviates significantly from the other records, an abnormal attitude marker is written.

4. The method according to claim 1, characterized in that, The process of projecting the accelerometer data includes: The accelerometer data projection processing includes: performing sequence number matching and time matching based on the attitude transfer matrix and the accelerometer measurement values ​​in the MEMS inertial sensor data; performing coordinate system transformation on the accelerometer measurement values ​​of each MEMS inertial sensor; and generating the navigation coordinate system acceleration corresponding to each sensor. When serial number matching failure, time inconsistency, or missing accelerometer measurement value occurs, record the cause of the abnormality and handle it according to the abnormality type: for short-term missing measurement, read the valid accelerometer measurement value of the sensor in the previous sampling period and retain the current sampling time mark; For inconsistent times, discard the late record and use the earliest valid record that completes the matching within the same sampling period; for continuous anomalies, output the navigation coordinate system acceleration masking flag so that it does not participate in the velocity calculation of this sampling period.

5. The method according to claim 1, characterized in that, The process of calculating and processing the velocity in the navigation coordinate system includes: The navigation coordinate system velocity calculation process includes: based on the navigation coordinate system acceleration and the initial alignment result, performing independent recursive calculations according to the sensor number, continuously updating the navigation coordinate system acceleration of the current sampling period with the navigation coordinate system velocity record of the previous sampling period, generating the navigation coordinate system velocity record of each MEMS inertial sensor in the current sampling period, and synchronously writing validity markers; when entering pure inertial calculation for the first time, the navigation coordinate system velocity record of the previous sampling period adopts zero velocity as the starting record; when a MEMS inertial sensor has continuous anomalies, projection failures, or initial attitude anomalies that have not been recovered in the current sampling period, the velocity record of that sensor is marked as invalid.

6. The method according to claim 1, characterized in that, The process of sorting in descending order includes: The descending sorting process includes: reading validity markers from the velocity results of the navigation coordinate system, sending only velocity records with valid validity markers into the sorting buffer, and performing descending sorting according to the size of the velocity records to generate the order reconstruction result, while retaining the sensor number and sampling time marker corresponding to each velocity record; for records with invalid markers or missing measurement markers, they are first written into the abnormal record area and removed from the sorting buffer of the current sampling period.

7. The method according to claim 1, characterized in that, The process of dividing the preset length sliding window includes: The preset length sliding window division process includes: based on the sequential reconstruction result, according to the preset fixed window length and adjacent advancement method, sequentially extracting adjacent velocity records starting from the sorting head to generate multiple local velocity groups. Each local velocity group includes velocity records within the group, sensor serial numbers within the group, and sampling time markers within the group, and synchronously writing the local group serial number, the group's start position, and the end position; when the number of valid velocity records is insufficient to form a complete local velocity group, the division stops and a window insufficiency marker is written; when duplicate sensor serial numbers or conflicting time markers occur within a local velocity group, an abnormal marker is written within the group.

8. The method according to claim 1, characterized in that, The process of calculating the range and sorting the range in ascending order includes: The range calculation and range ascending order sorting process includes: performing an intra-group integrity check on each local velocity group, confirming that the number of records, sensor serial numbers, and sampling time markers within the group meet the requirements, and then calculating the difference between the maximum and minimum velocity records within the group as the range value of that group; for local velocity groups with intra-group anomaly markers, anomaly markers are first written, and then the participation in the generation of valid range values ​​is determined according to the degree of anomaly. When the degree of anomaly reaches a preset threshold, the local velocity group will not participate in the generation of valid range value order results in this sampling period; after completing the range calculation for all local velocity groups, each range value and its corresponding local group serial number and intra-group sensor serial number are sorted in ascending order according to the range value from smallest to largest to generate a range value discrimination result. This result includes the local group serial number, the range value order, and the corresponding intra-group sensor serial number.

9. The method according to claim 1, characterized in that, The adaptive factor coefficient allocation process includes: The adaptive factor coefficient allocation process includes: based on the range discrimination results, performing hierarchical processing on each local velocity group according to the range order; marking the navigation coordinate system velocity results in the local velocity group with smaller range values ​​as priority participation records; marking the navigation coordinate system velocity results in the local velocity group with larger range values ​​as subsequent participation records; and assigning corresponding adaptive factor coefficients to each navigation coordinate system velocity result accordingly, so that priority participation records have larger coefficients and subsequent participation records have smaller coefficients; when the same navigation coordinate system velocity result appears in multiple local velocity groups, the final adaptive factor coefficient is generated after cumulative discrimination according to the order position of the local velocity group in which the velocity result is located; when a certain navigation coordinate system velocity result only appears in the local velocity group with discrimination anomaly markers, it is first written into the low priority record area, and then participates in the coefficient merging process at the end of the current sampling period.

10. The method according to claim 1, characterized in that, The process of calculating and processing the overall speed includes: The comprehensive velocity calculation process includes: based on the adaptive factor coefficients and the velocity results of the navigation coordinate system, performing sensor sequence number matching and sampling time matching; weighting the velocity results of each navigation coordinate system and their corresponding adaptive factor coefficients according to the current sampling period to generate the comprehensive velocity of the current sampling period; when a certain navigation coordinate system velocity result has an invalid mark or its corresponding adaptive factor coefficient is missing, the record is first transferred to the delayed processing area, and then the comprehensive velocity of the current sampling period is generated for the remaining valid records.

Images