A tilt measurement fast initial alignment method, device and equipment
By combining IMU and GNSS data using Kalman filtering, the problem of low initial alignment accuracy in tilt measurement in existing technologies is solved, achieving high-precision initial alignment and fast convergence, and outputting reliable initial parameters.
Patent Information
- Application Number
- CN202511472699.6
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- Filing Date
- 2025-10-15
- Publication Date
- 2026-11-10
- Estimated Expiration
- 2045-10-15
AI Technical Summary
Existing methods for rapid initial alignment of tilt measurements fail to fully utilize GNSS information during the initial alignment stage, resulting in low alignment accuracy. Furthermore, the lack of systematic modeling and filtering calculations leads to significant gross errors.
The initial attitude is calculated by acquiring IMU observation data from the GNSS receiver, a state vector is constructed and Kalman filtering is performed, and iterative filtering is performed in combination with GNSS positioning data. The filter is reset in each iteration using a preset reset strategy until the convergence condition is met. Finally, a second state vector is constructed and refined filtering is performed to obtain a high-precision initial alignment result.
It improves the alignment accuracy of GNSS receiver tilt measurement, shortens the preparation time for the system to reach a measurable state, and outputs reliable initial values that can be directly used for tilt measurement.
Smart Images

Figure CN121477243B_ABST
Abstract
Description
Technical Field
[0001] This invention relates to the field of attitude alignment technology, and in particular to a method, apparatus and equipment for rapid initial alignment by tilt measurement. Background Technology
[0002] Currently, in order to use RTK (Real-Time Kinematic) to measure scenarios where centering is difficult to achieve in environments such as corners and pipelines, GNSS (Global Navigation Satellite System) or INS (Inertial Navigation System) methods are used to provide the position and attitude of the RTK antenna, and then the position of the target point is calculated by pole length or laser ranging.
[0003] Existing methods for rapid initial alignment in tilt measurements are mainly divided into position (or trajectory) matching and velocity matching methods. Position (or trajectory) matching methods, on the one hand, utilize the principle that INS and GNSS trajectories are similar over a short period to calculate the angle between their projections onto the plane, thus obtaining the initial heading deviation; on the other hand, they constrain the centering rod tip by collecting GNSS positions at different points, constructing a set of position equations, and solving for the initial heading deviation. Velocity matching methods, however, obtain the initial heading deviation by calculating the angle between INS and GNSS velocities. Because they only use either GNSS position or velocity for initial alignment, they fail to fully utilize GNSS information during the initial alignment phase; and they generally only use sampling points at certain times for the solution, without systematic modeling and filtering calculations, resulting in significant gross errors and low alignment accuracy. Summary of the Invention
[0004] This invention provides a method, apparatus, and device for rapid initial alignment of tilt measurement, in order to improve the alignment accuracy of tilt measurement of GNSS receivers.
[0005] To address the aforementioned technical problems, embodiments of the present invention provide a rapid initial alignment method for tilt measurement, comprising: Acquire IMU observation data from the GNSS receiver, and calculate the initial attitude of the GNSS receiver based on the IMU observation data; GNSS positioning data is acquired, and a first state vector is constructed based on the initial attitude, IMU observation data, and GNSS positioning data. The first state vector is iteratively Kalman filtered based on a preset first filter, and the filtering is repeated until the first filter meets a preset first convergence condition to obtain a first attitude estimation result. In each iteration, the real-time motion state of the GNSS receiver is acquired and the first filter is reset according to a preset reset strategy. A second state vector is constructed based on the first attitude estimation result, and Kalman filtering is performed on the second state vector based on a preset second filter until the second filter meets the preset second convergence condition. The state and parameters of the second filter are retained, and the retained state and parameters are used as the initial values for tilt measurement to complete the initial alignment of tilt measurement.
[0006] This invention first acquires IMU observation data from a GNSS receiver and calculates the initial attitude based on this data, quickly obtaining a usable coarse attitude reference, reducing linearization errors in subsequent filtering and shortening the initial convergence time. Second, a first state vector is constructed based on the initial attitude, IMU observation data, and GNSS positioning data, and Kalman filtered using a preset first filter. By jointly fusing inertial and GNSS information, the observability of the first filter for attitude, position, and velocity errors is improved, enhancing the coarse estimation accuracy. Third, in the first filtering stage, real-time motion states are acquired, and the first filter is reset according to a preset reset strategy to repeatedly execute filtering until the first convergence condition is met, enhancing the filter's robustness to dynamic disturbances or sudden motion changes and avoiding estimation offsets caused by transient motion. Finally, a second state vector is constructed based on the first attitude estimation result and refined using a second filter until a second convergence is achieved, retaining its state and parameters as initial values for tilt measurement. This realizes a two-stage estimation process from coarse to fine, improving the alignment accuracy of GNSS receiver tilt measurement and outputting reliable initial values that can be directly used for tilt measurement while ensuring accuracy, shortening the system's preparation time to reach a measurable state.
[0007] Furthermore, the IMU observation data includes accelerometer data and gyroscope data; the acquisition of the GNSS receiver's IMU observation data and GNSS positioning data, and the calculation of the GNSS receiver's initial attitude based on the IMU observation data, includes: Acquire IMU observation data and GNSS positioning data from the GNSS receiver; The accelerometer and gyroscope data are processed by a preset sensor fusion algorithm to obtain the attitude quaternion of the GNSS receiver. The attitude quaternion is converted into Euler angles based on a preset transformation formula, and the initial attitude of the GNSS receiver is generated based on the Euler angles.
[0008] This invention incorporates IMU observation data, including accelerometer and gyroscope data, to ensure coverage of the essential raw quantities required for attitude determination, thus providing a complete source of inertial information to support subsequent fusion. A preset sensor fusion algorithm processes the accelerometer and gyroscope data to obtain attitude quaternions, which can filter out noise and suppress short-term drift while obtaining a numerically stable attitude representation without singularities, thereby improving the reliability of the initial estimate. Furthermore, based on a preset transformation formula, the attitude quaternions are converted into Euler angles to generate the initial attitude, making the attitude representation consistent with commonly used attitude parameters (i.e., Euler angles) in engineering, facilitating direct coupling and interpretation with subsequent algorithms based on Euler angle linearization or monitoring thresholds.
[0009] Further, the acquisition of GNSS positioning data involves constructing a first state vector based on the initial attitude, IMU observation data, and GNSS positioning data; performing iterative Kalman filtering on the first state vector based on a preset first filter, repeatedly executing the filtering until the first filter satisfies a preset first convergence condition, and obtaining a first attitude estimation result; wherein, in each iteration, the real-time motion state of the GNSS receiver is acquired and the first filter is reset according to a preset reset strategy, including: Based on the initial attitude, IMU observation data, and GNSS positioning data, calculate the misalignment angle trigonometric function, eastward velocity error, northward velocity error, eastward position error, and northward position error; A first state vector is constructed based on the aforementioned misalignment angle trigonometric function, eastward velocity error, northward velocity error, eastward position error, and northward position error; Based on a preset first filter and a preset first prediction frequency, perform iterative Kalman filtering on the first state vector to obtain the estimated value and covariance of the first state vector until the estimated value and covariance of the first state vector converge, stop filtering, and obtain the first attitude estimation result of the first filter. In each iteration, the real-time motion state of the GNSS receiver is acquired, and the first filter is reset based on the real-time motion and a preset reset strategy.
[0010] This invention calculates the misalignment angle trigonometric function, east-west velocity error, and position error as state components based on initial attitude, IMU observation data, and GNSS positioning data. This allows the attitude error to be expressed in an approximately linear and tractable trigonometric function form, thereby reducing the filtering linearization error caused by nonlinearity. A first state vector is constructed using the misalignment angle trigonometric function, east-west velocity error, and position error, making the filter state semantically correspond to the actual error term. Then, Kalman filtering is performed on this state vector using a preset first filter and a first prediction frequency to obtain the estimated value and covariance. This can balance the response rate and estimation stability in the time domain, while outputting an uncertainty metric. Simultaneously, real-time motion state is acquired during the filtering process, and the first filter is reset based on a reset strategy, which can prevent the filter from falling into erroneous convergence under static or insufficient dynamic conditions, thus maintaining the reliability of the estimation.
[0011] Furthermore, in each iteration, acquiring the real-time motion state of the GNSS receiver and resetting the first filter based on the real-time motion and a preset reset strategy includes: In each iteration, the real-time motion state of the GNSS receiver is acquired, including the acceleration vector, and the scalar average of the acceleration is calculated based on the acceleration vector. When the average value of the acceleration scalar meets the preset reset conditions, the first filter is reset based on the preset reset strategy.
[0012] In the first filtering stage, the present invention acquires the real-time motion state, which includes an acceleration vector and calculates the scalar average of the acceleration based on the vector. The scalar average of the acceleration is used as a concise quantitative indicator of the current motion intensity, which is helpful in distinguishing between static and maneuvering conditions. When the scalar average of the acceleration meets the preset reset conditions, the first filter is reset based on the preset reset strategy. The threshold-triggered judgment can promptly remove the estimation pollution caused by sudden motion changes, avoiding the weak observability of the attitude state under static or insufficient dynamic conditions. At this time, the measurement update causes the state estimation error covariance matrix to continuously converge.
[0013] Furthermore, when the average value of the acceleration scalar meets a preset reset condition, resetting the first filter based on a preset reset strategy includes: When the average value of the acceleration scalar meets the preset reset condition, the covariance in the first filter is set to a preset threshold, and the first state vector is reinitialized based on the current initial attitude, the current IMU observation data, and the current GNSS positioning data to reset the first filter.
[0014] This invention sets the covariance in the first filter to a preset threshold when the average value of the acceleration scalar meets the reset condition. This artificially increases or resets the estimation uncertainty, thereby enabling the filter to reconverge under subsequent observation and reducing the impact of historical erroneous states. Furthermore, the first state vector is reinitialized based on the current initial attitude, current IMU observation data, and current GNSS positioning data to ensure that the initial conditions of the filter are consistent with the current actual operating conditions. This avoids long-term deviations caused by using outdated states, which helps to accelerate filter recovery and improve the accuracy of the initial estimation.
[0015] Furthermore, the step of constructing a second state vector based on the first attitude estimation result, and performing Kalman filtering on the second state vector based on a preset second filter until the second filter meets a preset second convergence condition, retaining the state and parameters of the second filter, and using the retained state and parameters as initial values for tilt measurement to complete the initial alignment of tilt measurement, includes: Based on the first attitude estimation result, the relative attitude error, planar velocity error, and planar position error are calculated, and a second state vector is constructed based on the relative attitude error, planar velocity error, and planar position error. A second filter is preset based on the first attitude estimation result, and Kalman filtering is performed on the second state vector based on the second filter and the preset second prediction frequency; Until the second filter reaches the preset second condition, the state and parameters of the second filter are retained as the initial value for tilt measurement, and the initial alignment of tilt measurement is completed.
[0016] This invention calculates relative attitude error, planar velocity error, and planar position error based on the first attitude estimation result and constructs a second state vector accordingly. The state of the second stage can be designed to focus more on the high-precision relative error term, thereby achieving targeted correction of the coarse estimation result. By pre-setting a second filter based on the first attitude estimation result, the coarse estimation result of the first stage can be used to provide a good linearization point and initial uncertainty for the second stage, thereby significantly shortening the refinement convergence time and improving the final accuracy. Kalman filtering is applied to the second state vector. When the second filter reaches the preset second convergence condition and retains its state and parameters as initial values for tilt measurement, it can output high-confidence initial parameters for tilt measurement, directly supporting high-precision operation of subsequent measurement and control tasks.
[0017] Furthermore, the step of presetting a second filter based on the first attitude estimation result, and performing Kalman filtering on the second state vector based on the second filter and a preset second prediction frequency, includes: The estimated covariance is calculated based on the first attitude estimation result, and the initial state covariance matrix of the second filter is set based on the estimated covariance. The attitude, velocity, and position sub-matrices in the first filter are copied or mapped to the corresponding sub-matrices of the second filter to preset the second filter. Kalman filtering is performed on the second state vector based on the second filter and the preset second prediction frequency.
[0018] This invention uses the estimated covariance calculated based on the first attitude estimation result as the initial state covariance matrix of the second filter, which helps to retain the quantification information of uncertainty in the first stage, thereby making the filter in the second stage statistically continuous and consistent. Furthermore, by copying or mapping the attitude, velocity, and position submatrices in the first filter to the corresponding submatrices in the second filter to pre-set the second filter, it can directly inherit the converged state estimation and variable correlation, reduce repeated estimation work and restart jitter, thereby accelerating the convergence speed of the second filter and reducing transient errors in the refinement process.
[0019] Furthermore, the step of retaining the state and parameters of the second filter until the second filter reaches a preset second condition, and using this as the initial value for tilt measurement to complete the initial alignment of tilt measurement, includes: In the second filtering stage, the change amplitude of the gyroscope zero bias estimate and the characteristics of the GNSS measurement residuals in the second filter are acquired in real time. When the change amplitude of the gyroscope zero bias estimate and the measurement residual are both less than the corresponding preset threshold and remain stable within a continuous predetermined time period, the second filter is determined to have converged. The state and parameters of the second filter are retained as the initial value for tilt measurement, and the initial alignment of tilt measurement is completed.
[0020] In the second filtering stage, this invention acquires the change amplitude of the gyroscope zero-bias estimate in the second filter and the GNSS measurement residual characteristics in real time, respectively providing quality monitoring of the sensor's internal bias and external measurements. When the change amplitude of the gyroscope zero-bias estimate and the measurement residual are both less than the corresponding preset thresholds and remain stable within a continuous predetermined time period, convergence is determined. By simultaneously satisfying the conditions of bias stability and external measurement consistency, the refinement process can be effectively avoided from prematurely terminating due to transient noise or short-term convergence artifacts, ensuring that the retained states and parameters, when used as initial values for tilt measurement, meet usable standards in terms of accuracy and reliability.
[0021] In a second aspect, the present invention provides a rapid initial alignment device for tilt measurement, comprising: a data acquisition module, a first alignment module, and a second alignment module; The data acquisition module is used to acquire IMU observation data from the GNSS receiver and calculate the initial attitude of the GNSS receiver based on the IMU observation data. The first alignment module is used to acquire GNSS positioning data, construct a first state vector based on the initial attitude, IMU observation data and GNSS positioning data; perform iterative Kalman filtering on the first state vector based on a preset first filter, and repeatedly perform filtering until the first filter meets a preset first convergence condition to obtain a first attitude estimation result; wherein, in each iteration, the real-time motion state of the GNSS receiver is acquired and the first filter is reset according to a preset reset strategy; The second alignment module is used to construct a second state vector based on the first attitude estimation result, and to perform Kalman filtering on the second state vector based on a preset second filter until the second filter meets a preset second convergence condition. The state and parameters of the second filter are retained, and the retained state and parameters are used as the initial values for tilt measurement to complete the initial alignment of tilt measurement.
[0022] Thirdly, the present invention provides a terminal device including a processor, a memory, and a computer program stored in the memory and configured to be executed by the processor, wherein the processor executes the computer program to implement the tilt measurement rapid initial alignment method as described above. Attached Figure Description
[0023] Figure 1 A schematic flowchart of a rapid initial alignment method for tilt measurement provided in an embodiment of the present invention; Figure 2 This is a schematic diagram of another process for rapid initial alignment of tilt measurement provided in an embodiment of the present invention. Detailed Implementation
[0024] The specific embodiments of the present invention will be described in further detail below with reference to the accompanying drawings and examples. The following examples are for illustrative purposes only and are not intended to limit the scope of the invention.
[0025] The terms "first" and "second," etc., in the specification, claims, and drawings of this application are used to distinguish different objects, not to describe a specific order. Furthermore, the terms "comprising" and "having," and any variations thereof, are intended to cover non-exclusive inclusion. For example, a process, method, system, product, or apparatus that includes a series of steps or units is not limited to the listed steps or units, but may optionally include steps or units not listed, or may optionally include other steps or units inherent to such processes, methods, products, or apparatus.
[0026] In this document, the term "embodiment" means that a particular feature, structure, or characteristic described in connection with an embodiment may be included in at least one embodiment of this application. The appearance of this phrase in various places throughout the specification does not necessarily refer to the same embodiment, nor is it a separate or alternative embodiment mutually exclusive with other embodiments. It will be explicitly and implicitly understood by those skilled in the art that the embodiments described herein can be combined with other embodiments.
[0027] Example 1 See Figure 1 , Figure 1 This is a flowchart illustrating a rapid initial alignment method for tilt measurement of a GNSS receiver provided in an embodiment of the present invention. The embodiment of the present invention provides a rapid initial alignment method for tilt measurement of a GNSS receiver, including steps 101 to 103, as detailed below: Step 101: Acquire the IMU observation data of the GNSS receiver, and calculate the initial attitude of the GNSS receiver based on the IMU observation data; In this embodiment, the IMU observation data includes accelerometer data and gyroscope data; the step of acquiring the IMU observation data and GNSS positioning data of the GNSS receiver, and calculating the initial attitude of the GNSS receiver based on the IMU observation data, includes: Acquire IMU observation data and GNSS positioning data from the GNSS receiver; The accelerometer and gyroscope data are processed by a preset sensor fusion algorithm to obtain the attitude quaternion of the GNSS receiver. The attitude quaternion is converted into Euler angles based on a preset transformation formula, and the initial attitude of the GNSS receiver is generated based on the Euler angles.
[0028] In this embodiment, the accelerometer and gyroscope of the IMU undergo routine preprocessing and calibration, including: removing known bias and scale factors, performing time alignment and simple noise reduction on the measurement data, and determining the sampling period. If the device is powered on or stationary, a short-term static test should be performed first to estimate and compensate for the gyroscope bias. The gravity direction is conventionally used as the reference vector, while the instantaneous measurement of the accelerometer is the observation of this vector in the carrier (coordinate system).
[0029] In this embodiment, for each sampling moment, the gyroscope angular velocity after zero bias correction is used as input to calculate the instantaneous rate of change of attitude and make a short-term prediction of the current attitude. It can track the attitude change caused by angular velocity in a short time, but the zero bias will accumulate over time, causing drift. Therefore, external reference correction is required.
[0030] In this embodiment, the difference between the projection of the accelerometer measurement vector and the expected gravity vector in the carrier coordinate system is used as the external observation signal of attitude error. The Madgwick algorithm, based on the gradient descent direction correction method, is employed to directly estimate the quaternion components used to correct the attitude from the accelerometer measurements. This aligns the gyroscope's inertial prediction direction with the gravity direction observation provided by the accelerometer, thereby suppressing gyroscope drift and constraining the attitude solution to the correct gravity reference.
[0031] In this embodiment, the gyroscope prediction rate of change and the acceleration correction term are fused according to preset weights (the Madgwick algorithm adjusts their relative weights through a gain parameter). These two are combined to form the quaternion update at the current moment. Then, the quaternion is numerically integrated to obtain the attitude quaternion at the current moment. Finally, the resulting quaternion is normalized to ensure it is strictly a unit quaternion, avoiding numerical drift. The attitude quaternion is then converted to Euler angles according to a pre-defined conversion rule. The pitch and roll angles in the output are the required attitude components, which can be used as the "initial attitude" or for initialization of the subsequent first filter.
[0032] In this embodiment, the Madgwick algorithm uses IMU data to calculate... The attitude quaternion of time Represented as: (1) in, This represents the rate of change of quaternions estimated using an IMU; The quaternion rate of change, calculated from gyroscope data, is obtained from angular velocity. The divergence rate, representing the angular velocity, can be determined based on the zero bias value of the gyroscope; Let be the sampling interval. Assume... A reference vector in a geographic coordinate system. This is the measurement vector in the carrier coordinate system. The quaternion estimated by the accelerometer is obtained using the gradient descent method. Let be the objective function. express conjugate, The gradient of the cross product of quaternions is: After obtaining the attitude quaternion, convert it to Euler angles and obtain the pitch and roll angles.
[0033] In this embodiment, IMU observation data includes accelerometer data and gyroscope data to ensure that the basic raw quantities required for attitude solution are covered, thereby providing a complete source of inertial information to support subsequent fusion. The accelerometer data and gyroscope data are processed by a preset sensor fusion algorithm to obtain attitude quaternions, which can filter out noise and suppress short-term drift while obtaining a numerically stable attitude representation without singularities, thereby improving the reliability of the initial estimate. Then, the attitude quaternions are converted into Euler angles based on a preset transformation formula to generate the initial attitude, so that the attitude representation is consistent with the attitude parameters commonly used in engineering (i.e., Euler angles), which facilitates direct coupling and interpretation with subsequent algorithms based on Euler angle linearization or monitoring thresholds.
[0034] Step 102: Acquire GNSS positioning data, construct a first state vector based on the initial attitude, IMU observation data and GNSS positioning data; perform iterative Kalman filtering on the first state vector based on a preset first filter, and repeat the filtering until the first filter meets the preset first convergence condition to obtain the first attitude estimation result; wherein, in each iteration, the real-time motion state of the GNSS receiver is acquired and the first filter is reset according to a preset reset strategy. In this embodiment, the acquisition of GNSS positioning data involves constructing a first state vector based on the initial attitude, IMU observation data, and GNSS positioning data; performing iterative Kalman filtering on the first state vector based on a preset first filter, repeatedly executing the filtering until the first filter satisfies a preset first convergence condition, and obtaining a first attitude estimation result; wherein, in each iteration, the real-time motion state of the GNSS receiver is acquired and the first filter is reset according to a preset reset strategy, including: Based on the initial attitude, IMU observation data, and GNSS positioning data, calculate the misalignment angle trigonometric function, eastward velocity error, northward velocity error, eastward position error, and northward position error; A first state vector is constructed based on the aforementioned misalignment angle trigonometric function, eastward velocity error, northward velocity error, eastward position error, and northward position error; Kalman filtering is performed on the first state vector based on a preset first filter and a preset first prediction frequency to obtain the estimated value and covariance of the first state vector. The filtering is stopped when the estimated value and covariance of the first state vector converge, and the first attitude estimation result of the first filter is obtained. In each iteration, the real-time motion state of the GNSS receiver is acquired, and the first filter is reset based on the real-time motion and a preset reset strategy.
[0035] In this embodiment, the two trigonometric functions of the large azimuth misalignment angle, the planar velocity error, and the planar position error are used as the main state variables of the first filter for modeling. Specifically, the two trigonometric function components of the large azimuth misalignment angle are used to linearize the attitude errors related to heading and azimuth, ensuring the model approximates the linear filtering. The eastward and northward velocity errors represent the difference between the inertial navigation system's (INS) calculated velocity and the GNSS velocity, used to correct INS drift through velocity observation. The eastward and northward position errors represent the difference between the INS position and the GNSS position, used for position constraints and corrections.
[0036] In this embodiment, the first state vector is specifically: (2) in, and These represent the major azimuth misalignment angles. trigonometric functions and ; and Different speed errors are considered for eastward and northward directions; and This represents the positional error in latitude and longitude.
[0037] In this embodiment, the horizontal small-angle attitude error, gyroscope measurement details, and some position-related small terms are ignored or weakened, making the heading attitude error approximately linear. This facilitates the construction of linear or near-linear state equations. Since the effects of horizontal attitude error, gyroscope measurement error, and some position-related errors are ignored, the state equations can be constructed based on the first state vector: (3) (4) (5) (6) in, , and These represent pitch angle, roll angle, and yaw angle, respectively. For heading angle The differential; These represent the major azimuth misalignment angles. trigonometric functions and The differential; The derivative of the velocity error in the navigation coordinate system. This indicates the navigation coordinate system. Here, the "Northeast-Sky (ENU)" coordinate system is taken as the reference navigation coordinate system. Included , , East North , Tianxiang The speed error differential, Indicates the northbound velocity error. apparent speed error; , Indicate latitude and longitude The differential of position error; Represents the transformation matrix. Indicates the local horizontal coordinate system. Indicates the inertial navigation coordinate system. This is the transformation matrix from the inertial navigation coordinate system to the local horizontal coordinate system. Represents the navigation coordinate system under ideal conditions The transformation matrix to the local horizontal coordinate system. Indicates the case where there is an error ; Inertial navigation coordinate system The specific force measured by the accelerometer. This indicates the local horizontal coordinate system. The accelerometer specific force below, ; This is the Earth's semi-major axis.
[0038] In this embodiment, the state equation is transformed into matrix form, resulting in the following matrix-form state equation: (7) in, For angle random walk noise, This represents the velocity random walk noise.
[0039] In this embodiment, the Northeastern Sky (ENU) is used as the navigation reference frame, and the position / velocity of the GNSS provides observation constraints under this frame. The uncertainty of angle and velocity is modeled as a random walk or white noise process to express the uncertainty of the system dynamics.
[0040] In this embodiment, the first filter includes a state equation and a measurement equation. The state equation, derived from a first state vector, is used to predict the state. The measurement equation, consisting of constraints based on GNSS position and velocity, is used to achieve loose coupling fusion between the inertial navigation system (INS) and GNSS. Specifically, the state equation, based on the first state vector, performs time-series predictions on state quantities including trigonometric components of large azimuth misalignment angles, planar velocity errors, and planar position errors, reflecting the evolution of the INS under process noise and outputting the predicted state and predicted covariance. The measurement equation, using the position and velocity information provided by GNSS as observation inputs, imposes constraints on the INS prediction values, thereby correcting the state estimation and covariance in the update step.
[0041] In this embodiment, the filter directly incorporates the raw GNSS measurements and the inertial navigation state (i.e., the raw attitude) into the observation equation to enhance observability support in harsh environments. To ensure the robustness of the filter, quality control and time synchronization of GNSS measurements are required during implementation. The observation noise weight is dynamically adjusted, and covariance reset or state reinitialization strategies are triggered under conditions of abnormal observations or low dynamic observability. This ensures that the filter can converge quickly under sufficiently dynamic conditions to correct inertial navigation drift, and avoid false convergence during static or abnormal measurement periods. As a result, it outputs a first attitude estimation result with high confidence, providing reliable initial values for subsequent refined filtering and initial alignment of tilt measurements.
[0042] In this embodiment, the position and velocity of the GNSS are used as measurements to constrain the INS state, thereby constructing the observation equations for the first filter: (8) The observation equation is then transformed to obtain the measurement equation in matrix form: (9) in, These are the latitude, longitude, eastward velocity, and westward velocity calculated by INS, respectively. These are the latitude, longitude, eastward velocity, and westward velocity calculated by GNSS, respectively. These are the observation errors for GNSS latitude, longitude, eastward velocity, and westward velocity, respectively.
[0043] In this embodiment, the present invention calculates the misalignment angle trigonometric function, east-west velocity error, and position error as state components based on the initial attitude, IMU observation data, and GNSS positioning data. This allows the attitude error to be expressed in an approximately linear and tractable trigonometric function form, thereby reducing the filtering linearization error caused by nonlinearity. A first state vector is constructed using the misalignment angle trigonometric function, east-west velocity error, and position error, making the filter state semantically correspond to the actual error term. Then, Kalman filtering is performed on this state vector using a preset first filter and a first prediction frequency to obtain the estimated value and covariance. This can balance the response rate and estimation stability in the time domain, while outputting an uncertainty metric. Simultaneously, real-time motion state is acquired during the filtering process, and the first filter is reset based on a reset strategy. This can prevent the filter from falling into erroneous convergence under static or insufficient dynamic conditions, thus maintaining the reliability of the estimation.
[0044] In this embodiment, the step of acquiring the real-time motion state of the GNSS receiver during each iteration, and resetting the first filter based on the real-time motion and a preset reset strategy, includes: In each iteration, the real-time motion state of the GNSS receiver is acquired, including the acceleration vector, and the scalar average of the acceleration is calculated based on the acceleration vector. When the average value of the acceleration scalar meets the preset reset conditions, the first filter is reset based on the preset reset strategy.
[0045] In this embodiment, the real-time motion state of the GNSS receiver is continuously acquired. This real-time motion state includes at least a three-axis acceleration vector after zero-bias correction. The magnitude of this acceleration vector is statistically analyzed within a short time window, and its scalar average value is calculated as an indicator of the platform's current dynamic operating condition. The system compares this scalar average acceleration value with a pre-set reset criterion and, based on the duration, determines whether the conditions for triggering a reset are met. Only when this condition is met within a continuously predetermined time period is it considered a genuine reset trigger, thus avoiding malfunctions caused by transient noise.
[0046] In this embodiment, the reset criterion includes a threshold range of "low dynamic / near stationary" or "abnormal impact / violent maneuver". When the average value of the acceleration scalar is within the threshold range of low dynamic or near stationary, the reset criterion is met, and the first filter needs to be reset; or when the average value of the acceleration scalar is within the threshold range of abnormal impact or violent maneuver, the reset criterion is met, and the first filter needs to be reset.
[0047] In this embodiment, the threshold range can be determined based on the average value of acceleration scalars under different motion states.
[0048] In this embodiment, during the first filtering stage, the real-time motion state is acquired, which includes an acceleration vector and the scalar average of acceleration is calculated based on the vector. The scalar average of acceleration is used as a concise quantitative indicator of the current motion intensity, which is beneficial for distinguishing between static and maneuvering conditions. When the scalar average of acceleration meets the preset reset conditions, the first filter is reset based on the preset reset strategy. The threshold-triggered judgment can promptly remove the estimation pollution caused by sudden motion changes, avoiding weak observability of attitude state under static or insufficient dynamic conditions. At this time, the measurement update causes the state estimation error covariance matrix to continuously converge.
[0049] In this embodiment, when the average value of the acceleration scalar meets a preset reset condition, resetting the first filter based on a preset reset strategy includes: When the average value of the acceleration scalar meets the preset reset condition, the covariance in the first filter is set to a preset threshold, and the first state vector is reinitialized based on the current initial attitude, the current IMU observation data, and the current GNSS positioning data to reset the first filter.
[0050] In this embodiment, when a reset is triggered, the first filter is reset according to a preset reset strategy, including: setting the covariance submatrix related to attitude, velocity, and position to a predetermined large initial value, and reinitializing the first state vector based on the initial attitude output by the sensor fusion algorithm, the current IMU observations, and the current GNSS positioning and velocity information; simultaneously, process noise may be adjusted or GNSS observation weights may be temporarily modified to protect the filter from the influence of poor-quality observations. The entire reset process should record the triggering reason and timestamp, and after the reset, the acceleration scalar and GNSS residuals should be continuously monitored to confirm that the filter can reuse subsequent observations to converge under the new initial value.
[0051] In this embodiment, by resetting, on the one hand, the problem of premature and overly tight convergence of the filter under static or low dynamic conditions, which leads to a delayed response to subsequent dynamic observations, is avoided. On the other hand, by resetting the uncertainty and re-initializing the state in the event of sudden violent maneuvers or measurement anomalies, the robustness of the filter to external disturbances and measurement anomalies is enhanced, thereby improving the reliability and usability of the first attitude estimation result and providing a robust input for subsequent fine filtering and initial alignment of tilt measurement.
[0052] As a specific example of an embodiment of the present invention, due to the weak observability of attitude state under static or insufficient dynamic conditions, and the continuous convergence of the state estimation error covariance matrix caused by measurement updates, the state covariance becomes too small when there is sufficient dynamics, and the filter cannot effectively utilize new observation information to correct these state errors. Therefore, it is necessary to determine whether to reset the filter based on the motion state to ensure the correctness of the filtering convergence result.
[0053] (10) (11) in, Indicates the current time The average value of the acceleration scalar obtained statistically; Indicates the number of items counted; express The acceleration vector measured by the IMU at time 1; This represents the local gravitational constant value; and The threshold value set, ; Indicates whether to reset; a value of 1 indicates resetting.
[0054] In this embodiment, due to the weak observability of the attitude state in the inertial navigation system under static or insufficient dynamic conditions, if measurement updates continue, the state estimation error covariance matrix will continuously converge. Consequently, when sufficient dynamic conditions are subsequently available, the state covariance has converged to an excessively small level, preventing the filter from effectively utilizing new observation information to correct potential state estimation errors. Therefore, a motion-state-based filter reset mechanism is introduced in the first filtering stage. Specifically, the acceleration vectors acquired by the IMU are statistically analyzed within a preset time window, and the average scalar acceleration value within that window is calculated. Indicates the current time The average value of the acceleration scalar obtained statistically; Indicates the number of items counted; express The acceleration vector measured by the IMU at each moment is used as a reference, combined with the set upper and lower thresholds. and A judgment is made. When the reset criterion is met, the current motion state is considered to be either stationary or in an abnormal dynamic condition, and a reset flag is set at this time. =1, triggering a reset operation on the first filter; if the condition is not met, then =0, the filter continues to operate normally.
[0055] In this embodiment, by setting the covariance in the first filter to a preset threshold when the average value of the acceleration scalar meets the reset condition, the estimation uncertainty can be artificially increased or reset, thereby enabling the filter to reconverge under subsequent observation and reducing the impact of historical erroneous states. Furthermore, the first state vector is reinitialized based on the current initial attitude, current IMU observation data, and current GNSS positioning data to ensure that the initial conditions of the filter are consistent with the current actual operating conditions, avoiding long-term deviations caused by using outdated states, which is beneficial to accelerating filter recovery and improving the accuracy of initial estimation.
[0056] Step 103: Construct a second state vector based on the first attitude estimation result, and perform Kalman filtering on the second state vector based on a preset second filter until the second filter meets the preset second convergence condition. Retain the state and parameters of the second filter, and use the retained state and parameters as the initial values for tilt measurement to complete the initial alignment of tilt measurement.
[0057] In this embodiment, the step of constructing a second state vector based on the first attitude estimation result, and performing Kalman filtering on the second state vector based on a preset second filter until the second filter meets a preset second convergence condition, retaining the state and parameters of the second filter, and using the retained state and parameters as initial values for tilt measurement to complete the initial alignment of tilt measurement, includes: Based on the first attitude estimation result, the relative attitude error, planar velocity error, and planar position error are calculated, and a second state vector is constructed based on the relative attitude error, planar velocity error, and planar position error. A second filter is preset based on the first attitude estimation result, and Kalman filtering is performed on the second state vector based on the second filter and the preset second prediction frequency; The process continues until the second filter reaches the preset second condition, and the state and parameters of the second filter are retained as the initial value for tilt measurement, thus completing the initial alignment of tilt measurement.
[0058] In this embodiment, relative attitude error, planar velocity error, and planar position error are calculated based on the first attitude estimation result, and a second state vector is constructed accordingly. The state of the second stage can be designed to focus more on the high-precision relative error term, thereby achieving targeted correction of the coarse estimation result. By presetting a second filter based on the first attitude estimation result, the coarse estimation result of the first stage can be used to provide a good linearization point and initial uncertainty for the second stage, thereby significantly shortening the refinement convergence time and improving the final accuracy. Kalman filtering is performed on the second state vector. When the second filter reaches the preset second convergence condition and retains its state and parameters as the initial value for tilt measurement, high-confidence initial parameters for tilt measurement can be output, directly supporting the high-precision operation of subsequent measurement and control tasks.
[0059] In this embodiment, the step of presetting a second filter based on the first attitude estimation result, and performing Kalman filtering on the second state vector based on the second filter and a preset second prediction frequency, includes: The estimated covariance is calculated based on the first attitude estimation result, and the initial state covariance matrix of the second filter is set based on the estimated covariance. The attitude, velocity, and position sub-matrices in the first filter are copied or mapped to the corresponding sub-matrices of the second filter to preset the second filter. Kalman filtering is performed on the second state vector based on the second filter and the preset second prediction frequency.
[0060] In this embodiment, the estimated covariance calculated based on the first attitude estimation result is used to set the initial state covariance matrix of the second filter. This helps to retain the quantification information of uncertainty in the first stage, thereby making the filter in the second stage statistically continuous and consistent. Furthermore, copying or mapping the attitude, velocity, and position sub-matrices in the first filter to the corresponding sub-matrices of the second filter to preset the second filter allows for the direct inheritance of converged state estimates and variable correlations, reducing redundant estimation work and restart jitter, thereby accelerating the convergence speed of the second filter and reducing transient errors in the refinement process.
[0061] In this embodiment, after completing the coarse estimation of the first filter and obtaining the first attitude estimation result, the second filtering stage is entered to achieve high-precision initial alignment. Specifically, firstly, based on the first attitude estimation result, the relative attitude error, planar velocity error, and planar position error are calculated, and a second state vector is constructed accordingly, so that the state design in the second stage can focus on the high-precision relative error term. Subsequently, the second filter is preset using the first attitude estimation result, that is, the estimated covariance is calculated based on the first attitude estimation result, and the estimated covariance is used as the initial state covariance matrix of the second filter. At the same time, the converged attitude, velocity, and position sub-matrices in the first filter are copied or mapped to the corresponding sub-matrices of the second filter to ensure the continuity of state estimation and the inheritance of uncertainty. On this basis, based on the second filter and the preset second prediction frequency, Kalman filtering is performed on the second state vector, and the change amplitude of the gyroscope zero bias estimation and the GNSS measurement residual characteristics are continuously acquired during the filtering process. When the change amplitude of the gyroscope zero bias estimation and the measurement residual are both less than the corresponding preset thresholds and remain stable within a continuous predetermined time period, it is determined that the second filter has converged, and the state and parameters of the second filter are retained at this time. In this way, the convergence result of the second filter is used as the initial value for tilt measurement, thus completing the initial alignment of the tilt measurement. This method not only effectively shortens the refinement convergence time but also ensures the high reliability of the final alignment parameters, directly supporting subsequent high-precision measurement and control tasks.
[0062] In this embodiment, the second filter also consists of two parts: a state equation and a measurement equation. The state equation is constructed based on the second state vector and is used for time-series prediction and uncertainty propagation of the system state when there is no observation input. Specifically, the second state vector mainly includes error states related to precision alignment, such as relative attitude error components, planar velocity error, planar position error, gyroscope zero bias term, accelerometer zero bias term, and altitude error.
[0063] In this embodiment, for the case of a small misalignment angle, the misalignment angle is... Speed error Position error Gyroscope zero bias error and accelerometer zero bias error Model the parameters and construct the second state vector: (12) in, This indicates the navigation coordinate system, namely the Northeast-Sky (ENU) coordinate system; Indicates elevation error; Indicates the inertial navigation coordinate system. and It is a vector in the inertial navigation coordinate system.
[0064] In this embodiment, the state equation inputs inertial measurements (the ratio of gyro angular velocity to acceleration force after zero-bias compensation) into the kinematic model. Combined with a random walk model of gyro and accelerometer zero-bias, it recursively predicts the aforementioned error quantities. Simultaneously, the attitude estimate output by the first filter is used as the linearization point or initial reference, thereby ensuring the statistical and numerical continuity and consistency of the two-stage filtering. Specifically, considering the influence of inertial sensor measurement errors and navigation parameter calculation errors, the error state equation is as follows: (13) in, To indicate System relative to The rotation of the system; Calculate the error for the navigation system; The original measurement error vector of the gyroscope is in Projection under the system; Specific force measured by an accelerometer; for The speed of the system; for This is the error in the Earth's rotational angular velocity. Errors in navigation system rotation calculation; The rotation of the navigation system is caused by the Earth's rotation angle; The rotational speed of the inertial navigation system as it moves near the Earth's surface due to the curvature of the Earth's surface; This is due to accelerometer measurement error; This is due to gravity error; , and These represent the errors in latitude, longitude, and elevation, respectively. and These are the principal curvature radii of the meridian and the principal curvature radii of the lateral meridian, respectively.
[0065] In this embodiment, the observation equation constrains the second filter with GNSS observation information and uses the GNSS position and velocity as measurements to constrain the INS state: (14) in, and Velocities calculated for INS and GNSS respectively; and The positions are calculated for INS and GNSS, respectively.
[0066] In this embodiment, the second filter may be an extended Kalman filter or other filter variants adapted to nonlinearity, and prediction is performed at a preset second prediction frequency, with an update loop; after each update, the current state estimate and covariance are output.
[0067] In this embodiment, the step of maintaining the state and parameters of the second filter until the second filter reaches a preset second condition, and using this as the initial value for tilt measurement to complete the initial alignment of tilt measurement, includes: In the second filtering stage, the change amplitude of the gyroscope zero bias estimate and the characteristics of the GNSS measurement residuals in the second filter are acquired in real time. When the change amplitude of the gyroscope zero bias estimate and the measurement residual are both less than the corresponding preset threshold and remain stable within a continuous predetermined time period, the second filter is determined to have converged, and the state and parameters of the second filter are retained as the initial value for tilt measurement to complete the initial alignment of tilt measurement.
[0068] In this embodiment, the variation amplitude of the gyroscope zero-bias estimate and the statistical characteristics of the GNSS measurement residuals are continuously monitored. The second filter is considered converged only when both the variation amplitude of the zero-bias estimate and the measurement residuals are stable and less than a preset threshold, and remain stable within a continuous predetermined time window. After convergence, the state vector and covariance matrix of the filter are retained as initial values for tilt measurement for subsequent use by the integrated navigation or control module. Through the coordinated design of the state equation and measurement equation, the second filter can achieve high-precision refinement of attitude and navigation errors based on the coarse estimation in the first stage, thereby significantly improving the accuracy and reliability of the initial alignment of tilt measurement.
[0069] In this embodiment, during the second filtering stage, the change amplitude of the gyroscope zero bias estimate in the second filter and the GNSS measurement residual characteristics are acquired in real time, respectively providing quality monitoring of the sensor's internal bias and external measurements. When the change amplitude of the gyroscope zero bias estimate and the measurement residual are both less than the corresponding preset threshold and remain stable within a continuous predetermined time period, convergence is determined. By simultaneously satisfying the conditions of bias stability and external measurement consistency, the refinement process can be effectively avoided from prematurely terminating due to transient noise or short-term convergence artifacts, ensuring that the retained states and parameters, when used as initial values for tilt measurement, meet the usable standards in terms of accuracy and reliability.
[0070] In this embodiment, firstly, IMU observation data from the GNSS receiver is acquired, and the initial attitude is calculated based on this IMU observation data. This quickly yields a usable coarse attitude reference, reducing linearization errors in subsequent filtering and shortening the initial convergence time. Secondly, a first state vector is constructed based on the initial attitude, IMU observation data, and GNSS positioning data, and Kalman filtering is performed using a preset first filter. By jointly fusing inertial and GNSS information, the observability of the first filter for attitude, position, and velocity errors is improved, thus enhancing the coarse estimation accuracy. Thirdly, in the first filtering stage, the real-time motion state is acquired, and the first filter is reset according to a preset reset strategy to repeatedly perform filtering until the first convergence condition is met. This enhances the robustness of the filter to dynamic disturbances or sudden motion changes, avoiding estimation offsets caused by transient motion. Finally, a second state vector is constructed based on the first attitude estimation result and refined using a second filter until the second convergence is achieved. The state and parameters are retained as initial values for tilt measurement, realizing a two-stage estimation process from coarse to fine. This improves the alignment accuracy of GNSS receiver tilt measurement and outputs reliable initial values that can be directly used for tilt measurement while ensuring accuracy, shortening the preparation time for the system to reach a measurable state.
[0071] Please refer to Figure 2 , Figure 2 This is a schematic diagram of another process for rapid initial alignment of tilt measurement provided in an embodiment of the present invention.
[0072] In this embodiment, IMU observation data and GNSS positioning data are first acquired, and an initial attitude is calculated and output from the IMU data as the initial linearization point using a preset sensor fusion algorithm (such as a Madgwick-type algorithm based on quaternions). Then, a first state vector is constructed based on the initial attitude, IMU observation, and GNSS positioning. The first state vector includes trigonometric function components of the large azimuth misalignment angle, east-west / north-south velocity error, and position error. Kalman filtering is performed at the first prediction frequency using a preset first filter. The motion state of the carrier is collected in real time during the filtering loop, and a reset is triggered based on the short-time statistics of the average value of the acceleration scalar and a preset threshold. If a reset is triggered, the relevant covariance is amplified according to the reset strategy, and the first state vector is reinitialized based on the current IMU / GNSS observation to restore the observability of the filter. The loop continues until the first state estimate and covariance converge, and the first attitude estimation result is obtained.
[0073] In this embodiment, the relative attitude error, planar velocity error, and planar position error are calculated based on the first attitude estimation result, and a second state vector is constructed. The estimated covariance output by the first filter is mapped to the initial covariance of the second filter, and the attitude, velocity, and position sub-matrices are copied or mapped to the second filter. A higher-precision Kalman filter is applied to the second state vector at a preset second prediction frequency. During the second filtering stage, the change amplitude of the gyroscope zero-bias estimation and the characteristics of the GNSS measurement residuals are monitored in real time. Only when the above indicators are all less than the corresponding thresholds within a consecutive predetermined time window is the second filter considered to have converged, and its state and parameters are retained as the initial values for tilt measurement. This hierarchical filtering and motion state-based reset mechanism is intuitively presented in the figure. It can significantly shorten the convergence time and output a high-confidence initial alignment result while ensuring dynamic robustness, thereby completing the initial alignment of tilt measurement and providing it for use by subsequent integrated navigation or control modules.
[0074] This invention also provides a rapid initial alignment device for tilt measurement, comprising: a data acquisition module, a first alignment module, and a second alignment module; The data acquisition module is used to acquire IMU observation data from the GNSS receiver and calculate the initial attitude of the GNSS receiver based on the IMU observation data. The first alignment module is used to acquire GNSS positioning data, construct a first state vector based on the initial attitude, IMU observation data and GNSS positioning data; perform iterative Kalman filtering on the first state vector based on a preset first filter, and repeatedly perform filtering until the first filter meets a preset first convergence condition to obtain a first attitude estimation result; wherein, in each iteration, the real-time motion state of the GNSS receiver is acquired and the first filter is reset according to a preset reset strategy; The second alignment module is used to construct a second state vector based on the first attitude estimation result, and to perform Kalman filtering on the second state vector based on a preset second filter until the second filter meets a preset second convergence condition. The state and parameters of the second filter are retained, and the retained state and parameters are used as the initial values for tilt measurement to complete the initial alignment of tilt measurement.
[0075] In this embodiment of the invention, a terminal device is also provided, including a processor, a memory, and a computer program stored in the memory and configured to be executed by the processor. When the processor executes the computer program, it implements the above-described rapid initial alignment method for GNSS receiver tilt measurement.
[0076] In this embodiment of the invention, a computer-readable storage medium is also provided, which includes a stored computer program, wherein, when the computer program is running, it controls the device where the computer-readable storage medium is located to execute the above-described rapid initial alignment method for GNSS receiver tilt measurement.
[0077] For example, a computer program can be divided into one or more modules, one or more of which are stored in memory and executed by a processor to perform the present invention. The one or more modules can be a series of computer program instruction segments capable of performing a specific function, which describe the execution process of the computer program in a terminal device.
[0078] The terminal device may be a desktop computer, laptop, handheld computer, or cloud server, etc. The terminal device may include, but is not limited to, a processor, memory, and display. Those skilled in the art will understand that the above components are merely examples of terminal devices and do not constitute a limitation on the terminal device. It may include more or fewer components, or combinations of certain components, or different components. For example, the terminal device may also include input / output devices, network access devices, buses, etc.
[0079] The processor can be a Central Processing Unit (CPU), or other general-purpose processors, digital signal processors (DSPs), application-specific integrated circuits (ASICs), field-programmable gate arrays (FPGAs), or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components, etc. A general-purpose processor can be a microprocessor or any conventional processor. The processor is the control center of the terminal device, connecting all parts of the terminal device through various interfaces and lines.
[0080] Memory can be used to store computer programs and / or modules. The processor implements various functions of the terminal device by running or executing the computer programs and / or modules stored in the memory, and by accessing data stored in the memory. Memory can mainly include a program storage area and a data storage area. The program storage area can store the operating system, application programs required for at least one function (such as sound playback, text conversion, etc.), etc.; the data storage area can store data created based on the use of the mobile phone (such as audio data, text message data, etc.). In addition, memory can include high-speed random access memory, and can also include non-volatile memory, such as hard disks, RAM, plug-in hard disks, smart media cards (SMC), secure digital cards (SD cards), flash cards, at least one disk storage device, flash memory device, or other volatile solid-state storage devices.
[0081] The module for rapid initial alignment based on tilt measurement of the GNSS receiver, if implemented as a software functional unit and sold or used as an independent product, can be stored in a computer-readable storage medium. Based on this understanding, all or part of the processes in the above embodiments of the present invention can also be implemented by a computer program instructing related hardware. The computer program can be stored in a computer-readable storage medium, and when executed by a processor, it can implement the steps of the various method embodiments described above. The computer program includes computer program code, which can be in the form of source code, object code, executable files, or certain intermediate forms. The computer-readable medium can include: any entity or device capable of carrying computer program code, recording media, USB flash drives, portable hard drives, magnetic disks, optical disks, computer memory, read-only memory (ROM), random access memory (RAM), electrical carrier signals, telecommunication signals, and software distribution media, etc. Those skilled in the art can understand and implement this without any inventive effort.
[0082] The specific embodiments described above further illustrate the purpose, technical solution, and beneficial effects of the present invention. It should be understood that the above descriptions are merely specific embodiments of the present invention and are not intended to limit the scope of protection of the present invention. In particular, it should be noted that any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the scope of protection of the present invention for those skilled in the art.
Claims
1. A rapid initial alignment method for tilt measurement, characterized in that, include: Acquire IMU observation data from the GNSS receiver, and calculate the initial attitude of the GNSS receiver based on the IMU observation data; The process involves acquiring GNSS positioning data, constructing a first state vector based on the initial attitude, IMU observation data, and GNSS positioning data, and then performing iterative Kalman filtering on the first state vector using a preset first filter. This filtering is repeated until the first filter meets a preset first convergence condition, resulting in a first attitude estimation result. Specifically, in each iteration, the real-time motion state of the GNSS receiver is acquired, and the first filter is reset according to a preset reset strategy. This includes: acquiring the real-time motion state of the GNSS receiver, which includes an acceleration vector, and calculating a scalar average of the acceleration vector; when the scalar average of the acceleration meets a preset reset condition, the first filter is reset according to the preset reset strategy. A second state vector is constructed based on the first attitude estimation result; the second state vector is subjected to Kalman filtering based on a preset second filter until the second filter meets a preset second convergence condition, the state and parameters of the second filter are retained, and the retained state and parameters are used as the initial values for tilt measurement to complete the initial alignment of tilt measurement.
2. The rapid initial alignment method for tilt measurement as described in claim 1, characterized in that, The IMU observation data includes accelerometer data and gyroscope data; The process of acquiring IMU observation data and GNSS positioning data from the GNSS receiver, and calculating the initial attitude of the GNSS receiver based on the IMU observation data, includes: Acquire IMU observation data and GNSS positioning data from the GNSS receiver; The accelerometer and gyroscope data are processed by a preset sensor fusion algorithm to obtain the attitude quaternion of the GNSS receiver; The attitude quaternion is converted into Euler angles based on a preset transformation formula, and the initial attitude of the GNSS receiver is generated based on the Euler angles.
3. The rapid initial alignment method for tilt measurement as described in claim 1, characterized in that, The process of acquiring GNSS positioning data involves constructing a first state vector based on the initial attitude, IMU observation data, and GNSS positioning data; performing iterative Kalman filtering on the first state vector based on a preset first filter, repeating the filtering until the first filter satisfies a preset first convergence condition, and obtaining a first attitude estimation result; wherein, in each iteration, the real-time motion state of the GNSS receiver is acquired and the first filter is reset according to a preset reset strategy, including: Based on the initial attitude, IMU observation data, and GNSS positioning data, calculate the misalignment angle trigonometric function, eastward velocity error, northward velocity error, eastward position error, and northward position error; A first state vector is constructed based on the aforementioned misalignment angle trigonometric function, eastward velocity error, northward velocity error, eastward position error, and northward position error; Based on a preset first filter and a preset first prediction frequency, perform iterative Kalman filtering on the first state vector to obtain the estimated value and covariance of the first state vector until the estimated value and covariance of the first state vector converge, stop filtering, and obtain the first attitude estimation result of the first filter. In each iteration, the real-time motion state of the GNSS receiver is acquired, and the first filter is reset based on the real-time motion and a preset reset strategy.
4. The rapid initial alignment method for tilt measurement as described in claim 3, characterized in that, When the average value of the acceleration scalar meets a preset reset condition, the first filter is reset based on a preset reset strategy, including: When the average value of the acceleration scalar meets the preset reset condition, the covariance in the first filter is set to a preset threshold, and the first state vector is reinitialized based on the current initial attitude, the current IMU observation data, and the current GNSS positioning data to reset the first filter.
5. The rapid initial alignment method for tilt measurement as described in claim 1, characterized in that, The process of constructing a second state vector based on the first attitude estimation result, and performing Kalman filtering on the second state vector based on a preset second filter until the second filter meets a preset second convergence condition, retaining the state and parameters of the second filter, and using the retained state and parameters as initial values for tilt measurement to complete the initial alignment of tilt measurement, includes: Based on the first attitude estimation result, the relative attitude error, planar velocity error, and planar position error are calculated, and a second state vector is constructed based on the relative attitude error, planar velocity error, and planar position error. A second filter is preset based on the first attitude estimation result, and Kalman filtering is performed on the second state vector based on the second filter and the preset second prediction frequency; Until the second filter reaches the preset second condition, the state and parameters of the second filter are retained as the initial value for tilt measurement, and the initial alignment of tilt measurement is completed.
6. The rapid initial alignment method for tilt measurement as described in claim 5, characterized in that, The step of presetting a second filter based on the first attitude estimation result, and performing Kalman filtering on the second state vector based on the second filter and a preset second prediction frequency, includes: The estimated covariance is calculated based on the first attitude estimation result, and the initial state covariance matrix of the second filter is set based on the estimated covariance. The attitude, velocity, and position sub-matrices in the first filter are copied or mapped to the corresponding sub-matrices of the second filter to preset the second filter. Kalman filtering is performed on the second state vector based on the second filter and the preset second prediction frequency.
7. The rapid initial alignment method for tilt measurement as described in claim 6, characterized in that, The step of retaining the state and parameters of the second filter until the second filter reaches a preset second condition, and using this as the initial value for tilt measurement, to complete the initial alignment of the tilt measurement, includes: In the second filtering stage, the change amplitude of the gyroscope zero bias estimate and the characteristics of the GNSS measurement residuals in the second filter are acquired in real time. When the change amplitude of the gyroscope zero bias estimate and the measurement residual are both less than the corresponding preset threshold and remain stable within a continuous predetermined time period, the second filter is determined to have converged. The state and parameters of the second filter are retained as the initial value for tilt measurement, and the initial alignment of tilt measurement is completed.
8. A rapid initial alignment device for tilt measurement, characterized in that, include: Data acquisition module, first alignment module, and second alignment module; The data acquisition module is used to acquire IMU observation data from the GNSS receiver and calculate the initial attitude of the GNSS receiver based on the IMU observation data. The first alignment module is used to acquire GNSS positioning data, construct a first state vector based on the initial attitude, IMU observation data, and GNSS positioning data; perform iterative Kalman filtering on the first state vector based on a preset first filter, repeatedly executing the filtering until the first filter meets a preset first convergence condition, and obtain a first attitude estimation result; wherein, in each iteration, the real-time motion state of the GNSS receiver is acquired and the first filter is reset according to a preset reset strategy, including: in each iteration, acquiring the real-time motion state of the GNSS receiver, the real-time motion state including an acceleration vector, calculating an average scalar acceleration value based on the acceleration vector; when the average scalar acceleration value meets a preset reset condition, resetting the first filter according to the preset reset strategy; The second alignment module is used to construct a second state vector based on the first attitude estimation result, and to perform Kalman filtering on the second state vector based on a preset second filter until the second filter meets a preset second convergence condition. The state and parameters of the second filter are retained, and the retained state and parameters are used as the initial values for tilt measurement to complete the initial alignment of tilt measurement.
9. A terminal device, characterized in that, It includes a processor, a memory, and a computer program stored in the memory and configured to be executed by the processor, wherein the processor executes the computer program to implement a rapid initial alignment method for tilt measurement as described in any one of claims 1 to 7.
Citation Information
Patent Citations
System, method and device for measuring tilt amount of GNSS receiver and storage medium
CN109814133A
Additive-quaternion-based large azimuth misalignment angle two-stage linear alignment method
CN110779550A
GNSS carrier phase and Doppler fusion speed measurement method assisted by adaptive kinematics model
CN111007557A
