Geological occurrence measuring and positioning handheld terminal and dual-task cooperative control method thereof

Abstract

The application provides a geological occurrence measurement and positioning handheld terminal and a dual-task cooperative control method thereof, and belongs to the technical field of geological exploration measurement and combined navigation positioning. The terminal comprises a main control MCU, a multi-source sensor, a power supply unit, a display module and a storage module. The main control MCU runs a dual-task heterogeneous real-time firmware based on a real-time operating system. A first task executes a multi-source sensor fusion positioning algorithm of an error state extended Kalman filter, fusing MEMS nine-axis inertial sensor, GNSS positioning module and barometer data. A second task executes a rock stratum surface fitting detection and occurrence attitude solving algorithm. The two tasks share the same inertial sensor raw data stream. The terminal has a flat bottom surface. When fitting detection is performed to determine whether the bottom surface is fitted with the rock stratum surface, occurrence solving is automatically triggered, attitude angles are mapped into strike, dip direction and dip angle, and are stored in association with position coordinates. The application realizes deep cooperation of positioning and occurrence measurement functions, maintains continuous positioning through inertial navigation and zero velocity update in a GNSS denial environment, effectively suppresses false triggering through fitting detection, and is suitable for field operation scenarios such as geological mapping, mineral exploration and geological disaster assessment.

Description

Technical Field

[0001] This invention relates to the field of geological exploration and measurement instruments and integrated navigation and positioning technology, specifically to a handheld terminal for geological occurrence measurement and positioning that integrates multi-source sensor fusion positioning and digital measurement of rock strata occurrence, and its dual-task collaborative control method. Background Technology

[0002] Geological occurrence (dip, dip angle, strike) is a fundamental parameter in geotechnical engineering investigation, mineral exploration, and geological hazard assessment. Its measurement accuracy directly affects the reliability of engineering decisions and geological modeling. Currently, the field measurement of geological occurrence mainly relies on the following schemes: The first scheme is the traditional mechanical geological compass. This type of geological compass uses a mechanical pointer, magnetic needle, and inclinometer in conjunction with a scale for manual reading. It has the following shortcomings: (1) It requires manual reading and handwriting of occurrence data, which is inefficient and prone to errors; (2) The data needs to be manually entered into the computer later, which cannot achieve digital acquisition and real-time storage; (3) In areas with strong magnetic interference such as iron ore bodies, the magnetic needle can deviate by tens of degrees, making the measurement results unreliable. The second approach is based on a geological compass application for smartphones. In recent years, apps that utilize the built-in nine-axis sensor of a smartphone (such as FieldMoveClino) have emerged to measure the attitude. However, as a general consumer electronics product, smartphones have the following limitations: (1) Smartphones lack dedicated positioning enhancement modules (such as barometers / inertial navigation systems), and cannot maintain continuous positioning in environments with weak GPS signals, such as canyons and tunnels; (2) Smartphones lack dedicated physical measurement triggering mechanisms, and cannot effectively determine whether the sensor is in contact with the rock surface. The measurement result is essentially a "handheld posture" rather than a "rock surface posture"; (3) Data is stored in the app's proprietary format, making it difficult to interface with professional geological modeling software. The third approach is to measure profiles based on the traverse method. This method requires the use of a compass to determine the orientation, a tape measure to measure the distance, and requires that adjacent measuring points be in line of sight. Since the data needs to undergo two projection processes, namely "projecting the spatial traverse onto the horizontal plane" and "re-projecting it onto the baseline", it often leads to the accumulation of errors. Moreover, the endpoint of the traverse often does not coincide with the actual endpoint, requiring manual correction. This not only makes the operation cumbersome but also limits the accuracy. The fourth approach is a separate integrated navigation and measurement terminal. Existing technologies disclose a scheme that separates the positioning module and the measurement sensing module, with the main control unit and the sensors connected by cables. Although this approach achieves basic functions, it has obvious shortcomings: (1) The separate design results in a large device size and poor portability, which is not conducive to operations in complex terrain in the field; (2) The positioning module and the measurement module work independently, lacking data-level coordination, and cannot automatically associate the attitude measurement results with the positioning trajectory; (3) Its anti-magnetic interference strategy still relies on magnetometer data, and the heading angle accuracy is still limited in a strong magnetic environment. In summary, the existing technology lacks a handheld terminal device that deeply integrates multi-source fusion positioning and nine-axis attitude measurement on the same hardware platform and realizes the coordinated operation of positioning and attitude measurement functions. Summary of the Invention

[0003] The technical problem to be solved by this invention is: addressing the issue that existing geological attitude measurement terminals have independent and uncoordinated positioning and navigation functions and attitude measurement functions, resulting in insufficient continuous positioning capability and low measurement reliability in GPS-denied environments. This invention provides a handheld terminal that integrates multi-source fusion positioning and nine-axis attitude measurement depth on the same hardware platform, along with a dual-task collaborative control method.

[0004] To achieve the above objectives, the present invention provides a handheld terminal for geological attitude measurement and positioning, including a main control MCU (1), a multi-source sensor (2), a power supply unit (3), a display module (4), a storage module (5), and a flat bottom surface (6). The main control MCU runs a dual-task heterogeneous real-time firmware based on a real-time operating system. The first task executes a multi-source sensor fusion positioning algorithm with an error state extended Kalman filter at a first update rate, and the second task executes a rock surface adhesion detection and attitude calculation algorithm at a second update rate. The two tasks share the raw data stream of the same MEMS nine-axis inertial sensor. The terminal also has a flat bottom surface for adhering to the rock surface. The second task triggers attitude calculation when it detects that the flat bottom surface is in contact with the rock surface.

