A trajectory resolving method and system based on MEMS inertial navigation

Abstract

The application discloses a trajectory resolving method and system based on MEMS inertial navigation, which comprises the following steps: converting angle from degree system into radian system; calculating the projection components of gravity on three axes of a carrier and compensating the same according to attitude angle, and separating the real movement acceleration of the carrier relative to the ground; converting body axis acceleration to carrier horizontal coordinate system based on roll angle and pitch angle, and decomposing horizontal acceleration to geographical east-north-sky coordinate system in combination with heading angle; adopting a second-order integral recursive algorithm containing acceleration quadratic term, and sequentially iteratively obtaining three-way velocity and three-way displacement sequence of the carrier, and drawing a movement trajectory. The application has low cost, can effectively inhibit the accumulation of short-time integral error of MEMS inertial navigation, and can output a short-time trajectory basically consistent with the actual movement form under the condition of no GNSS. The application is suitable for real-time trajectory resolving of small movement carriers such as indoor AGV, small mobile robots, wearable devices and micro air vehicles under the condition of no external auxiliary positioning.

Description

Technical Field

[0001] This invention relates to the field of inertial navigation and motion vehicle trajectory measurement technology, and in particular to a trajectory calculation method and system based on MEMS inertial navigation. Background Technology

[0002] An Inertial Navigation System (INS) is a fully autonomous navigation system that does not rely on any external signals and can independently output motion parameters such as attitude, velocity, and position of the vehicle. Traditional INS systems typically employ high-precision inertial devices such as fiber optic gyroscopes and laser gyroscopes, and require external auxiliary equipment such as Doppler Velocity Logs (DVL), odometers, or Global Navigation Satellite Systems (GNSS) to calculate the vehicle's position and trajectory. However, the hardware costs of these high-precision inertial devices and their supporting equipment are extremely high, and the systems are bulky and consume a lot of power, making it difficult to promote their application on miniaturized, low-cost mobile vehicles.

[0003] In recent years, with the rapid development of Micro-Electro-Mechanical Systems (MEMS) technology, low-cost MEMS Inertial Measurement Units (MEMS-IMUs) have been widely integrated into various small moving vehicles, such as Automated Guided Vehicles (AGVs), small mobile robots, wearable sports devices, and micro-aircraft, due to their significant advantages such as small size, light weight, extremely low power consumption, and low price. However, MEMS inertial navigation devices themselves have inherent technical defects, mainly manifested in problems such as high measurement noise, poor zero-bias stability, and severe long-term drift. Therefore, relying solely on MEMS inertial navigation for long-term, high-precision trajectory calculation will face significant technical challenges.

[0004] Currently, most trajectory calculation methods based on MEMS inertial navigation employ a simple first-order integral recursive algorithm (i.e., recursively calculating the displacement of the vehicle according to the formula "current position = previous position + velocity × sampling time interval"). This algorithm assumes that the acceleration remains constant within each sampling interval, ignoring the continuous changes in acceleration during actual motion, leading to a rapid accumulation of integral errors as the motion time increases. As a result, the accuracy of the calculated trajectory drops sharply, falling far short of the basic positioning accuracy requirements of practical applications.

[0005] On the other hand, while some existing technologies can improve trajectory calculation accuracy, these methods generally rely on multi-sensor combinations (such as fiber optic inertial navigation, DVL, GNSS, etc.) or external auxiliary signals (such as ultra-wideband UWB positioning signals) to periodically correct the errors of MEMS inertial navigation. These solutions not only significantly increase the hardware cost and deployment complexity of the system, but also violate the core advantages of inertial navigation—"autonomy and passivity"—making them difficult to promote and use in low-cost, purely passive application scenarios.

[0006] In summary, how to effectively suppress the accumulation of integral errors and significantly improve the accuracy of short-time trajectory calculation under the harsh conditions of relying solely on a single low-cost MEMS inertial navigation sensor has become a key technical problem that urgently needs to be solved by those skilled in the art. Summary of the Invention

[0007] To address the shortcomings of existing technologies, this invention provides a trajectory calculation method and system based on MEMS inertial navigation.

