A real-time positioning method for forest environments

Abstract

This invention relates to the technical field of positioning methods, specifically to a real-time positioning method for forest environments. The method includes the following steps: S1: Solving IMU and BeiDou data; S2: Establishing the BeiDou measurement equation and the IMU state equation; S3: Using ESKF to achieve data fusion and output compensation; S4: SLSTM network training and data augmentation; S5: After BeiDou loss of lock, the SLSTM network performs real-time prediction; after BeiDou recovery, step S3 continues. This method, through data augmentation technology, improves the robustness and adaptability of the neural network model, enabling the system to maintain high-precision positioning even in complex environments.

Description

Technical Field

[0001] This invention relates to the technical field of positioning methods, and more specifically to a real-time positioning method for forest environments. Background Technology

[0002] With the continuous development of positioning technology, BeiDou receivers and inertial measurement units (IMUs) have been widely used in various navigation systems.

[0003] Application document CN112285756A discloses a high-precision positioning method and system based on an IMU and the BeiDou system. The method includes: acquiring spatial attitude data of a motion system using the IMU device; wherein the motion system is used to capture positioning signals during the movement of a motion platform. Based on the GPRS DTU wireless terminal device and the spatial attitude data, rectified positioning data or local positioning data in a non-positioning state is generated and transmitted to the IMU device; positioning data from BeiDou satellites is received via a signal antenna, and the positioning data is transmitted to the IMU device for data fusion to obtain target data; the lidar and IMU device are fixed to the motion platform using a lidar mounting bracket, and a decision-making scheme is generated based on the target data.

[0004] However, in complex environments such as forests, satellite signals are often blocked by trees, leading to weakened or even completely lost signals, which severely impacts positioning accuracy. Furthermore, the cumulative error of the IMU gradually increases over long periods of operation, causing the positioning results to diverge. Therefore, maintaining high-precision positioning capabilities even when satellite signals are unstable or lost has become a pressing technical problem to be solved. Summary of the Invention

[0005] The purpose of this invention is to provide a high-precision positioning method. In view of the above-mentioned shortcomings, a real-time positioning method for forest environments is proposed.

[0006] The present invention adopts the following technical solution: A real-time positioning method for forest environments, comprising the following steps: S1: Solving IMU data and BeiDou data; S2: Establish the measurement equations for BeiDou and the state equations for the IMU; S3: Utilize ESKF to achieve data fusion and output compensation; S4: SLSTM network training and data augmentation; S5: After BeiDou loses lock, the SLSTM network performs real-time prediction. After BeiDou recovers, continue to step S3.

[0007] Optionally, in step S1, the navigation information of the carrier based on the IMU is calculated and the increment is calculated and transmitted to the host computer; the Beidou receiver acquires the navigation information of the carrier based on Beidou in real time and transmits it to the host computer; the IMU data and Beidou data are timestamped and aligned on the host computer.

[0008] Optionally, in step S1, timestamp alignment uses the three nearest neighbor averaging algorithm.

[0009] Optionally, in step S1, the first BeiDou information is obtained using the three nearest neighbor averaging algorithm as the initial point. The current longitude, latitude, and altitude information are recorded, along with the BeiDou timestamp. and current local time Then, complete the following algorithm steps: Data acquisition: Collect 10 GPS data points, denoted as P1, P2, ..., P10; Distance calculation: Calculate the Euclidean distance between these 10 data points; GPS data point selection: Select the three data points closest to the initial point, i.e., K=3; Average calculation: Average the positions of these three data points to obtain the current position; IMU data point processing: Collect the IMU data point closest to the initial point and record the IMU's timestamp. and current local time ;Pick As a timestamp compensation for both, all IMU timestamps are synchronized and compensated.

[0010] Optionally, in step S2, the IMU's state equation is used to describe the motion state of the carrier, and the BeiDou observation equation is used to correlate BeiDou data and IMU data.

[0011] Optionally, in step S3, ESKF fuses BeiDou data and IMU data through measurement equations and state equations. During the fusion process, ESKF predicts the current position, velocity and attitude of the carrier, and reduces the accumulated error of IMU data through BeiDou data correction.

[0012] Optionally, in step S4, an SLSTM network is trained using IMU data, error state variables, and position, velocity, and attitude information output by ESKF, and data augmentation is achieved using random time expansion and compression.

[0013] The beneficial effects achieved by this invention are: 1. By preprocessing the input IMU and BeiDou data, noise is filtered out and data quality is improved, thereby enhancing the accuracy and stability of subsequent processing and compensation; 2. A real-time data augmentation method based on SLSTM (Scalar Long Short-Term Memory Network) was adopted. This method improves the robustness and adaptability of the neural network model through data augmentation technology, enabling the system to maintain high-precision positioning even in complex environments.

[0014] To further understand the features and technical content of the present invention, please refer to the following detailed description and drawings of the present invention. However, the drawings provided are for reference and illustration only and are not intended to limit the present invention. Attached Figure Description

[0015] Figure 1 This is a flowchart of the steps of the present invention; Figure 2 This is a schematic diagram of the SLSTM network training structure in this invention; Figure 3 This is a schematic diagram of the SLSTM network prediction structure in this invention; Figure 4 This is a flowchart of the program of the present invention; Figure 5 This is a schematic diagram of the data flow during SLSTM network training in this invention. Detailed Implementation