[0005] Furthermore, the first task receives data from the MEMS nine-axis inertial sensor, GNSS positioning module, and barometer through a message queue, performs prediction and correction steps, and generates continuous terminal position coordinates which are stored in the storage module; the second task reads inertial sensor data through zero-copy global variables, and after multi-level bonding detection and confirmation, maps the three-axis attitude angles to the three elements of rock strata attitude (strike, dip, and dip angle), and associates them with the position coordinates to store them in the storage module.

[0006] Furthermore, the thread priority of the first task is higher than that of the thread priority of the second task.

[0007] Furthermore, the multi-source sensor includes a MEMS nine-axis inertial sensor, a barometer, and a multi-constellation GNSS positioning module, wherein the MEMS nine-axis inertial sensor integrates a three-axis accelerometer, a three-axis gyroscope, and a three-axis magnetometer.

[0008] Furthermore, the magnetometer module in the MEMS nine-axis inertial sensor has a built-in ferromagnetic correction algorithm, and the second task uses the corrected magnetometer data when performing attitude calculation.

[0009] Furthermore, in the second task, under strong magnetic interference environment, the gyroscope and accelerometer in the nine-axis inertial sensor calculate the state of orientation through a fusion algorithm, achieving a high-precision state of orientation measurement.

[0010] Furthermore, the digital filtering parameters of the first task include the process noise covariance matrix of the error state extended Kalman filter, and the observation noise covariance matrices corresponding to the GNSS positioning module, zero velocity correction, and barometer measurement, respectively.

[0011] Furthermore, the fitting detection parameters used in the second task include acceleration modulus threshold, angular velocity root mean square threshold, button debouncing delay, and continuous stabilization time.

[0012] Furthermore, the storage module is an SD card, a TF card, or an onboard Flash memory.

[0013] As a preferred technical solution of the present invention, the main control MCU adopts an STM32H723ZGTx chip, an ARM Cortex-M7 core, a built-in single-precision floating-point unit and a memory protection unit, and runs the RT-Thread Nano real-time operating system.

[0014] As a preferred embodiment of the present invention, the error state extended Kalman filter adopts a 15-dimensional error state vector, including three-dimensional position error, three-dimensional velocity error, three-dimensional attitude error, three-dimensional gyroscope zero bias error, and three-dimensional accelerometer zero bias error.

[0015] As a preferred technical solution of the present invention, the multi-level bonding detection includes: physical measurement trigger button, hardware level detection of physical measurement trigger button, acceleration modulus detection, angular velocity root mean square value detection, button debouncing delay and continuous stabilization time monitoring.

[0016] As a preferred technical solution of the present invention, the terminal further includes a positioning mode adaptive switching logic, which calculates a comprehensive signal quality index based on the number of visible satellites and the average signal-to-noise ratio, and operates in GNSS / inertial navigation deep combination mode when the GNSS signal quality is higher than a preset threshold, and switches to inertial navigation / barometer fusion mode when it is lower than the preset threshold.

[0017] As a preferred technical solution of the present invention, the display interface of the display module includes multiple cyclically switching pages: the first page is a geological profile map, which plots the relationship between the profile distance and altitude of the trajectory points; the second page is a state details page, which displays the three elements of state and the corresponding latitude, longitude and altitude; the third page is a top view of the trajectory, which displays the markers from the start point to the end point, as well as the trajectory and a compass rose.

[0018] The beneficial effects of this invention include: (i) deep fusion positioning of multi-source sensors: a 15-dimensional error state extended Kalman filter deeply fuses GNSS positioning data, MEMS nine-axis inertial sensor data, and barometer data, achieving high-precision combined positioning when GNSS signal is good, and maintaining continuous positioning capability through inertial navigation plus zero-velocity correction algorithm in GNSS denied environments; (ii) zero-velocity correction significantly reduces drift. By combining three-level joint detection (horizontal velocity threshold, vertical velocity threshold, and angular velocity magnitude threshold) with a preset continuous stable duration to trigger zero-velocity correction, the position drift of the inertial navigation system is effectively suppressed, and the positioning accuracy under long-term GNSS signal absence is significantly improved; (III) High-precision digital measurement of attitude, with dip accuracy better than or equal to ±1.0 degrees and dip accuracy better than or equal to ±1.5 degrees, which is better than traditional mechanical geological compasses, realizes full digital measurement, and avoids manual reading errors; (IV) Dual-task firmware collaborative work, the positioning task and the attitude measurement task realize data sharing and collaboration through message queues and zero-copy global variables, realize the integrated workflow of "measurement is positioning, positioning is recording", fill the gap of separation between positioning and attitude measurement functions, and facilitate the use of geologists; (V) Four-level fit detection confirmation, The multi-level confirmation process, which includes acceleration modulus threshold, angular velocity root mean square threshold, button debounce delay, and continuous stabilization time, effectively suppresses false triggering caused by hand shaking, breathing, and muscle tremors, and improves the reliability of attitude measurement; (vi) Data storage and on-site visualization: It supports the storage of data files in multiple formats, and realizes on-site visualization of profile diagrams, attitude details, and trajectory top views through the LCD display module, realizing the on-site closed-loop workflow of "measurement yields the diagram". Attached Figure Description