[0008] To achieve the above-mentioned objectives, the technical solution adopted by the present invention is as follows:

[0009] A trajectory calculation method based on MEMS inertial navigation includes the following steps:

[0010] Step 1: Obtain the carrier's three-axis attitude angles and original three-axis accelerations in real time from the MEMS inertial navigation system, and convert the three-axis attitude angles from degrees to radians;

[0011] Step 2: Calculate the projection components of gravity on the three axes of the carrier's body coordinate system based on the converted attitude angle, and remove the projection components from the original three-axis acceleration to obtain the true body axis motion acceleration of the carrier relative to the ground;

[0012] Step 3: Based on the roll angle and pitch angle, transform the actual body axis motion acceleration from the carrier body coordinate system to the carrier horizontal coordinate system to obtain the triaxial acceleration in the carrier horizontal coordinate system;

[0013] Step 4: Based on the heading angle, decompose the three-axis acceleration of the carrier in the horizontal coordinate system into the geographic East-North-Sky coordinate system to obtain the three-axis acceleration in the geographic coordinate system;

[0014] Step 5: Perform discrete second-order integral iteration to solve the triaxial acceleration in the geographic coordinate system to obtain the velocity sequence and displacement sequence of the carrier in the geographic coordinate system, and draw the motion trajectory based on the displacement sequence.

[0015] Furthermore, in step 2, the projection components of gravity on the three axes of the carrier's body coordinate system are calculated using the following formula:

[0016]

[0017] in, is the gravitational acceleration constant, with units of meters per second squared. The roll angle is converted to radians, and the unit is radians. The pitch angle is converted to radians, and the unit is radians. , , These are the projected components of gravity on the X, Y, and Z axes of the carrier's body coordinate system, respectively, in meters per second squared.

[0018] The acceleration of the actual body axis motion is:

[0019]

[0020] in, , , The original three-axis acceleration output by the MEMS inertial navigation system; , , This refers to the actual body-axis acceleration of the carrier relative to the ground after removing the gravitational component.

[0021] Furthermore, in step 3, the transformation relationship from the carrier body coordinate system to the carrier horizontal coordinate system is as follows:

[0022]

[0023]

[0024]

[0025] in, The roll angle is the angle converted from radians. The pitch angle after conversion in radians; , , These represent the forward, rightward, and vertical accelerations of the carrier in the horizontal coordinate system, respectively, in meters per second squared.

[0026] Furthermore, in step 4, the decomposition relationship between the carrier's horizontal coordinate system and the geographic east-north-sky coordinate system is as follows:

[0027]

[0028] in, The heading angle after conversion in radians; , , These represent the eastward, northward, and celestial accelerations of the carrier in the geographic coordinate system, respectively.

[0029] Furthermore, in step 5, the discrete second-order integral iterative solution includes the recursive derivation of the first-order velocity integral and the recursive derivation of the second-order displacement integral, wherein:

[0030] The recursive formula for the first-order integral of velocity is:

[0031]

[0032] The recursive formula for the second-order integral of displacement is:

[0033]

[0034] in, The sampling time interval, The sampling point number, ; , , The first Eastward, northward, and celestial accelerations in the geographic coordinate system at each sampling time; , , The first The eastward, northward, and celestial velocities in the geographic coordinate system at each sampling time; , , The first The eastward, northward, and celestial velocities in the geographic coordinate system at each sampling time; , , The first The eastward, northward, and celestial displacements of the geographic coordinate system at each sampling time, in meters; , , The first The eastward, northward, and celestial displacements of the geographic coordinate system at each sampling time;

[0035] Initial conditions are .

[0036] This invention also discloses a trajectory calculation system based on MEMS inertial navigation, used to execute the aforementioned trajectory calculation method based on MEMS inertial navigation, specifically including:

[0037] The data acquisition module is used to acquire the carrier's three-axis attitude angles and three-axis raw acceleration in real time;