[0016] The following specific embodiments illustrate the implementation of the present invention. Those skilled in the art can understand the advantages and effects of the present invention from the content disclosed in this specification. The present invention can be implemented or applied through other different specific embodiments, and various details in this specification can also be modified and changed based on different viewpoints and applications without departing from the spirit of the present invention. Furthermore, the accompanying drawings of the present invention are for simple illustrative purposes only and are not depictions of actual dimensions; this is stated in advance. The following embodiments will further describe the relevant technical content of the present invention in detail, but the disclosed content is not intended to limit the scope of protection of the present invention.

[0017] This embodiment provides a real-time positioning method for forest environments, combined with... Figures 1 to 5 As shown.

[0018] A real-time positioning method for forest environments, comprising the following steps: S1: Solving IMU data and BeiDou data; S2: Establish the measurement equations for BeiDou and the state equations for the IMU; S3: Utilize ESKF to achieve data fusion and output compensation; S4: SLSTM network training and data augmentation; S5: After BeiDou loses lock, the SLSTM network performs real-time prediction. After BeiDou recovers, continue to step S3.

[0019] Optionally, in step S1, the navigation information of the carrier based on the IMU is calculated and the increment is calculated and transmitted to the host computer; the Beidou receiver acquires the navigation information of the carrier based on Beidou in real time and transmits it to the host computer; the IMU data and Beidou data are timestamped and aligned on the host computer.