[0019] Appendix Figure 1 This is a simplified hardware structure diagram of an embodiment of the present invention; attached. Figure 2 This is a hardware structure block diagram of an embodiment of the present invention; attached. Figure 3 This is a flowchart illustrating the dual-task scheduling process according to an embodiment of the present invention; attached. Figure 4 This is a schematic diagram of the positioning mode switching state machine according to an embodiment of the present invention; (See attached diagram) Figure 5 This is a schematic diagram of the positioning mode switching state machine according to an embodiment of the present invention (Embodiment 7); (See attached diagram) Figure 6 This is a flowchart of the bonding detection process according to an embodiment of the present invention; attached. Figure 7 This is a schematic diagram of the data file format according to an embodiment of the present invention.

[0020] Explanation of reference numerals in the attached diagram: 1-Main control MCU; 2-Multi-source sensor; 3-Power supply unit; 4-Display module; 5-Storage module; 6-Flat bottom surface. Detailed Implementation

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

[0022] In the description of this invention, it should be understood that the terms "center," "longitudinal," "lateral," "upper," "lower," "front," "rear," "left," "right," "vertical," "horizontal," "top," "bottom," "inner," and "outer," etc., indicate the orientation or positional relationship based on the orientation or positional relationship shown in the accompanying drawings. They are used only for the convenience of describing this invention and simplifying the description, and do not indicate or imply that the device or element referred to must have a specific orientation, or be constructed and operated in a specific orientation. Therefore, they should not be construed as limiting this invention.

[0023] Furthermore, the terms "first" and "second" are used for descriptive purposes only and should not be construed as indicating or implying relative importance or implicitly specifying the number of technical features indicated. Thus, a feature defined as "first" or "second" may explicitly or implicitly include one or more of that feature. In the description of this invention, "a plurality of" means two or more, unless otherwise explicitly specified.

[0024] Example 1: Refer to Appendix Figure 2 This embodiment provides a specific hardware implementation scheme for a handheld terminal for geological occurrence measurement and positioning. The terminal hardware structure of this embodiment includes: a main control MCU (1), a multi-source sensor (2), a power supply unit (3), a display module (4), a storage module (5), and a flat bottom surface (6).

[0025] The main control MCU (1) uses an STM32H723ZGTx chip, an ARM Cortex-M7 core, an LQFP144 package, a main frequency of 64MHz, and a built-in single-precision floating-point unit and memory protection unit. The main control MCU runs a real-time operating system and is configured with multi-level thread priorities, system tick interrupts, message queue inter-process communication mechanisms, and memory heap management. The main control MCU integrates program memory and data memory on-chip. The program memory is used to store firmware code and font data, and the data memory provides memory space for message queues and extended Kalman filter covariance matrix. The main control MCU also integrates a direct memory access controller, which is used for data transfer in inertial sensor data reception, GNSS positioning module data reception, and storage module read / write operations. The main control MCU integrates a physical measurement trigger button, which is used to trigger effective adhesion and enter the attitude data acquisition stage when the flat bottom surface adheres to the rock surface.

[0026] The multi-source sensor (2) includes a MEMS nine-axis inertial sensor, a barometer, and a multi-constellation GNSS positioning module.

[0027] The MEMS nine-axis inertial sensor uses the MPU9250 chip, integrating a three-axis accelerometer, a three-axis gyroscope, and a three-axis magnetometer. The accelerometer range is configurable to ±2g, ±4g, ±8g, or ±16g; the gyroscope range is configurable to ±250° / s, ±500° / s, ±1000° / s, or ±2000° / s; and the magnetometer range is ±4800μT. The MEMS nine-axis inertial sensor connects to the main control MCU via a UART interface, transmitting three-axis acceleration, three-axis angular velocity, three-axis geomagnetic data, and attitude angles calculated by the embedded digital motion processor in fixed-length data frame format. The main control MCU receives data byte-by-byte using an interrupt frame synchronous state machine with a sampling rate of 100Hz. The magnetometer module incorporates a ferromagnetic correction algorithm to compensate for interference from hard and soft iron.

[0028] The barometer employs a high-precision SPL06-007 MEMS barometric pressure sensor with a resolution better than or equal to 0.2 Pa and a measurement range of 300 hPa to 1200 hPa. The barometer connects to the main control MCU via an SPI interface. The main control MCU reads multiple sets of signed calibration coefficients built into the barometer and performs three-stage compensation on the raw pressure and temperature values: temperature compensation, pressure polynomial compensation, and standard barometric altitude conversion, outputting the compensated ambient pressure and altitude. The barometer sampling period is 30 ms, and the update rate is approximately 33 Hz.

[0029] The multi-constellation GNSS positioning module uses a U-BLOX NEO-8M or NEO-9N series multi-constellation receiver, supporting joint positioning across GPS L1 band, GLONASS G1 band, and BeiDou B1 band. The GNSS positioning module connects to the main control MCU via a UART interface, supports the NMEA 0183 protocol, and can parse at least GGA positioning data statements, RMC recommended minimum navigation information statements, and GSA accuracy factor and satellite status statements. The GNSS positioning module outputs latitude and longitude, altitude, ground velocity, positioning mode, and accuracy factor, with an update rate of 1Hz. Latitude and longitude are converted from degree-minute format to decimal purity format.