[0038] The central processing module is communicatively connected to the data acquisition module. It is used to receive the three-axis attitude angle and the three-axis raw acceleration, and to perform the calculations in steps 1 to 5, and output the velocity sequence and displacement sequence of the carrier.

[0039] A storage module, connected to the central processing module, is used to store raw data, intermediate calculation results, and final trajectory data;

[0040] The display and interaction module is connected to the central processing module and is used to display trajectory graphics and status information in real time, and to receive user operation commands.

[0041] Furthermore, the central processing module includes:

[0042] Angle unit conversion unit, used to convert three-axis attitude angles from degrees to radians;

[0043] The gravity compensation unit is used to calculate the gravity projection component based on the attitude angle and remove it from the original acceleration to obtain the true body axis motion acceleration.

[0044] The coordinate transformation unit is used to perform two coordinate transformations in sequence: from the carrier body coordinate system to the carrier horizontal coordinate system, and from the carrier horizontal coordinate system to the geographic east-north-sky coordinate system.

[0045] The integral calculation unit is used to iteratively calculate the velocity sequence and displacement sequence using a discrete second-order integral recursive algorithm with corrections for quadratic acceleration terms.

[0046] The trajectory generation unit is used to generate motion trajectory curves based on the displacement sequence.

[0047] Furthermore, the data acquisition module is a MEMS inertial measurement unit, which integrates a three-axis gyroscope, a three-axis accelerometer, and a three-axis magnetometer, with a sampling frequency of 10Hz to 100Hz.

[0048] Furthermore, the central processing module is an embedded microprocessor, a digital signal processor, a field-programmable gate array, or an industrial control computer.

[0049] Furthermore, the trajectory calculation system based on MEMS inertial navigation also includes a power management module, which supplies power to the system and has a low-power management mode, causing the system to enter a sleep mode when the carrier remains stationary for more than a preset threshold.

[0050] Compared with the prior art, the advantages of the present invention are as follows:

[0051] 1. This invention achieves short-time, high-precision trajectory calculation using only a single, low-cost MEMS inertial measurement unit, eliminating the need for high-precision inertial devices such as fiber optic gyroscopes and laser gyroscopes, as well as high-cost auxiliary equipment such as Doppler odometers, odometers, and satellite positioning receivers. The system hardware cost is significantly reduced, the structure is compact, and deployment is convenient, making it particularly suitable for cost-sensitive miniaturized motion vehicles.

[0052] 2. This invention does not rely on satellite navigation signals, base station signals, or any other external positioning signals. It can independently complete short-time trajectory calculations in indoor environments without GNSS signal coverage, signal rejection areas, or scenarios with severe electromagnetic interference. This characteristic gives this invention unique application advantages in typical signal-free environments such as tunnels, underground parking garages, mines, and inside buildings.

[0053] 3. This invention sequentially completes the step-by-step acceleration transformation from "carrier coordinate system → carrier horizontal coordinate system → geographic east-north-sky (ENU) coordinate system," and the final output trajectory result directly adopts the standard geographic coordinate system specification. This allows the solution results to be easily compared, integrated, or further processed with satellite positioning data, electronic maps, geographic information systems, etc., exhibiting good compatibility and versatility.

[0054] 4. This invention introduces a correction for the quadratic term of acceleration in the displacement calculation stage and adopts a discrete second-order integral recursive algorithm instead of the traditional first-order integral recursive algorithm. This improvement makes the integration process closer to the real kinematic laws, which can significantly reduce the integration error caused by ignoring acceleration changes, thereby effectively suppressing the position drift of MEMS inertial navigation during short-term motion and significantly improving the accuracy of trajectory calculation.

[0055] 5. The method proposed in this invention has clear steps and moderate computational load. Each step involves simple algebraic operations and matrix multiplications, without the need for complex filtering iterations or large-scale matrix inversions. This allows the algorithm to be easily ported to resource-constrained hardware platforms such as embedded microprocessors, digital signal processors, or field-programmable gate arrays, enabling real-time trajectory calculation of carrier motion. Attached Figure Description