[0020] Specifically, regarding the IMU (Integrated Measurement Unit), the IMU continuously collects information such as the carrier's acceleration and angular velocity. This data constitutes the carrier's raw motion information. Based on the angular velocity and acceleration collected by the IMU, the carrier's current position, velocity, and attitude are estimated through integration calculations. During the calculation process, the raw IMU data is processed. For example, acceleration is calculated using double integration to obtain the current position, while angular velocity is integrated to obtain the rotation angle. These calculations are performed in real time, updating the carrier's motion state based on updated IMU data. Since the IMU cannot directly measure absolute position, this is the difference between the IMU and BeiDou (China's satellite navigation system). The increment refers to the change measured by the IMU over a very short period of time. For example, how much have the carrier's position, velocity, and attitude changed between the previous and current moments? By comparing the data from the current moment with the previous moment, the displacement increment, velocity increment, and attitude angle increment at each moment are calculated. These incremental data represent the updated motion information, showing the changes compared to the previous state.

[0021] Regarding the BeiDou part, BeiDou directly obtains the absolute position of the current moment from the satellite, without relying on past measurement results to calculate the current position, so there is no need for incremental calculation.

[0022] Because the IMU and BeiDou have different sampling frequencies, directly fusing these two types of data may introduce errors. Therefore, time alignment, or "timestamp alignment," is necessary.

[0023] The three nearest neighbor averaging algorithm can be used. Here, "three" can be understood as follows: First, take 10 data points from the current location (due to errors in BeiDou data acquisition, these 10 data points will not be identical), find the three data points with the closest Euclidean distance, and then assume that the actual position is the average of these three positions. The entire process runs after these 10 BeiDou data acquisitions. This is the cornerstone of the entire positioning process because subsequent results yield relative positions, while this is the only absolute coordinate position. (The main purpose of using the three nearest neighbor averaging algorithm is to reduce fluctuations in individual BeiDou data points and smooth the position estimation results.) Using a serial port to receive data from BeiDou and IMU, the first BeiDou information is obtained as the initial point using the Three-Nearest Neighbors Averaging Algorithm. The current longitude, latitude, and altitude information are recorded, along with the BeiDou timestamp. and current local time .

[0024] Optionally, in step S1, the first BeiDou information is obtained using the three nearest neighbor averaging algorithm as the initial point. The current longitude, latitude, and altitude information are recorded, along with the BeiDou timestamp. and current local time Then, complete the following algorithm steps: Data acquisition: Collect 10 GPS data points, denoted as P1, P2, ..., P10; Distance calculation: Calculate the Euclidean distance between these 10 data points; GPS data point selection: Select the three data points closest to the initial point, i.e., K=3; Average calculation: Average the positions of these three data points to obtain the current position; IMU data point processing: Collect the IMU data point closest to the initial point and record the IMU's timestamp. and current local time ;Pick As a timestamp compensation for both, all IMU timestamps are synchronized and compensated.

[0025] Specifically, the IMU is selected here to quickly obtain the next IMU data for compensation after the averaged BeiDou data is obtained. The current local time t_1 is the local clock domain time (computer time) when the BeiDou data is collected, and t_2 is the local clock domain time when the next IMU data arrives after the BeiDou data is collected.

[0026] Algorithm steps 1. Data Acquisition: Collect 10 GPS data points, denoted as P1, P2, ..., P10.

[0027] 2. Calculate the distance: Calculate the Euclidean distance between these 10 data points.

[0028] 3. Select the closest points: Select the three data points that are closest to each other, i.e., K=3.

[0029] 4. Average calculation: The positions of the three data points are averaged to obtain the current position.

[0030] Then collect the next most recent IMU data and record the IMU's timestamp. and current local time .

[0031] Pick As a timestamp compensation between the two, all IMU timestamps are synchronized. This algorithm takes into account the millisecond-level time difference between the two, making time synchronization more accurate.

[0032] Optionally, in step S2, the IMU's state equation is used to describe the motion state of the carrier, and the BeiDou observation equation is used to correlate BeiDou data and IMU data.

[0033] Specifically, the observation equations are established using Kalman filtering, and the observed quantities are BeiDou data (longitude, latitude, and elevation). Kalman filtering requires both prediction and observation equations for iterative processing.

[0034] Optionally, in step S3, ESKF fuses BeiDou data and IMU data through measurement equations and state equations. During the fusion process, ESKF predicts the current position, velocity and attitude of the carrier, and reduces the accumulated error of IMU data through BeiDou data correction.

[0035] Specifically, BeiDou provides global absolute position information (such as latitude, longitude, and altitude), offering high positioning accuracy, but its data update frequency is low, and it may temporarily fail under certain conditions (such as signal blockage or loss of lock). IMU, on the other hand, uses built-in accelerometers and gyroscopes to measure the acceleration and angular velocity of a device in real time. This data is used to calculate the relative position, velocity, and attitude of the vehicle. A characteristic of IMUs is that they can accumulate errors over long periods (for example, sensor drift or noise can cause navigation errors to accumulate over time). Therefore, combining the two, and fusing BeiDou and IMU data via ESKF, allows us to use BeiDou's accurate position information to correct the accumulated errors of the IMU, while using high-frequency IMU data to compensate for the low-frequency deficiencies of BeiDou.

[0036] The core function of ESKF is to dynamically fuse the predictive capabilities of IMU data and the correction capabilities of BeiDou data, thereby reducing their respective errors and providing more accurate navigation results.

[0037] This includes a prediction step: based on the IMU measurements, the system state equation predicts the system state (such as position, velocity, and attitude) at the next moment. At this time, ESKF updates the current state by calculating the state transition equation. For the accumulated error of the IMU measurements, ESKF adjusts it through the state transition matrix and the process noise covariance matrix, gradually accumulating the prediction of the error.

[0038] Update steps: When the BeiDou position information is updated, ESKF compares the actual measured BeiDou position information with the position information predicted by the IMU, and calculates the difference between the two according to the measurement equation. Using this difference, ESKF corrects the state of the IMU to reduce the deviation caused by the accumulated error of the IMU. In this step, the Kalman gain is dynamically adjusted according to the change of the error covariance matrix to ensure that the system gradually tends to a smaller error range.

[0039] Output Compensation: After data fusion, ESKF outputs a corrected state estimate, including position, velocity, and attitude. This output is more accurate than relying solely on IMU or BeiDou data. By correcting the accumulated errors of the IMU using BeiDou data, it ensures that the navigation system can maintain accurate navigation for extended periods, even when there is no BeiDou signal.

[0040] To ensure that ESKF can provide optimal performance under different environments and dynamic conditions, its parameters must be tuned offline, a process that is usually achieved through repeated experimentation and optimization.

[0041] For example, adjusting the parameters: Process noise covariance matrix (Q): reflects the characteristics of accelerometer and gyroscope noise in the IMU, affecting the prediction accuracy of IMU data. Measurement noise covariance matrix (R): reflects the measurement noise level in BeiDou data, affecting the accuracy of BeiDou correction. Initial state covariance matrix (P): defines the uncertainty of the system's initial state, affecting the estimation accuracy of ESKF at the initial time.

[0042] Offline debugging: By conducting offline tests in different scenarios (stationary, accelerating, turning, high speed, etc.), the performance of ESKF under different parameter combinations (e.g., position error, speed error) is observed. Optimized parameters enable the system to maintain stable high-precision positioning and navigation in various complex environments.

[0043] Verification and optimization: Verify the filter output through simulation and field testing, and continuously adjust the parameters until the error is minimized.

[0044] To ensure the integrated navigation system can accurately start under any position, attitude, or velocity conditions, the ESKF (Electronic Navigation Frame) must be adaptable to various initial conditions during initialization. Upon startup, the system initializes its state based on the initial data received from the IMU (Integrated Measurement Unit) and BeiDou (BeiDou Navigation Satellite System), estimating the initial system state (such as initial position, velocity, and attitude) based on current position and acceleration information. Utilizing high-frequency dynamic information from the IMU ensures rapid entry into operational mode even under high-speed motion or varying attitudes. Through ESKF design and parameter optimization, the system can converge quickly and maintain stable navigation output even under complex initial conditions (such as different velocities, positions, and attitudes). While the system may exhibit significant uncertainties in the initial phase, these uncertainties are rapidly reduced through gradual correction, leading to a steady-state operating mode.

[0045] The following are the steps for using ESKF as a foundation: It's important to note here that Kalman filtering is applied to the IMU data to smooth it. At this stage, it's not ESKF (Electronic Science Foundation Filtering) but KF (Kinetic Science Foundation Filtering). This smoothed data is then used for ESKF fusion of the IMU and BeiDou data. The advantage of smoothing the IMU data first is:

[0046] IMU sensors are susceptible to high-frequency noise when measuring acceleration and angular velocity, including mechanical vibration, electrical noise, and inherent random errors in the sensor. Data smoothing can reduce noise levels, making useful signals stand out more in the data and facilitating subsequent processing.

[0047] Due to sensor malfunctions, environmental interference, and other reasons, there may be sudden anomalies in IMU data. Smoothing algorithms can filter out or reduce the impact of outliers to a certain extent, preventing them from impacting the filter, thus reducing the sensitivity of the entire system to noise and outliers and improving the stability of the filter.

[0048] The performance of a Kalman filter depends heavily on the quality of the initial state estimation. If the initial estimate is significantly flawed, the filter will take a long time to converge and may even diverge. Smoothed data can be used to estimate the initial state more accurately, reducing the initial error.

[0049] The aim is to estimate acceleration and its derivatives with high accuracy, thereby improving the quality of IMU measurement data. The Kalman filter provides a recursive solution to estimate the state of a dynamic system from a series of noisy measurements. The initial state vector is defined as representing the acceleration components along the three axes and their respective derivatives:

[0050] in, , and These are the initial acceleration readings along the X, Y, and Z axes from the IMU. , and The units are all meters per second squared.

[0051] State transition matrix Based on discrete time step Defined as follows (state transition matrix) This demonstrates the linear change of acceleration and velocity over time. The matrix contains... This indicates the sampling interval, which affects the update of velocity and position over time. If A larger value indicates that the system's state changes rapidly; if A smaller time step indicates that the system's state changes slowly. (equal to the reciprocal of the IMU sampling frequency)

[0052]

[0053] Initial state covariance matrix The definition is as follows:

[0054] Measurement Matrix The definition is as follows:

[0055] Process noise covariance matrix Based on time step The definition is as follows:

[0056] Where dt is the sampling interval.

[0057]

[0058] Measurement noise covariance matrix Defined as follows

[0059] First, perform state prediction: The state of the next moment

[0060] in, Let represent the optimal system state variables at time k-1. Let represent the system state variables predicted at time k-1. The Kalman filter generates an error covariance matrix P at each step, which describes the statistical distribution of the estimation error. The design goal of the filter is to minimize P during update and prediction iterations. If other linear unbiased filters exist that achieve a smaller error covariance than KF, then the Kalman filter is no longer optimal. Under ideal conditions of linearity, Gaussianity, and known noise statistics, KF demonstrates its optimality by minimizing the error covariance (MSE).

[0061] covariance

[0062] in, This represents the updated error covariance value at time k-1. This represents the error covariance value predicted at time k from time k-1. Here, Pa,k|k-1 is the updated prior covariance at time k. The update method is the above formula. The logic is as follows: Given the posterior covariance at time k-1, we obtain the prior covariance at time k. Then, using the formula for updating the covariance below, we obtain the posterior covariance at time k, and so on, iterating. The first posterior covariance is the manually set initial state covariance Pa.

[0063] Using new measurements Update state estimates and covariance residual

[0064] in, This represents the measurement of the state at time k.

[0065] Innovation Covariance

[0066] in, The noise covariance matrix is ​​measured; specific values ​​are shown above.

[0067] Kalman gain

[0068] Status update

[0069] Update covariance

[0070] After smoothing the IMU data as described above, ESKF uses the IMU measurements to recursively correct the estimates of acceleration and its derivatives through prediction and update steps, thereby achieving a smooth estimate of the actual acceleration signal.

[0071] Similarly, filtering is performed using the IMU angular velocity to improve the accuracy and reliability of the IMU data.

[0072] When performing incremental calculations on IMU data, the trapezoidal integral is taken, and the result is: For the data from the first IMU, ,in, The output frequency of the IMU. Let be the six-dimensional vector of the IMU at time n-1. This is the six-dimensional vector of the IMU at time n. Here, the six-dimensional vector represents the three-dimensional coordinates of angular velocity and acceleration. The denominator is the sum of the one-to-one correspondences of the three-dimensional acceleration and three-dimensional angular velocity acquired by the IMU at times n-1 and n. This is also a six-dimensional vector, and the sum will also produce six numbers, which will be processed within the vector.

[0073] The following describes the linearization and discretization process of the error state equation: A 21-dimensional error state variable is established to compensate for the position, velocity, and attitude data of the IMU data and its output. This algorithm is based on the code of the reference network, and the offline processing is modified to real-time processing.

[0074]

[0075] in, Let be the state error vector. For triaxial position error, For triaxial speed error, For three-axis attitude error, For gyroscope zero bias, For accelerometer zero bias, For gyroscope scaling factor error, This refers to the accelerometer scaling factor error. The gyroscope scaling factor error and accelerometer scaling factor error are factory calibration parameters of the IMU, and the corresponding data can be obtained by querying.

[0076] The prediction equation is

[0077] in, The rate of change of the state error, Here is the state transition matrix. Let be the state error vector. The input matrix is ​​the process noise matrix, which describes the impact of process noise on the error. Process noise represents random disturbances in the system. This describes the impact of process noise on state transitions in a continuous state. An 18-dimensional noise matrix Q is constructed using angular velocity random walk (arw), acceleration random walk (vrw), angular velocity zero-bias white noise, acceleration zero-bias white noise, angular velocity proportion, and acceleration zero-bias proportion. Then, a 21×18-dimensional noise driving matrix is ​​calculated each time the inertial navigation system is updated. The noise array is calculated here. The mean is 0, and the discretized covariance matrix is ​​Q. For example, the angular velocity random walk includes three directions, where the random walk in each direction follows a Gaussian distribution, as do other noises; therefore, wt contains random data from different noise sources that conform to a Gaussian distribution. The statistics here are ultimately abstracted into Q to represent them for mathematical calculations.

[0078] Discretization yields

[0079] in, Let be the state error vector at time k. This is the discretized form of the state transition matrix, representing the state transition from time [time]. At the time state propagation, For a moment The state error vector, For at any time Process noise.

[0080] in

[0081] above Each block of the matrix is ​​a 3×3 matrix, representing position, velocity, attitude, gyroscope bias, accelerometer bias, gyroscope proportional gain, and accelerometer specific force, for a total of 21 dimensions.

[0082] State transition matrix The exponential form represents the state from time t. At the time The propagation process; t k For time k, t k-1 This is the (k-1)th time.

[0083]

[0084] Let be the state transition matrix, representing the state propagation from time t to k. Here, t represents... arrive Any point in time.

[0085] The observation equation is

[0086] in, For the observation error vector, For the observation matrix, To observe the noise vector, It represents the error of each observation and is a random quantity that follows a Gaussian distribution. The magnitude of each component reflects the measurement uncertainty in the corresponding direction. Larger values ​​indicate lower reliability of the measurement data in that direction, possibly due to sensor performance limitations, external environmental influences, or other significant interference. This data is obtained by subtracting the observation errors of the IMU and BeiDou. Observation noise is caused by positioning errors of the BeiDou sensor, such as positioning errors; process noise is caused by IMU errors, such as zero bias, scaling factor, and acceleration zero drift.

[0087] in

[0088] The above representative introduced the observation of the lever arm, in which: : A 3×3 identity matrix.

[0089] A 3×3 zero matrix.

[0090] It is the Direction Cosine Matrix (DCM) from the body frame to the navigation frame, used to transform vectors from the body frame to the navigation frame.

[0091] The lever arm vector represents the displacement vector in the machine system from the inertial measurement unit (IMU) to the positioning antenna (or other measurement point). In multi-sensor fusion, the IMU and the antenna are usually not in the same position, and the lever arm vector is used to correct this spatial deviation.

[0092] This indicates the conversion of the lever arm vector from the machine system to the navigation system.

[0093] This is the antisymmetric matrix of the lever arm vectors. In matrix operations, multiplying the antisymmetric matrix by the angular velocity (or attitude error) is equivalent to calculating the cross product (outer product) of the vectors. The observation matrix reflects the impact of attitude error on position observation, particularly the position error introduced by the lever arm effect.

[0094] The algorithm can be given initial position standard deviation, velocity standard deviation, attitude standard deviation, angular random walk, velocity random walk, gyroscope bias stability, accelerometer bias stability, gyroscope scaling factor stability, and accelerometer scaling factor stability. The position standard deviation, velocity standard deviation, and attitude standard deviation are obtained by the NAV-PVT protocol received by the Beidou receiver.

[0095] The specific ESKF algorithm is as follows: Make predictions when IMU data arrives. Prediction Equations State prediction: ,in, This represents the optimal state at time k-1. Covariance prediction:

[0096] Let Q and P be the 18-dimensional noise matrix at time k-1. k-1 It is the a priori covariance.

[0097] Update when BeiDou signal arrives. Update Equations Kalman gain calculation (ESKF error state Kalman filter): Since the data smoothing above uses KF (Kalman filter), the integrated navigation here uses ESKF (error state Kalman filter), which is a more complex nonlinear method.

[0098]

[0099] Status Update:

[0100] It is a measurement vector, representing the observation value obtained by the BeiDou Navigation Satellite System at time k. It should be changed to... More suitable.

[0101] Covariance update:

[0102] Indicates the predicted time State estimation.

[0103] Indicates the time after the update State estimation.

[0104] Indicates the predicted time The covariance matrix.

[0105] Indicates the time after the update The covariance matrix.

[0106] This represents the process noise covariance matrix. k-1 should not be used here; it is treated as a constant in the algorithm and manually set according to the IMU parameters.

[0107] This represents the measurement noise covariance matrix.

[0108] Represents the identity matrix.

[0109] Status is activated upon arrival of BeiDou signal. After the update, it will be based on the state transition matrix. Transfer the data from the current moment to the next IMU moment, ensuring an output of 200Hz per second.

[0110] Optionally, in step S4, an SLSTM network is trained using IMU data, error state variables, and position, velocity, and attitude information output by ESKF, and data augmentation is achieved using random time expansion and compression.

[0111] Specifically, the error state variable is obtained through the ESKF prediction step; the position, velocity, and attitude information output by the ESKF are obtained through the ESKF prediction and update steps; finally, the SLSTM network is trained based on the IMU data, the error state variable, and the position, velocity, and attitude information output by the ESKF to improve the overall accuracy and robustness.

[0112] The steps for data augmentation, error prediction, and model training using an SLSTM network are as follows: The data augmentation algorithm employs stochastic time expansion and compression to improve the model's generalization ability. SLSTM is more suitable for this type of task than general time series models such as LSTM.

[0113] SLSTM (Scalar LSTM) adds a scalar update mechanism to the traditional LSTM. This design optimizes the gating mechanism through fine-grained control of internal memory units, making it more suitable for processing sequence data with subtle temporal variations. SLSTM typically uses exponential gating and normalization techniques to improve the model's stability and accuracy in processing long sequence data. In this way, SLSTM can provide performance comparable to complex models while maintaining low computational complexity, making it particularly suitable for resource-constrained environments or applications requiring fast response times.

[0114] The IMU operates at a frequency of 200 Hz, and the BeiDou receiver operates at a frequency of 1 Hz. Data from the IMU is collected in batches of one second. Each IMU data point contains three-axis acceleration and three-axis angular velocity, denoted as follows:

[0115] Where k represents the data that enters at the k-th timestamp, for example, 200 data points per second, k ranges from 1 to 200. Ax, Ay, and Az represent the accelerations in the x, y, and z directions with the volume coordinate axis as the reference axis, respectively; Gx, Gy, and Gz represent the angular velocities in the x, y, and z directions with the volume coordinate axis as the reference axis, respectively.

[0116] And the corresponding three-dimensional position in the northeast direction obtained from the compensation at the previous moment. Posture Quaternions 3D velocity The data here is the result of IMU mechanical orchestration output after IMU compensation, and navigation compensation has been performed. It is denoted as...

[0117] In this context, the northeast celestial coordinate system serves as the reference coordinate system. The vector represents the three-dimensional position vectors from the initial point to the current point in the east, north, and sky directions. It is the three-dimensional data of the current position relative to the initial position in the northeast-sky coordinate system, i.e., the three-dimensional vector. A four-dimensional vector representing the current pose using quaternions; This indicates the current velocity of the object relative to the east, north, and sky directions.

[0118] Put the timestamp Compared to the previous timestamp The concatenated data is used as input for data augmentation, while also incorporating the error state obtained this time. As the input corresponding to this timestamp.

[0119] The new vector here is

[0120] All three have now been defined.

[0121] To accelerate training and meet real-time requirements, 200 adjacent pairs are used here. As a batch, let it be denoted as .

[0122] For example, arrive Record these as data points 1 to 200 of this batch.

[0123] Random time expansion and compression algorithms: Necessity: This algorithm can enhance the generalization ability of the model. In localization, if only the obtained data is used for training, the model is prone to getting trapped in local optima. By using time-spreading compression algorithms and random selection, the model can be effectively prevented from getting trapped in local optima. The algorithm is as follows: Choose a random time expansion / compression factor α, where α is randomly selected from a predetermined range (e.g., 0.8 to 1.2). Then use this α to compress or stretch the timestamps of this batch, obtaining new timestamps corresponding to the original data. Corresponding to the formula below, S remains unchanged for each x, but x is scaled.

[0124] Resample the IMU data sequence using an interpolation method.

[0125] Cubic spline interpolation is a method of interpolating data using a series of cubic polynomials, where each polynomial is valid between two data points, the entire curve is continuous at the data points, and the first and second derivatives at the data points are also continuous.

[0126] For data points and We define a spline function between these two points. for:

[0127] in , , , These are the coefficients to be determined.

[0128] Coefficient calculation:

[0129]

[0130]

[0131] Boundary conditions:

[0132] Here is the arrive Each row vector is subjected to cubic spline interpolation.

[0133] 200 data points were randomly selected from this time period to serve as the data augmentation data. Each data point is denoted as

[0134] Will It is sent to SLSTM for training, where As input for training, As labels for training. The data is first normalized and then fed into the neural network.

[0135] The formula for forward propagation SLSTM with a stabilizing gate is: Cell State:

[0136] : The cell state vector at the current time t.

[0137] Cell states are the core of LSTM, running throughout the entire network and storing long-term memory information. Information can be added to or forgotten, depending on the actions of the forget gate and the input gate. A cell state is typically a vector of real numbers, where each element can take values ​​within the real number range (usually from -1 to 1, due to the output range of the tanh function). In a multi-layer LSTM, each layer has a vector ct, which is continuously updated by the algorithm.

[0138] Forget gate vector: controls how much previous information is forgotten.

[0139] The forget gate determines what information is discarded from the cell state. It outputs a value between 0 and 1 through a sigmoid layer, where 0 represents complete forgetting and 1 represents complete retention. The forget gate vector affects the current cell state by controlling the degree to which information from the previous cell state is retained.

[0140] : The cell state at the previous time step t-1.

[0141] : Input gate vector, which controls the amount of new information input.

[0142] The input gate determines the importance of new information, i.e., the amount that should be added to the cell state. It is also expressed by a sigmoid layer, outputting a value between 0 and 1. The input gate vector controls how much new candidate information (zt) is added to the current cell state.

[0143] Cellular input, new candidate information.

[0144] Element-wise product (Hadamard product).

[0145] Normalizer State:

[0146] : The normalized state at the current time t.

[0147] : The normalized state at the previous time step t-1.

[0148] Hidden State:

[0149] in,

[0150] : The hidden state vector at the current time t, the output of the network.

[0151] : Output gate vector, controls how much information is output.

[0152] : Normalized cell state.

[0153] Cell Input:

[0154] in,

[0155] : The cell input at the current time t, and the candidate information after passing through the activation function.

[0156] : Cell input after linear transformation, inactive candidate information.

[0157] : Activation function, taking the hyperbolic tangent function tanh.

[0158] More detailed, : Weight matrix, applied to the input data.

[0159] The recurrent weight matrix rz is used to map the hidden state h_{t-1} from the previous time step to the candidate information at the current time step. It acts on the hidden state h_{t-1} from the previous time step. h_{t-1} is the hidden state vector of the SLSTM at the previous time step. h_{t-1} can be seen as a compact representation obtained after encoding the input sequence information in the previous t-1 steps; it contains information captured by the model in previous time steps (e.g., sensor data from previous time steps, past navigation state features, etc.).

[0160] The shape of rz typically matches the dimension of the hidden state. For example, if the hidden state h_{t-1} is a vector of length H, then rz is an H×H matrix, so the result of rz and h_{t-1} is still a vector of length H. rz is a trainable parameter in the network. During training, rz is continuously updated with backpropagation and optimization processes (such as gradient descent) to allow the model to better fit or predict given the training data and loss function.

[0161] Bias vector.

[0162] : The estimated value of the IMU data at the current moment, under the machine system.

[0163] : The estimated value of the navigation state at the previous moment, under the navigation system.

[0164] Input Gate:

[0165] in,

[0166] : Input gate vector, which controls the degree to which new information is written into the cell state.

[0167] : Input gate value after linear transformation.

[0168] More detailed, Weight matrix.

[0169] : Cyclic weight matrix.

[0170] Bias vector.

[0171] Forget Gate:

[0172] in,

[0173] Forget gate vector: controls the degree to which old information is forgotten.

[0174] `ft` is the output vector of the Forget Gate, which determines how much information from past states should be retained or forgotten when updating the cell state. Simply put, `ft` is calculated based on the current input and the hidden state from the previous time step, thus "filtering" the information in the cell state from the previous time step, deciding which information to retain and which to gradually forget over time. This allows the network to flexibly manage long-term and short-term historical information, thereby representing time-series data more effectively. During network parameter initialization and the early stages of training, `ft` is close to 1 (corresponding to `ft ≈ 0`). After appropriate training, `ft` will dynamically adjust according to task requirements and input features. Different elements may take values ​​ranging from 0 to tens or hundreds, mainly depending on how well the network parameters converge.

[0175] Forget gate value after linear transformation.

[0176] Output Gate:

[0177] in,

[0178] : Cyclic weight matrix.

[0179] Bias vector.

[0180] Similar to rz and bz mentioned earlier, r_o: Circular weight matrix r_o is the cyclic weight matrix applied to the hidden state h_{t-1} at the previous time step, and its dimension matches that of h_{t-1}.

[0181] bo: bias vector bo is a bias vector with the same dimension as the output gate. In neural networks, the bias term is used to ensure that the network still has a certain "baseline output level" when there is no input signal, which helps to improve the model's representation ability and stability.

[0182] In summary, r_o is the weight matrix calculated when the hidden state from the previous time step enters the output gate, and b_o is the corresponding bias term.

[0183] : Output gate vector, which controls the influence of cell state on hidden state.

[0184] : Output gate value after linear transformation.

[0185] : Sigmoid activation function.

[0186] To prevent numerical overflow caused by the exponential activation function, a stable state is introduced. The calculation formula is as follows:

[0187] The current stable state is used to adjust the input gate and forget gate to prevent numerical overflow.

[0188] The stable state at the previous moment.

[0189] Using stable states Adjust the input gate and forget gate, and calculate the stabilized input gate. And the Gate of Oblivion : Stabilized Input Gate:

[0190] Stabilized Forget Gate:

[0191] By introducing the aforementioned stabilization mechanism, SLSTM avoids numerical overflow while maintaining the same functionality as the original LSTM, thus improving the numerical stability of the model.

[0192] Considering real-time processing, a Hidden Size of 32 is chosen here, so a fully connected layer is required. This is a weight matrix with dimensions 21×32; The bias vector has a dimension of 21.

[0193] This maps the data output from the hidden layer to... And it is trained using the gradient descent algorithm.

[0194] Specifically, in step S5, when the horizontal precision factor of the satellite signal is found to be greater than 10 during real-time data reading and detection, the SLSTM prediction mechanism is triggered. At this time, the SLSTM reads... and output , here It can directly compensate for the current IMU data and the navigation data after the IMU is mechanically arranged, thereby improving the positioning accuracy and suppressing IMU divergence.

[0195] During training, a batch contains 200 data points, but during prediction, the data points are entered one by one, which is allowed in this neural network model.

[0196] Important details to note here: Originally, there were 200 data points per second. The first 199 of these were from the previous BeiDou signal correction, because the IMU outputs 200 data points per second, while BeiDou outputs only one. These 199 IMUs all underwent IMU compensation, followed by mechanical arrangement. This IMU compensation was estimated based on the fusion of the previous BeiDou data and the IMU forward propagation. In other words, these 199 IMU data points all underwent the same compensation until a new BeiDou signal appeared, at which point a new optimal compensation was recalculated. Therefore, the neural network of this invention also has this characteristic. However, it predicts the compensation based on the input IMU and the previous navigation state. But to further highlight the effect of the neural network, it compensates for each IMU. This compensation is not necessarily the same for the first 199 points, as it outputs an appropriate compensation amount based on the learned content. This makes the neural network's compensation more flexible than ESKF, and since it learns from ESKF-processed data during training, it also possesses some characteristics of ESKF. This makes the entire compensation process more flexible and accurate.

[0197] The Horizontal Dilution of Precision (HDOP) is one of the important indicators for measuring satellite signal quality. It represents the amplification factor of horizontal position error caused by the satellite's geometric distribution. The smaller the HDOP value, the better the satellite's geometric distribution and the higher the positioning accuracy; conversely, the larger the HDOP value, the worse the satellite's geometric distribution and the lower the positioning accuracy.

[0198] Mechanical orchestration refers to obtaining the position, velocity, and attitude data of the northeast sky using inertial data in the carrier coordinate system of the IMU.

[0199] This method acquires the acceleration and angular velocity information of the current carrier through an Inertial Measurement Unit (IMU); and obtains the three-axis standard deviation information of the current carrier based on the BeiDou (BD) receiver's position information and the current carrier's position. The increments of acceleration and angular velocity obtained from the IMU, along with the current position and standard deviation information obtained from the BeiDou module, are used as inputs to an Error State Extended Kalman (ESKF) filter. An IMU error model incorporating rotation and twin-sample paddling effects is established as the prediction equation, and the position difference between the IMU and BeiDou, considering the lever arm, is established as the observation equation. An SLSTM network is trained using IMU data, error state variables, and the position, velocity, and attitude information output by the ESKF. The network model is trained when the BeiDou signal is working normally. When the BeiDou signal is lost, the trained model is used to perform online corrections on the IMU data and the position, velocity, and attitude output by the ESKF. This invention features high navigation accuracy, strong anti-interference capability, and strong real-time performance.

[0200] Combination Figure 4 The following details the overall process: The program begins with the manual operation of the software to open the serial port and wait for the first BeiDou signal to arrive, used to initialize the entire program. Then, each IMU data point is aligned with a localized timestamp to ensure timing synchronization. Upon arrival of new data, it is determined whether the new data is an IMU or BeiDou signal. If it is a BeiDou signal, three-nearest-neighbor filtering is performed. Then, the accuracy factor in the data is compared with a threshold. If it is less than the threshold, the data is considered reliable, and ESKF observations, IMU, and navigation data are updated accordingly. If it is greater than the threshold, neural network prediction is performed, at which point network training ends, and the neural network output is used as compensation. This algorithm can detect the current satellite data status in real time and flexibly and adaptively change the architecture in real time to match the real-time situation. If it is an IMU signal, KF filtering is first performed to smooth acceleration and angular velocity, then IMU error compensation is performed, followed by mechanical orchestration to output the current navigation state and compensation, and forward propagation of errors. In neural network training mode, uncompensated IMU data, compensated navigation data, and the current ESKF state variables are all fed into the data augmentation module for network training. The program will terminate when the host computer issues an end flag.

[0201] The method of this invention can process and fuse IMU and navigation data in real time when the BeiDou receiver and IMU are working normally, ensuring high-precision positioning results.

[0202] By preprocessing the input IMU and BeiDou data to filter out noise and improve data quality, the accuracy and stability of subsequent processing and compensation are enhanced.

[0203] In addition, a real-time data augmentation method based on SLSTM (Scalar Long Short-Term Memory) was employed. This method improves the robustness and adaptability of the neural network model through data augmentation techniques, enabling the system to maintain high-precision positioning even in complex environments.

[0204] During the training phase, the input includes current IMU information, the previous time step's position, velocity, and attitude information, and a 21-dimensional state variable. This method trains the neural network so that during the prediction phase, it can take current IMU information and the previous time step's position, velocity, and attitude information as input, and output the 21-dimensional state variable to compensate for IMU data and the final positioning result, ensuring high-precision positioning even in the event of satellite signal loss.

[0205] The content disclosed above is only a preferred and feasible embodiment of the present invention, and is not intended to limit the scope of protection of the present invention. Therefore, all equivalent technical changes made based on the content of the present invention specification and drawings are included within the scope of protection of the present invention. Furthermore, the elements therein can be updated as technology develops.

Claims

1. A real-time positioning method for forest environments, characterized in that, The method includes the following steps: S1: Solving IMU data and BeiDou data; S2: Establish the measurement equations for BeiDou and the state equations for the IMU; S3: Utilize ESKF to achieve data fusion and output compensation; S4: SLSTM network training and data augmentation; S5: After BeiDou loses lock, the SLSTM network performs real-time prediction. After BeiDou recovers, continue to step S3.

2. The real-time positioning method for forest environments as described in claim 1, characterized in that, In step S1, the navigation information of the carrier based on the IMU is calculated and the increment is transmitted to the host computer; the Beidou receiver acquires the navigation information of the carrier based on Beidou in real time and transmits it to the host computer; the IMU data and Beidou data are timestamped and aligned on the host computer.

3. The real-time positioning method for forest environments as described in claim 2, characterized in that, In step S1, timestamp alignment uses the three nearest neighbor averaging algorithm.

4. A real-time positioning method for forest environments as described in claim 3, characterized in that, In step S1, the first BeiDou information is obtained using the three nearest neighbor average algorithm as the initial point. The current longitude, latitude, and altitude information are recorded, along with the BeiDou timestamp. and current local time Then, complete the following algorithm steps: Data acquisition: Collect 10 GPS data points, denoted as P1, P2, ..., P10; Distance calculation: Calculate the Euclidean distance between these 10 data points; GPS data point selection: Select the three data points closest to the initial point, i.e., K=3; Average calculation: Average the positions of these three data points to obtain the current position; IMU data point processing: Collect the IMU data point closest to the initial point and record the IMU's timestamp. and current local time ;Pick As a timestamp compensation for both, all IMU timestamps are synchronized and compensated.

5. A real-time positioning method for forest environments as described in claim 2, characterized in that, In step S2, the IMU's state equation is used to describe the motion state of the carrier, and the BeiDou observation equation is used to correlate BeiDou data and IMU data.

6. A real-time positioning method for forest environments as described in claim 5, characterized in that, In step S3, ESKF fuses BeiDou data and IMU data through measurement equations and state equations. During the fusion process, ESKF predicts the current position, velocity and attitude of the carrier, and reduces the accumulated error of IMU data by correcting it with BeiDou data.

7. A real-time positioning method for a forest environment as described in claim 6, characterized in that, In step S4, the SLSTM network is trained using IMU data, error state variables, and position, velocity, and attitude information output by ESKF, and data augmentation is achieved using random time expansion and compression.

Images