[0030] The display module (4) uses a TFT-LCD color screen and is connected to the main control MCU via an SPI interface. The display interface of the display module includes multiple cyclically switching pages: the first page is a geological profile, which plots the relationship between the profile distance and elevation of the trajectory points and supports various lithological fillings and attitude symbols; the second page is the attitude details page, which displays the strike, dip, dip angle and corresponding latitude, longitude, elevation and timestamp; the third page is a top view of the trajectory, displayed in a north-south orientation, and includes a scale bar, grid lines, start and end point marks and a compass rose.

[0031] The storage module (5) uses an SD card and connects to the main control MCU via the SDMMC interface. It adopts a 4-bit SDIO mode and supports the FatFs file system. The storage module uses a spinlock to protect against concurrent multi-threaded access. The stored files include: fused trajectory data file (binary format), trajectory index file, exported data file (CSV format), inertial sensor raw data file, and GNSS raw data file.

[0032] The power supply unit (3) provides working power to the above modules.

[0033] The terminal also has a flat bottom surface for adhering to the rock surface. This flat bottom surface is located at the bottom of the terminal housing and is used to make contact with the rock surface during measurement.

[0034] Example 2: Refer to Appendix Figure 3 This embodiment provides a specific implementation scheme for dual-task heterogeneous real-time firmware running on the main control MCU.

[0035] The main control MCU runs a dual-task heterogeneous real-time firmware based on a real-time operating system. The real-time operating system uses RT-Thread Nano and is configured with multi-level thread priorities (e.g., 32 priority levels), system tick interrupts (e.g., 1000Hz, i.e., 1ms / tick), message queue inter-process communication mechanisms, and memory heap management.

[0036] The system thread architecture includes at least the following threads: an IMU producer thread, which reads data frames from the MEMS nine-axis inertial sensor at a 100Hz cycle, parses them, and sends them to the fusion thread via a message queue; a GPS producer thread, which polls the NMEA parser at a 200ms cycle, packages valid data, and sends it to the fusion thread via a message queue; a barometer reading thread, which reads barometer data via SPI at a 30ms cycle and sends it to the fusion thread via a message queue; a fusion thread, which executes the prediction and correction steps of the error state extended Kalman filter in a 200ms main loop; and a state display thread, which reads the most recent frame of data from the MEMS nine-axis inertial sensor at a 200ms cycle, calculates the attitude angle and state, and updates the LCD display.

[0037] The inter-thread communication protocol is as follows: The IMU producer thread sends an `imu_msg_t` structure message to the fusion thread through a message queue with a depth of 256. This structure contains three-axis acceleration, three-axis angular velocity, and time increment. The GPS producer thread sends a `gps_msg_t` structure message to the fusion thread through a message queue. This structure contains latitude and longitude, altitude, accuracy factor, positioning type, and number of satellites. The barometer reading thread sends a `baro_msg_t` structure message to the fusion thread through a message queue. This structure contains altitude, air pressure, and temperature. The IMU producer thread passes `mpu9250_data_t` structure data to the state display thread via zero-copy through a volatile global variable. This structure contains multiple sets of sensor data and attitude angles.

[0038] The dual-task heterogeneous real-time firmware includes a first task and a second task.

[0039] The first task executes a multi-source sensor fusion localization algorithm using an error-state extended Kalman filter at a first update rate. The first task receives data from a MEMS nine-axis inertial sensor, a GNSS positioning module, and a barometer via a message queue, performs prediction and correction steps, and generates continuous terminal position coordinates which are stored in a storage module. The thread priority of the first task is higher than that of the second task. The digital filtering parameters of the first task include the process noise covariance matrix of the error-state extended Kalman filter and the observation noise covariance matrices corresponding to the GNSS positioning module, zero-velocity correction, and barometer measurements, respectively.

[0040] The second task executes the rock strata alignment detection and attitude calculation algorithm at the second update rate. The second task reads MEMS nine-axis inertial sensor data through zero-copy global variables. After multi-level alignment detection confirmation, it maps the three-axis attitude angles to the three elements of rock strata attitude (strike, dip, and dip angle) and stores them in the storage module in association with the position coordinates. The second task uses magnetometer data corrected by a ferromagnetic correction algorithm when performing attitude calculation. The alignment detection parameters used in the second task include acceleration modulus threshold, angular velocity root mean square threshold, button debouncing delay, and sustained stabilization time.

[0041] Both tasks share the raw data stream from the same MEMS nine-axis inertial sensor.

[0042] Example 3: This example provides a specific implementation scheme for a multi-source sensor fusion positioning method using an error state extended Kalman filter for the terminal.

[0043] The error state extended Kalman filter uses a 15-dimensional error state vector dx=[dp(3), dv(3), dθ(3), db_g(3), db_a(3)]^T, where dp is the three-dimensional position error, dv is the three-dimensional velocity error, dθ is the three-dimensional attitude error, db_g is the three-dimensional gyroscope bias error, and db_a is the three-dimensional accelerometer bias error. The indices are defined as follows: position index IDX_POSI=0, velocity index IDX_VEL=3, attitude index IDX_ORI=6, gyroscope bias index IDX_GYRO_B=9, and accelerometer bias index IDX_ACC_B=12.

[0044] The nominal states include: position vector, velocity vector, attitude quaternion, rotation matrix from navigation coordinate system to body coordinate system, gyroscope bias, accelerometer bias, and latitude, longitude and altitude of the origin of the WGS84 reference ellipsoid.

[0045] The diagonal elements of the initial covariance matrix P0 are set as follows: position covariance is 10m², velocity covariance is 1 (m / s)², attitude covariance is 0.1rad², gyroscope zero-bias covariance is 0.01 (rad / s)², and accelerometer zero-bias covariance is 0.1 (m / s²)².