[0056] To more clearly illustrate the technical solutions of the embodiments of the present invention, the drawings used in the description of the embodiments of the present invention will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0057] Figure 1 This is a flowchart of the trajectory calculation method based on MEMS inertial navigation in an embodiment of the present invention;

[0058] Figure 2 This is a schematic diagram of the trajectory obtained by the trajectory calculation method based on MEMS inertial navigation in an embodiment of the present invention;

[0059] Figure 3 This is a satellite map recording the actual motion trajectory of the carrier in this embodiment of the invention. Detailed Implementation

[0060] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some, not all, of the embodiments of the present invention. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0061] Example 1

[0062] like Figure 1 As shown, this embodiment aims to illustrate in detail the specific implementation process of a trajectory estimation method based on MEMS inertial navigation provided by the present invention. This embodiment uses a commercially available, low-cost nine-DOF MEMS inertial measurement unit (integrating a three-axis gyroscope, a three-axis accelerometer, and a three-axis magnetometer), and fixes it on a small motion carrier. This carrier performs actual motion for approximately 150 seconds on a flat road surface between buildings in a certain industrial park, with motion modes including straight-line driving, turning, and other typical maneuvers. The sampling frequency of the MEMS inertial navigation is set to 10Hz, and approximately 1500 sets of raw data samples are collected and saved as test data files.

[0063] To verify the effectiveness of this method, the actual movement path of the carrier was synchronously recorded on the satellite map using an independent, high-precision differential GPS positioning device within the same time period, which served as the comparison benchmark for this embodiment.

[0064] The core of this invention lies in relying solely on the real-time output of three-axis attitude angles (roll, pitch, and yaw) and three-axis accelerations from this low-cost MEMS inertial navigation system. , , Through a complete coordinate transformation and improved integration algorithm, the velocity and position of the vehicle in the navigation coordinate system are calculated. The specific steps are as follows:

[0065] Step 1: Conversion of attitude angle units.

[0066] MEMS inertial navigation systems typically output roll, pitch, and yaw angles in degrees (°). Since all subsequent trigonometric function and coordinate matrix operations require input in radians, unit conversion is necessary. The conversion formula is as follows:

[0067]

[0068] in, Pi is the mathematical constant for a circle. The resulting radian value is uniformly denoted as π in this embodiment. , , And participate in all subsequent calculations.

[0069] Step 2: Remove the gravitational component from the original acceleration.

[0070] The physical quantity actually measured by MEMS accelerometers is "specific force," which is the vector sum of the vehicle's acceleration relative to inertial space and the acceleration due to gravity in the opposite direction. To obtain the vehicle's true acceleration relative to the ground, the influence of gravitational acceleration must be removed from the original measurement. Specifically, based on the current attitude angle, the projection components of the gravity vector on the vehicle's body coordinate system (usually defined as front-right-down) are calculated, and then subtracted from the original measurement.

[0071] The formulas for calculating the projected components of gravity on the three axes of the carrier's body coordinate system are as follows:

[0072]

[0073] in, For the local gravitational acceleration constant (e.g., can be taken as...), ).

[0074] After gravity compensation, the acceleration of the carrier relative to the ground along its true body axis is:

[0075]

[0076] in, , , This is the raw acceleration measurement output by the MEMS inertial navigation system.

[0077] Step 3: Acceleration transformation from the carrier body coordinate system to the carrier horizontal coordinate system.

[0078] To eliminate the influence of vehicle pitch and roll attitude changes on acceleration measurements, the actual motion acceleration in the body coordinate system needs to be transformed into a coordinate system parallel to the local horizontal plane—the vehicle's horizontal coordinate system (front-right-down). This transformation depends only on the roll and pitch angles. The transformation relationship is as follows:

[0079]

[0080]

[0081]

[0082] in, The roll angle is converted to radians, and the unit is radians. The pitch angle is converted to radians, and the unit is radians. , , The actual body axis motion acceleration obtained in step 2 is expressed in meters per second squared. , , These represent the forward, rightward, and vertical accelerations of the carrier in the horizontal coordinate system, respectively, in meters per second squared.

[0083] Step 4: Acceleration decomposition from the carrier horizontal coordinate system to the geographic ENU coordinate system.