[0046] The filter operation process is as follows: Initialization steps: When GNSS positioning is valid (positioning type is valid and the number of satellites is not less than 4), initialize the WGS84 origin with GNSS position, set the initial covariance of position, velocity, attitude and sensor zero bias, and mark the filter initialization as complete.

[0047] IMU prediction steps: Perform the following operations sequentially on each frame of inertial data: (a) Acceleration zero bias correction: acc_c = acc_raw - accel_bias; (b) Gravity component subtraction: The gravity vector (0,0,g) of the navigation coordinate system is rotated to the body coordinate system by quaternion conjugate rotation, and the gravity component is subtracted from the accelerometer measurement; (c) Position prediction: pos += vel × dt + 0.5 × acc_c_ned × dt²; (d) Velocity prediction: vel += acc_c_ned × dt; (e) Quaternion update: q = q ⊗ exp((gyro - gyro_bias) × dt / 2), then normalize; (f) Covariance propagation: P = F × P × F^T + G × Q × G^T, where F is the state transition matrix, G is the noise control matrix, and Q is the process noise covariance matrix.

[0048] The block form of matrix F is: dp' = I3 × dp + I3 × dt × dv; dv' = I3 × dv - [a]×× dt × dθ - R(q) × dt × db_a, where [a]× is the acceleration cross product matrix and R(q) is the quaternion rotation matrix; dθ' = I3 × dθ - [ω]× × dt × dθ - I3 × dt × db_g; db_g' = I3 × db_g; db_a' = I3 × db_a.

[0049] GNSS Correction Steps: When GNSS positioning is valid (positioning type valid and number of satellites not less than 4), calculate the observation noise covariance using the accuracy factor, and perform position observation correction on the extended Kalman filter. In the observation update step, calculate the innovation covariance S = C × P × C^T + R, the Kalman gain K = P × C^T × S⁻¹, and the update covariance P = (I - K × C) × P. The GNSS observation noise covariance R = (pdop × 2.0m)² × I³.

[0050] Zero-velocity correction steps: A stationary state is determined by three conditions: horizontal velocity is below a preset horizontal velocity threshold, absolute vertical velocity is below a preset vertical velocity threshold, and angular velocity magnitude is below a preset angular velocity threshold. The stationary time is accumulated when all three conditions are met; if any condition is not met, the accumulated time is reset to zero. Once the accumulated stationary time reaches a preset stable duration, zero-velocity correction is triggered, setting the velocity observations in the Kalman filter to zero. The zero-velocity correction observation noise covariance R = (0.01 / confidence)² × I³.

[0051] Barometer calibration steps: Using the altitude output by the barometer as the observed value, perform altitude observation calibration on the extended Kalman filter using the preset noise standard deviation.

[0052] Error state injection steps: After each correction, inject the error state increment into the nominal state: pos += dx[0:3], vel += dx[3:6], q = q ⊗ exp(dx[6:9]), gyro_bias += dx[9:12], accel_bias += dx[12:15], and then clear the error state vector to zero.

[0053] The conversion from GNSS latitude and longitude to navigation coordinates is based on the Earth ellipsoid model, and the latitude and longitude difference is converted into northward, eastward and groundward components according to the meridian curvature radius and the trochanter curvature radius.

[0054] The matrix operations of the extended Kalman filter are performed by a matrix operation library, including 3×3 matrix multiplication, 3×3 matrix inversion, 15×15 matrix multiplication, innovative covariance calculation, Kalman gain calculation, and covariance update function.

[0055] Example 4: This example provides a specific implementation scheme for an elevation complementary filter.

[0056] The elevation complementary filter fuses the barometer high-frequency update and the GNSS low-frequency update with a preset complementary coefficient and an offset smoothing coefficient to generate a fused height as the input value for the extended Kalman filter barometer observation correction.

[0057] The parameters of the complementary filter are set as follows: complementary coefficient α = 0.98 (range 0.95 to 0.99), bias smoothing coefficient = 0.95.

[0058] When the barometer is updated (around 33Hz), the fused height calculation formula is: fused_alt = baro_alt + bias.

[0059] During GNSS updates (approximately 1 Hz), the initial fused height is directly initialized using the GNSS altitude and the initial bias is calculated when the GNSS signal is first received: fused_alt = gps_alt, bias = gps_alt - baro_alt; during subsequent GNSS updates, the bias tracking formula is: bias = 0.95 × bias + 0.05 × (gps_alt - baro_alt), and the fused height is: fused_alt = 0.98 × fused_alt + 0.02 × gps_alt.

[0060] The fused height output by the elevation complementary filter is injected into the error state extended Kalman filter using a preset noise standard deviation as the barometer observation correction value.

[0061] Example 5: This example provides a specific implementation scheme for a method of measuring the state of a combined module under strong magnetic interference. The core of the scheme is to replace "magnetic orientation" with "motion orientation".

[0062] The tilt angle is directly calculated from the gravity sensed by the accelerometer; the heading (or azimuth) is calculated using the GNSS velocity vector as the equipment moves, and is used as the north reference.

[0063] Extended Kalman Filter (EKF) deeply integrates accelerometer attitude with GNSS heading, correcting gyro integral drift in real time without the need for geomagnetic intervention.

[0064] The solution outputs continuous output when the device is moving, and when it is static, it relies on a high-precision gyroscope to sense the Earth's rotation to find north, ultimately avoiding magnetic field interference and achieving accurate measurement in all scenarios.

[0065] Example 6: This example provides a specific implementation scheme for a digital measurement method of rock strata attitude for the terminal.

[0066] The terminal has a flat bottom surface for adhering to the rock surface, and the second task triggers attitude calculation when it detects that the flat bottom surface is adhering to the rock surface.

[0067] Attitude calculation steps: Calculate roll and pitch angles using accelerometer data via trigonometric transformation: roll = atan2(ay, az); pitch = atan2(-ax, √(ay² + az²)); Calculate yaw angle using magnetometer data with tilt compensation. Magnetometer compensation calculation is: mx_h = mx × cos(pitch) + my × sin(roll) × sin(pitch) + mz × cos(roll) × sin(pitch), my_h = my × cos(roll) - mz × sin(roll), yaw = atan2(-my_h, mx_h), normalized to the range of 0° to 360°.

[0068] The attitude mapping steps are as follows: The normal vector of the body coordinate system is transformed to the navigation coordinate system through the attitude rotation matrix, and the dip angle, strike angle and tilt angle are calculated. The body z-axis (0,0,1) is transformed to the navigation coordinate system through the rotation matrix R(yaw)×R(pitch)×R(roll) to obtain the normal vector n. The dip angle is the azimuth angle of the horizontal component of the normal vector: dip_dir = atan2(n[1], n[0]), normalized to the range of 0° to 360°. The strike angle is the dip angle minus 90°: strike = dip_dir - 90°, normalized to the range of 0° to 360°. The tilt angle is: dip = 90° - |pitch|.

[0069] Adhesion testing steps: Refer to the attached document. Figure 6The bonding status is determined through a multi-level confirmation process. The first level of confirmation consists of hardware level detection of the button trigger and acceleration magnitude detection, requiring the acceleration magnitude to be within a preset gravitational acceleration range (e.g., 0.98g to 1.02g). The second level of confirmation consists of root mean square angular velocity detection and button debouncing delay, requiring the root mean square angular velocity to be below a preset threshold (e.g., 0.5 rad / s, calculation window of 200ms) and the button to be pressed continuously for more than a preset debouncing delay (e.g., 300ms). After both levels of conditions are met, a continuous stabilization monitoring stage is entered, requiring the acceleration and angular velocity conditions to remain stable for a preset time (e.g., 3 seconds) before a valid bonding is determined.

[0070] Occurrence locking and association steps: After determining effective alignment, the three elements of occurrence (strike, dip, and dip angle) are locked. The occurrence data is associated with the current position coordinates and stored in the storage module in a structured format. When the acceleration modulus deviates from the range of gravitational acceleration, or the angular velocity exceeds the preset threshold, or the button is released, the alignment state is determined to be released, and occurrence data acquisition stops.

[0071] Example 7: This example provides a specific implementation scheme for the adaptive switching logic of the positioning mode.

[0072] The multi-source sensor fusion localization algorithm includes adaptive switching logic for localization sources. (See attached diagram.) Figure 5 The system calculates the comprehensive signal quality index Q = α × (N_sat / N_max) + β × (SNR_avg / SNR_max) based on the number of visible satellites and the average signal-to-noise ratio, where α+β=1, N_sat is the number of visible satellites, N_max is the preset maximum number of satellites, SNR_avg is the average signal-to-noise ratio, and SNR_max is the preset maximum signal-to-noise ratio.

[0073] When the quality index is not lower than the preset threshold (corresponding to no less than 4 satellites and an accuracy factor of less than 5), the system operates in GNSS / inertial navigation deep combination mode, and the extended Kalman filter simultaneously receives IMU predictions and GNSS observation corrections.

[0074] When the quality index is below the preset threshold, the system switches to the inertial navigation / barometer fusion mode. The extended Kalman filter only performs IMU prediction steps and barometer observation corrections, and the altitude drift is suppressed by barometer data.

[0075] The handover process employs a hysteresis comparison strategy, delaying the handover for a preset time when the quality index crosses the threshold to prevent frequent handovers caused by boundary jitter.

[0076] Example 8: This example provides a specific implementation scheme for the display interface and data storage.

[0077] The display module's interface includes multiple cyclically switching pages. The first page is a geological profile map, reading trajectory points from the storage module. The horizontal axis represents the profile distance, and the vertical axis represents the altitude. It supports various lithological fillings and attitude symbol annotations. The second page is the attitude details page, displaying the latest locked strike, dip, dip angle, latitude, longitude, altitude, and recording time. The third page is a top-down view of the trajectory, displayed in a north-south orientation, with a scale of 50 pixels corresponding to 100 meters. It includes grid lines, a green start point marker, a red end point marker, and a north arrow. The system cycles through the three pages using a toggle button.

[0078] The storage module uses spinlocks to protect against concurrent multi-threaded access. The storage files include: a fused trajectory data file (binary format, each record containing latitude and longitude, altitude, three-axis velocity, attitude angle, attitude three elements, attitude validity indicator, and timestamp); a trajectory index file (containing magic number, version number, total number of points, data offset, and latest offset); an exported data file (CSV format, containing 15 fields: sequence number, latitude, longitude, altitude, northward velocity, eastward velocity, groundward velocity, roll angle, pitch angle, heading angle, strike, dip angle, dip direction, attitude validity indicator, and timestamp); an inertial sensor raw data file (binary format, containing three-axis acceleration, three-axis angular velocity, and timestamp); and a GNSS raw data file (binary format, containing latitude and longitude, altitude, velocity, accuracy factor, positioning type, number of satellites, and timestamp). The maximum number of trajectory points is 10,000.