[0084] After completing the transformation to the horizontal coordinate system, it is also necessary to combine it with the heading angle ( ), the acceleration component on the horizontal plane ( , This is further decomposed into the geographic East-North-Sky (ENU) coordinate system. This is equivalent to rotating by a heading angle about the vertical axis. The decomposition formula is as follows:

[0085]

[0086] in, The heading angle is converted to radians, and the unit is radians. , , The triaxial acceleration of the carrier in the horizontal coordinate system obtained in step 3 is... , , These represent the eastward, northward, and celestial accelerations of the carrier in the geographic coordinate system, respectively.

[0087] Step 5: Solve for velocity and displacement using discrete second-order integral iteration.

[0088] Thus, the three-axis acceleration time series in the geographic coordinate system is obtained. Next, the velocity and displacement are solved using numerical integration methods. To improve accuracy, this invention employs a discrete second-order integral recursive algorithm that includes a quadratic acceleration term correction.

[0089] Let the sampling time interval be . (In this embodiment) The total number of sampling points is Iteration variables from Take to .

[0090] The recurrence relation for the first-order integral of velocity is:

[0091]

[0092] The recursive formula for the second integral of displacement (including correction for the quadratic term of acceleration) is as follows:

[0093]

[0094] The initial conditions are set as follows:

[0095]

[0096] Through the above iterative calculations, the velocity sequence of the carrier in the east, north, and sky directions during the entire motion can be obtained. , , ) and displacement sequence ( , , ).

[0097] Verification results:

[0098] The test data was processed according to the steps described above, and the results are as follows: Figure 2 As shown. Analysis Figure 2 It can be seen that:

[0099] 1. Acceleration curve: Eastward ( ) and north ( The true acceleration () , The acceleration, constant speed, and deceleration processes of the carrier are clearly reflected, and gravity bias has been eliminated. (Tianxiang) acceleration ( Keeping it near zero is consistent with the actual situation of the carrier moving on the horizontal plane.

[0100] 2. Velocity curves: Eastward and northward velocities ( , The velocity essentially returns to zero at the beginning and end of the motion, while the peak velocity in the middle section matches the actual velocity of the carrier.

[0101] 3. Displacement curve: Eastward displacement ( ) and northward displacement ( The range of variation of the axial displacement is consistent with the actual range of motion. It remains near zero.

[0102] by For the horizontal axis, The horizontal plane motion trajectory plotted on the vertical axis ( Figure 2 The area marked in red in the middle appears as a closed loop curve. Compare this calculated trajectory with... Figure 3 Comparing the actual trajectories shown on the satellite maps reveals a high degree of similarity in shape, direction, start and end points, and closed-loop direction.

[0103] This comparative result strongly demonstrates that the method of the present invention, relying solely on low-cost MEMS inertial navigation and without any external auxiliary positioning means, can output trajectory results that are basically consistent with the actual motion pattern during a short motion process of about 150 seconds, effectively suppressing the accumulation of integral error.

[0104] Scope and limitations:

[0105] This method is particularly suitable for real-time trajectory estimation of small mobile vehicles such as indoor AGVs, warehouse mobile robots, inspection robots, wearable sports devices, and micro-aircraft in short-term (hundreds of seconds) and short-distance movement scenarios. It should be noted that as the movement time continues to increase, the zero-bias drift and noise of MEMS devices will still cause the integration error to gradually accumulate. For longer-term applications, auxiliary algorithms such as online zero-bias estimation, zero-rate update (ZUPT), or non-integrity constraints (NHC) can be introduced for compensation.

[0106] It should be understood that the sequence number of each step in the above embodiments does not imply the order of execution. The execution order of each process should be determined by its function and internal logic, and should not constitute any limitation on the implementation process of the embodiments of the present invention.

[0107] Example 2

[0108] This embodiment provides a trajectory calculation system based on MEMS inertial navigation, which is used to execute the trajectory calculation method based on MEMS inertial navigation as described in Embodiment 1. The system includes the following functional modules:

[0109] (a) Data Acquisition Module

[0110] The data acquisition module is a low-cost MEMS inertial measurement unit (MEMS-IMU) that integrates a three-axis gyroscope, a three-axis accelerometer, and a three-axis magnetometer. This module acquires the carrier's three-axis angular velocity, three-axis acceleration, and geomagnetic field strength data in real time at a fixed sampling frequency (e.g., 10Hz–100Hz), and outputs the carrier's three-axis attitude angles (roll, pitch, and yaw) and raw three-axis acceleration in real time using a built-in attitude calculation algorithm (such as complementary filtering or Kalman filtering). , , The data acquisition module connects to the central processing module via a wired interface (such as SPI, I2C, UART) or wireless communication, and transmits the acquired raw data to the central processing module.

[0111] (ii) Central Processing Module

[0112] The central processing module is the core computing unit and can be implemented using an embedded microprocessor (MCU), digital signal processor (DSP), field-programmable gate array (FPGA), or industrial control computer (ICC). This module internally contains the software program for the trajectory calculation algorithm described in Example 1, which includes at least the following sub-functional units:

[0113] 1. Angle unit conversion unit: Used to receive the three-axis attitude angles from the data acquisition module and convert them according to the formula. Convert it from degrees to radians.

[0114] 2. Gravity compensation unit: It is used to calculate the projection components of gravity on the three axes of the carrier's body coordinate system based on the converted attitude angle, and subtract this component from the original acceleration to obtain the true body axis motion acceleration of the carrier relative to the ground.

[0115] 3. Coordinate Transformation Unit: This unit performs two coordinate transformations sequentially: "Carrier Coordinate System → Carrier Horizon Coordinate System" and "Carrier Horizon Coordinate System → Geographic END Coordinate System", ultimately outputting a three-axis acceleration sequence in the geographic coordinate system.

[0116] 4. Integral Solving Unit: Used to iteratively calculate the three-dimensional velocity sequence and three-dimensional displacement sequence of the carrier based on the acceleration sequence in the geographic coordinate system, using a discrete second-order integral recursive algorithm with correction for quadratic acceleration terms.

[0117] 5. Trajectory Generation Unit: Used to draw the trajectory curve of the carrier in the horizontal plane with the eastward displacement as the horizontal axis and the northward displacement as the vertical axis, and output the trajectory data in graphic or text format.

[0118] (iii) Storage Module

[0119] The storage module is a non-volatile memory (such as Flash, SD card, or solid-state drive) used to store the raw data output by the data acquisition module, intermediate variables during the calculation process of the central processing module (such as acceleration, velocity, and displacement sequences in various coordinate systems), and the final trajectory results. The storage module also pre-stores parameters required for system operation, including but not limited to the gravitational acceleration constant. Sampling time interval Initial position and initial velocity, etc.

[0120] (iv) Display and Interaction Module

[0121] The display and interaction module is a touch screen or a communication interface (such as USB, Ethernet, or Wi-Fi) connected to a host computer. This module is used to display the carrier's current position coordinates, motion trajectory graph, and key status information (such as attitude angle and velocity) in real time. Users can use this module to set system parameters (such as sampling frequency and initial position), start / stop trajectory calculation tasks, and export historical trajectory data.

[0122] (v) Power Management Module

[0123] The power management module provides a stable operating voltage for the entire system and features a low-power management mode. When the carrier remains stationary for more than a preset threshold (e.g., 30 seconds), the system automatically enters sleep mode to save power; when carrier movement is detected, the system quickly wakes up and resumes trajectory calculation.

[0124] System workflow:

[0125] 1. Upon system power-on initialization, the central processing module reads the configuration parameters from the storage module.

[0126] 2. The data acquisition module starts acquiring MEMS inertial navigation data at a preset frequency and sends the attitude angle and acceleration data to the central processing module in real time.

[0127] 3. The central processing module sequentially calls the angle unit conversion unit, gravity compensation unit, coordinate transformation unit, and integral calculation unit to process the data at each sampling moment frame by frame and update the carrier's speed and position in real time.

[0128] 4. The trajectory generation unit dynamically refreshes the trajectory graphics on the display and interaction module based on the accumulated location data.

[0129] 5. When the exercise ends or the user issues a stop command, the central processing module writes all the data from this calculation into the storage module for subsequent playback and analysis.

[0130] This system is compact and low-cost, and can achieve high-precision trajectory calculation in a short time by relying on only a single MEMS inertial navigation sensor. It is particularly suitable for autonomous navigation of small moving vehicles such as indoor AGVs, warehouse robots, and wearable positioning devices in environments without external positioning signals.

[0131] Those skilled in the art will understand that all or part of the processes in the methods of the above embodiments can be implemented by a computer program instructing related hardware. This computer program can be stored in a non-volatile computer-readable storage medium. When executed, the computer program can include the processes of the embodiments of the above methods. Any references to memory, storage, databases, or other media used in the embodiments provided in this application can include non-volatile and / or volatile memory. Non-volatile memory can include read-only memory (ROM), programmable ROM (PROM), electrically programmable ROM (EPROM), electrically erasable programmable ROM (EEPROM), or flash memory. Volatile memory can include random access memory (RAM) or external cache memory. By way of illustration and not limitation, RAM is available in various forms, such as static RAM (SRAM), dynamic RAM (DRAM), synchronous DRAM (SDRAM), dual data rate SDRAM (DDRSDRAM), enhanced SDRAM (ESDRAM), synchronous link DRAM (SLDRAM), Rambus direct RAM (RDRAM), direct memory bus dynamic RAM (DRDRAM), and memory bus dynamic RAM (RDRAM), etc.

[0132] Those skilled in the art will clearly understand that, for the sake of convenience and brevity, the above-described division of functional units and modules is used as an example. In practical applications, the above functions can be assigned to different functional units and modules as needed, that is, the internal structure of the device can be divided into different functional units or modules to complete all or part of the functions described above.

[0133] The above-described embodiments are only used to illustrate the technical solutions of the present invention, and are not intended to limit it. Although the present invention has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that modifications can still be made to the technical solutions described in the foregoing embodiments, or equivalent substitutions can be made to some of the technical features. Such modifications or substitutions do not cause the essence of the corresponding technical solutions to deviate from the spirit and scope of the technical solutions of the embodiments of the present invention, and should all be included within the protection scope of the present invention.

Claims

1. A trajectory calculation method based on MEMS inertial navigation, characterized in that, Includes the following steps: Step 1: Obtain the carrier's three-axis attitude angles and original three-axis accelerations in real time from the MEMS inertial navigation system, and convert the three-axis attitude angles from degrees to radians; Step 2: Calculate the projection components of gravity on the three axes of the carrier's body coordinate system based on the converted attitude angle, and remove the projection components from the original three-axis acceleration to obtain the true body axis motion acceleration of the carrier relative to the ground; Step 3: Based on the roll angle and pitch angle, transform the actual body axis motion acceleration from the carrier body coordinate system to the carrier horizontal coordinate system to obtain the triaxial acceleration in the carrier horizontal coordinate system; Step 4: Based on the heading angle, decompose the three-axis acceleration of the carrier in the horizontal coordinate system into the geographic East-North-Sky coordinate system to obtain the three-axis acceleration in the geographic coordinate system; Step 5: Perform discrete second-order integral iteration to solve the triaxial acceleration in the geographic coordinate system to obtain the velocity sequence and displacement sequence of the carrier in the geographic coordinate system, and draw the motion trajectory based on the displacement sequence.

2. The trajectory calculation method based on MEMS inertial navigation according to claim 1, characterized in that, In step 2, the projection components of gravity on the three axes of the carrier's body coordinate system are calculated using the following formula: ， in, is the gravitational acceleration constant, with units of meters per second squared. The roll angle is converted to radians, and the unit is radians. The pitch angle is converted to radians, and the unit is radians. , , These are the projected components of gravity onto the X, Y, and Z axes of the carrier's body coordinate system, respectively, in meters per second squared. The acceleration of the actual body axis motion is: ， in, , , The original three-axis acceleration output by the MEMS inertial navigation system; , , This refers to the actual body-axis acceleration of the carrier relative to the ground after removing the gravitational component.

3. The trajectory calculation method based on MEMS inertial navigation according to claim 2, characterized in that, In step 3, the transformation relationship from the carrier body coordinate system to the carrier horizontal coordinate system is as follows: ， ， ， in, The roll angle is the value converted from radians. The pitch angle after conversion in radians; , , These represent the forward, rightward, and vertical accelerations of the carrier in the horizontal coordinate system, respectively, in meters per second squared.

4. The trajectory calculation method based on MEMS inertial navigation according to claim 3, characterized in that, In step 4, the decomposition relationship from the carrier horizontal coordinate system to the geographic east-north-sky coordinate system is as follows: ， in, The heading angle after conversion in radians; , , These represent the eastward, northward, and celestial accelerations of the carrier in the geographic coordinate system, respectively.

5. The trajectory calculation method based on MEMS inertial navigation according to claim 1, characterized in that, In step 5, the discrete second-order integral iterative solution includes the recursive derivation of the first-order velocity integral and the recursive derivation of the second-order displacement integral, wherein: The recursive formula for the first-order integral of velocity is: ， The recursive formula for the second-order integral of displacement is: ， in, The sampling time interval, The sampling point number, ; , , The first Eastward, northward, and celestial accelerations in the geographic coordinate system at each sampling time; , , The first The eastward, northward, and celestial velocities in the geographic coordinate system at each sampling time; , , The first The eastward, northward, and celestial velocities in the geographic coordinate system at each sampling time; , , The first The eastward, northward, and celestial displacements of the geographic coordinate system at each sampling time, in meters; , , The first The eastward, northward, and celestial displacements of the geographic coordinate system at each sampling time; Initial conditions are .

6. A trajectory calculation system based on MEMS inertial navigation, used to perform the method according to any one of claims 1 to 5, characterized in that, include: The data acquisition module is used to acquire the carrier's three-axis attitude angles and three-axis raw acceleration in real time; The central processing module is communicatively connected to the data acquisition module. It is used to receive the three-axis attitude angle and the three-axis raw acceleration, and to perform the calculations in steps 1 to 5, and output the velocity sequence and displacement sequence of the carrier. A storage module, connected to the central processing module, is used to store raw data, intermediate calculation results, and final trajectory data; The display and interaction module is connected to the central processing module and is used to display trajectory graphics and status information in real time, and to receive user operation commands.

7. The trajectory calculation system based on MEMS inertial navigation according to claim 6, characterized in that, The central processing module includes: Angle unit conversion unit, used to convert three-axis attitude angles from degrees to radians; The gravity compensation unit is used to calculate the gravity projection component based on the attitude angle and remove it from the original acceleration to obtain the true body axis motion acceleration. The coordinate transformation unit is used to perform two coordinate transformations in sequence: from the carrier body coordinate system to the carrier horizontal coordinate system, and from the carrier horizontal coordinate system to the geographic east-north-sky coordinate system. The integral calculation unit is used to iteratively calculate the velocity sequence and displacement sequence using a discrete second-order integral recursive algorithm with corrections for quadratic acceleration terms. The trajectory generation unit is used to generate motion trajectory curves based on the displacement sequence.

8. The trajectory calculation system based on MEMS inertial navigation according to claim 6, characterized in that, The data acquisition module is a MEMS inertial measurement unit, which integrates a three-axis gyroscope, a three-axis accelerometer, and a three-axis magnetometer, with a sampling frequency of 10Hz to 100Hz.

9. The trajectory calculation system based on MEMS inertial navigation according to claim 6, characterized in that, The central processing module is an embedded microprocessor, digital signal processor, field-programmable gate array, or industrial control computer.

10. The trajectory calculation system based on MEMS inertial navigation according to claim 6, characterized in that, It also includes a power management module, which supplies power to the system and has a low-power management mode, which puts the system into sleep mode when the carrier is stationary for more than a preset threshold.

Images