[0079] See attached document Figure 7 The exported CSV file contains 15 fields, in the following order: serial number, latitude, longitude, altitude, north velocity, east velocity, ground velocity, roll angle, pitch angle, heading angle, strike, dip angle, dip direction, valid index of attitude, and timestamp.

[0080] Example 9: Based on Example 5, this example optimizes the fit detection process of the physical measurement trigger button.

[0081] A secondary confirmation mechanism has been added: After the physical measurement trigger button is pressed and the acceleration modulus is within the range of 0.98g to 1.02g, the system calculates the root mean square (RMS) value of the angular velocity within a 200ms window. Only when the RMS angular velocity value remains below 0.5 rad / s and the button press duration exceeds the preset debounce delay (default 300ms) is the system considered to have achieved valid contact and enter the orientation data acquisition stage. This additional condition effectively suppresses accidental triggering caused by breathing and muscle tremors during stable handheld operation.

[0082] Example 10: Based on Example 3, this example further optimizes the zero-speed correction function.

[0083] To address the slow movement (typically 0.5 to 2 m / s) and frequent stops for observation by geological personnel, a zero-velocity correction function was added to the inertial navigation / barometer fusion mode: when the acceleration magnitude output by the MEMS nine-axis inertial sensor stabilizes near the gravitational acceleration g and the angular velocity magnitude remains below 0.1 rad / s for more than 3 seconds, the terminal is determined to be stationary. The velocity observation value in the Kalman filter is set to zero, and the velocity error is used as the observation to correct the position drift of the inertial navigation system. Specifically, the zero-velocity correction criteria are: horizontal velocity less than 0.05 m / s, absolute vertical velocity less than 0.02 m / s, and angular velocity magnitude less than 0.1 rad / s, all three conditions must be met simultaneously and for more than 3 seconds.

[0084] The confidence parameter for zero-rate correction is set to 1.0, and the observation noise covariance is R = (0.01 / confidence)² ×I³.

[0085] Experiments have shown that in outdoor walking scenarios involving frequent stops (stopping once every 2 minutes for 30 seconds each time), the cumulative position error of the inertial navigation / barometer mode after introducing zero-speed correction is reduced from 5% of the travel distance to 2% of the travel distance over 30 minutes.

[0086] Example 11: This example provides an optimized solution for sensor data acquisition.

[0087] The main control MCU integrates three direct memory access controllers, which are used for UART data reception from the MEMS nine-axis inertial sensor, UART data reception from the GNSS positioning module, and SD card storage read / write, respectively. Data transfer is automatically completed by the direct memory access controllers, and the central processing unit of the main control MCU only performs data parsing and fusion calculations, reducing the load on the central processing unit and improving the system's real-time performance.

[0088] The main control MCU integrates two independent SPI controllers. The first SPI controller is dedicated to reading barometer data (full-duplex mode), and the second SPI controller is dedicated to writing LCD display data (3-line write mode). The two SPIs work independently and do not interfere with each other.

[0089] The MEMS nine-axis inertial sensor receives data using an interrupt frame synchronous state machine byte-by-byte reception method. The data frame format is fixed length, starting with a preset frame header and containing multiple sets of three-byte signed integer data. Each 24-bit signed integer is in big-endian byte order, and the high 16 bits are taken as a signed short integer and multiplied by a scaling factor to obtain the physical quantity value. The scaling factors are: angular velocity 0.061035 degrees / second, acceleration 0.000488g, magnetic force 0.146469μT, Euler angle 0.010986 degrees, and quaternion 0.0000305.

[0090] The data reading process of the barometer is as follows: After a power-on delay, the chip identifier is read for verification, multiple sets of 24-bit calibration coefficients (c00, c01, c10, c11, c20, c21, c30) are read, the oversampling rate (e.g., ×64) and measurement rate (e.g., 16 times / second) are configured, and the system is set to continuous measurement mode. In each reading cycle, 6 bytes of data (pressure MSB, LSB, XLSB and temperature MSB, LSB, XLSB) are read. Each 24-bit signed number is sign-extended and then compensated for temperature and pressure polynomials, finally converted to altitude.

[0091] The above description is merely a preferred embodiment of the present invention and is not intended to limit the invention. Various modifications and variations can be made to the present invention by those skilled in the art. 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.

Claims

1. A handheld terminal for geological occurrence measurement and positioning, comprising a main control MCU (1), a multi-source sensor (2), a power supply unit (3), a display module (4), a storage module (5), and a flat bottom surface (6), characterized in that... The main control MCU runs a dual-task heterogeneous real-time firmware based on a real-time operating system. The first task executes a multi-source sensor fusion positioning algorithm with an error state extended Kalman filter at a first update rate, and the second task executes a rock surface adhesion detection and attitude calculation algorithm at a second update rate. The two tasks share the raw data stream of the same MEMS nine-axis inertial sensor. The terminal also has a flat bottom surface for adhering to the rock surface. The second task triggers attitude calculation when it detects that the flat bottom surface is in contact with the rock surface.

2. The handheld terminal for geological occurrence measurement and positioning according to claim 1, characterized in that... The first task receives data from the MEMS nine-axis inertial sensor, GNSS positioning module, and barometer through a message queue, performs prediction and correction steps, and generates continuous terminal position coordinates which are stored in the storage module. The second task reads the MEMS nine-axis inertial sensor data through zero-copy global variables, and after multi-level bonding detection and confirmation, maps the three-axis attitude angles to the three elements of rock strata attitude (strike, dip, and dip angle), and associates them with the position coordinates to store them in the storage module.

3. The handheld terminal for geological occurrence measurement and positioning according to claim 1, characterized in that, The thread priority of the first task is higher than that of the second task.

4. The handheld terminal for geological occurrence measurement and positioning according to claim 1, characterized in that... The multi-source sensors include a MEMS nine-axis inertial sensor, a barometer, and a multi-constellation GNSS positioning module, wherein the MEMS nine-axis inertial sensor integrates a three-axis accelerometer, a three-axis gyroscope, and a three-axis magnetometer.

5. The handheld terminal for geological occurrence measurement and positioning according to claim 1, characterized in that... The magnetometer module in the MEMS nine-axis inertial sensor has a built-in ferromagnetic correction algorithm, and the second task uses the corrected magnetometer data when performing attitude calculation.

6. The handheld terminal for geological occurrence measurement and positioning according to claim 1, characterized in that... In the second task, under strong magnetic interference environment, the gyroscope and accelerometer in the MEMS nine-axis inertial sensor are used to infer the state through a fusion algorithm.

7. The handheld terminal for geological occurrence measurement and positioning according to claim 1, characterized in that... The digital filtering parameters for the first task include the process noise covariance matrix of the error state extended Kalman filter, and the observation noise covariance matrices corresponding to the GNSS positioning module, zero velocity correction, and barometer measurement, respectively.

8. The handheld terminal for geological occurrence measurement and positioning according to claim 1, characterized in that... The fitting detection parameters used in the second task include acceleration modulus threshold, root mean square threshold of angular velocity, button debouncing delay, and continuous stabilization time.

9. The handheld terminal for geological occurrence measurement and positioning according to claim 1, characterized in that... The storage module is an SD card, a TF card, or an onboard Flash memory.

10. The handheld terminal for geological occurrence measurement and positioning according to claim 1, characterized in that, The main control MCU uses an STM32H723ZGTx chip with an ARM Cortex-M7 core, built-in single-precision floating-point unit and memory protection unit, and runs the RT-Thread Nano real-time operating system.

11. The handheld terminal for geological occurrence measurement and positioning according to claim 1, characterized in that, The error state extended Kalman filter employs a 15-dimensional error state vector, including three-dimensional position error, three-dimensional velocity error, three-dimensional attitude error, three-dimensional gyroscope bias error, and three-dimensional accelerometer bias error.

12. A dual-task collaborative control method for a handheld terminal for geological attitude measurement and positioning, applied to the handheld terminal for geological attitude measurement and positioning as described in any one of claims 1 to 11, characterized in that, include: The first task executes a multi-source sensor fusion localization algorithm using an error state extended Kalman filter at a first update rate. It receives data from a MEMS nine-axis inertial sensor, a GNSS positioning module, and a barometer via a message queue, performs prediction and correction steps, and generates continuous terminal position coordinates. The second task executes a rock surface adhesion detection and attitude calculation algorithm at a second update rate. It reads the raw data stream from the MEMS nine-axis inertial sensor using zero-copy global variables. When it detects that the flat bottom surface of the terminal is in contact with the rock surface, it triggers attitude calculation, maps the three-axis attitude angles to the three elements of rock surface attitude (strike, dip, and dip angle), and stores them in the storage module in association with the position coordinates. The two tasks share the raw data stream from the same MEMS nine-axis inertial sensor.

13. The dual-task cooperative control method according to claim 12, characterized in that, The fit detection includes a multi-level confirmation process: the first level is the physical measurement of the hardware level detection and acceleration modulus detection of the trigger button, requiring the acceleration modulus to be within a preset gravitational acceleration range; the second level is the angular velocity root mean square value detection and button debounce delay detection, requiring the angular velocity root mean square value to be lower than a preset threshold and the button to be pressed continuously for more than a preset debounce delay; after both conditions are met, the continuous stability monitoring stage is entered, requiring the acceleration and angular velocity conditions to be continuously maintained for a preset stable time before it is determined to be a valid fit.

14. The dual-task collaborative control method according to claim 12, characterized in that, It also includes a positioning mode adaptive switching step: calculate the comprehensive signal quality index based on the number of visible satellites and the average signal-to-noise ratio, and operate in GNSS / inertial navigation deep combination mode when the GNSS signal quality is higher than the preset threshold, and switch to inertial navigation / barometer fusion mode when it is lower than the preset threshold.

15. The dual-task collaborative control method according to claim 12, characterized in that, It also includes a zero-velocity correction step: the stationary state is determined by three conditions: the horizontal velocity is lower than the preset horizontal velocity threshold, the absolute value of the vertical velocity is lower than the preset vertical velocity threshold, and the magnitude of the angular velocity is lower than the preset angular velocity threshold. When all three conditions are met, the stationary time is accumulated. After the preset stable time is reached, the zero-velocity correction is triggered, and the velocity observation value in the Kalman filter is set to zero.

16. The dual-task cooperative control method according to claim 12, characterized in that, The error state extended Kalman filter adopts a 15-dimensional error state vector dx=[dp(3), dv(3), dθ(3), db_g(3), db_a(3)]^T, where dp is the three-dimensional position error, dv is the three-dimensional velocity error, dθ is the three-dimensional attitude error, db_g is the three-dimensional gyroscope zero bias error, and db_a is the three-dimensional accelerometer zero bias error.

Images