Positioning method based on Beidou RTK and inertial navigation adaptive compensation

Abstract

The invention provides a positioning method based on Beidou RTK and inertial navigation adaptive compensation, and relates to the technical field of Beidou positioning, and the method comprises the following steps: collecting IMU data in a static state; the IMU-based human body motion sensing and posture tracking system enters a dynamic working mode, and a carrier posture is updated by using gyroscope data integration in the IMU; receiving positioning data output by the Beidou RTK module in real time through RTK performance adaptive evaluation and credibility factor calculation; carrying out adaptive Kalman filtering fusion and compensation to establish a Kalman filter; an RTK signal is continuously monitored, when the RTK signal is recovered to be credible, a new RTK position is updated to be a reliable reference to be stored in the historical library, and displacement tracking is restarted; the method can effectively solve the problems of positioning interruption and sudden precision drop caused by RTK signal attenuation or failure in complex environments such as a power distribution room in a transformer substation, an underground cable layer and the like.

Description

Technical Field

[0001] This invention relates to the field of BeiDou positioning technology, and in particular to a positioning method based on BeiDou RTK and inertial navigation adaptive compensation. Background Technology

[0002] With the intelligent development of the power industry, substation inspections and operations place extremely high demands on the accuracy, continuity, and real-time performance of personnel positioning. Centimeter-level to sub-meter-level positioning accuracy is the core support for ensuring safe management and trajectory tracking in power operations. BeiDou RTK (Real-Time Kinematic) technology, due to its theoretical centimeter-level positioning accuracy, has become one of the mainstream technologies in this field. However, existing BeiDou RTK positioning technology still faces many technical bottlenecks in practical applications, severely limiting its effectiveness in complex scenarios. Poor signal adaptability: Existing solutions generally rely on joint calculation by multiple satellite systems. In scenarios where the signal of a single satellite system is limited (such as the obstructed environment of tall equipment areas in substations, underground cable layers, indoor power distribution rooms, or scenarios such as urban canyons and near-ground flight), the positioning performance drops sharply or even fails completely. Accuracy decay problem: Conventional RTK technology is sensitive to the distance from the base station. As the distance from the base station increases, the positioning accuracy decays rapidly, making it difficult to maintain stable high-precision output; High hardware and computing power costs: Some existing technologies use multi-sensor fusion solutions (such as combining IMU, barometer, etc.) to achieve high-precision positioning, but such solutions have stringent requirements for hardware configuration and data processing computing power, resulting in high deployment costs; Insufficient real-time performance: Another solution reduces the cost of use by using a network RTK solution method of "transmit first and then centrally solve", but this mode has obvious real-time defects and cannot meet the core needs of substation personnel for real-time positioning and safety monitoring.

[0003] In summary, existing BeiDou RTK positioning technology struggles to balance adaptability to complex environments, high accuracy, low hardware dependence, and real-time performance. There is an urgent need for a positioning solution that can maintain stable positioning accuracy in signal-constrained scenarios, eliminates the need for complex manual calibration, and has controllable deployment costs. Summary of the Invention

[0004] To address the aforementioned technical problems, the technical solution adopted by this invention is as follows: According to a first aspect of this application, a positioning method based on BeiDou RTK and inertial navigation adaptive compensation is provided, comprising the following steps: S1, after the device is powered on, IMU data in a static state is collected; the IMU data at least contains three-axis accelerometer data and three-axis magnetometer data; a gravity vector is determined by using the three-axis accelerometer data, a pitch angle and a roll angle of a carrier coordinate system relative to a horizontal plane are calculated; the magnetometer data is compensated by combining the pitch angle and the roll angle, and a heading angle is calculated; based on the pitch angle, the roll angle and the heading angle, an initial transformation matrix of the carrier coordinate system to a navigation coordinate system is established, and self-calibration is completed; S2, the IMU-based human motion sensing and posture tracking system enters a dynamic working mode, and the carrier posture is updated by integrating gyroscope data in the IMU; three-axis acceleration in the carrier coordinate system is converted to the navigation coordinate system through the initial transformation matrix, and human motion acceleration is obtained by deducting the gravity acceleration component; the human motion acceleration is sequentially integrated to obtain velocity and displacement increment in the navigation coordinate system; S3, RTK performance self-adaptive evaluation and reliability factor calculation real-time receiving positioning data output by a Beidou RTK module; the positioning data at least contains a solution state, an accuracy factor and a satellite number; based on multi-dimensional indexes in the positioning data, a reliability factor a is calculated, the value range of the reliability factor a is 0-1, a = 1 indicates that the RTK data is completely reliable, and a = 0 indicates that the RTK data is unreliable; S4, adaptive Kalman filter fusion and compensation establishing a Kalman filter; the state variables of the filter at least contain three-dimensional position, three-dimensional velocity and sensor zero offset; the position output by the RTK is taken as an observation value input into the filter, and the reliability factor a obtained in step S3 is dynamically injected into an observation noise covariance matrix R of the filter; when a tends to 1, a first observation noise covariance matrix R1 is set, and the cumulative error of the IMU is corrected by the RTK observation value; when a tends to 0, a second observation noise covariance matrix R2 is set, the latest reliable RTK reference position in a historical reference library is called, and a dead reckoning result is obtained by combining the IMU displacement increment obtained in step S2; the RTK signal is continuously monitored, when the RTK signal is recovered to be reliable, the new RTK position is updated as a reliable reference and stored in the historical library, and the displacement tracking is restarted; R1 < R2.

[0005] The application has at least the following beneficial effects: The positioning method based on adaptive compensation of Beidou RTK and inertial navigation of the application, through step S1, the accelerometer and magnetometer data of the IMU are used to automatically complete the self-calibration of the carrier coordinate system to the navigation coordinate system, without manual intervention for calibration, effectively solving the problem of tedious manual calibration of traditional positioning equipment due to different wearing postures, greatly improving the efficiency of equipment deployment and user convenience; step S2, based on the real-time sensing of human motion by the IMU and the calculation of acceleration, speed and displacement increment, provides accurate data support for positioning compensation when the RTK signal is poor; step S3, through dynamic calculation of the reliability factor a of the multi-dimensional RTK index, the reliability of the RTK data can be accurately identified, and the interference of false RTK data on the positioning result is avoided; step S4, the observation noise covariance matrix of the Kalman filter is adaptively adjusted combined with a, when the RTK is reliable (a tends to 1), the RTK is used to correct the cumulative error of the IMU to ensure high-precision positioning, when the RTK is not reliable (a tends to 0), the dead reckoning compensation is carried out through the displacement increment of the IMU and the historical reliable RTK reference, effectively solving the problem of positioning interruption and precision drop caused by RTK signal attenuation or failure in complex environments such as indoor distribution room of substation and underground cable layer, realizing the continuity and stability of indoor and outdoor positioning, and without relying on complex hardware configuration of multi-sensor fusion, reducing the hardware cost and algorithm demand while ensuring the positioning performance, and stably supporting the safety control and trajectory tracing demand of substation personnel positioning. BRIEF DESCRIPTION OF DRAWINGS

[0006] In order to more clearly illustrate the technical solutions in the embodiments of the present application, the drawings needed in the embodiment description will be briefly introduced. Obviously, the drawings in the following description are only some embodiments of the present application, and other drawings can be obtained by those skilled in the art without creative labor.

[0007] Figure 1 The flowchart of the positioning method based on adaptive compensation of Beidou RTK and inertial navigation provided by the embodiments of the present application is shown. DETAILED DESCRIPTION

[0008] The technical solutions in the embodiments of the present application will be described clearly and completely in combination with the drawings in the embodiments of the present application. Obviously, the described embodiments are only some embodiments of the present application, not all embodiments. Based on the embodiments in the present application, all other embodiments obtained by those skilled in the art without creative labor are within the scope of protection of the present application.

[0009] It is noted that, based on the present disclosure, one skilled in the art will appreciate that one aspect described herein can be implemented independently of any other aspect and that two or more of these aspects can be combined in any

[0010] Reference will now be made to Figure 1 FIG. 1 shows a flowchart of a positioning method based on adaptive compensation of Beidou RTK and inertial navigation, and a positioning method based on adaptive compensation of Beidou RTK and inertial navigation will be introduced.

[0011] The positioning method based on adaptive compensation of Beidou RTK and inertial navigation can include the following steps: S1, after the device is powered on, IMU data in a stationary state is collected; the IMU data at least contains three-axis accelerometer data and three-axis magnetometer data; a gravity vector is determined using the three-axis accelerometer data, a pitch angle and a roll angle of a carrier coordinate system relative to a horizontal plane are calculated; the magnetometer data is compensated in combination with the pitch angle and the roll angle, a heading angle is calculated; based on the pitch angle, the roll angle and the heading angle, an initial transformation matrix of the carrier coordinate system to a navigation coordinate system is established, and self-calibration is completed.

[0012] In this embodiment, after the device is powered on, it first enters a stationary calibration stage (usually 1-3 seconds are required to ensure stable IMU data), and IMU data in this stage is collected - here, "at least contains three-axis accelerometer data and three-axis magnetometer data", and in actual application, three-axis gyroscope data (used for subsequent attitude initialization verification) is also collected synchronously; the device refers to an integrated positioning terminal integrating a Beidou RTK positioning module, an inertial measurement unit (IMU), a core processor, a data storage module and a power supply module, which is a hardware carrier for implementing the "positioning method based on adaptive compensation of Beidou RTK and inertial navigation".

[0013] Gravity vector and pitch angle and roll angle calculation: the average output of the accelerometer when stationary can be approximated as the gravitational acceleration (define the navigation coordinate system as positive, the gravity vector (g n = [0, 0, -g] T , n is the abbreviation of "navigation coordinate system" to clearly indicate the "coordinate system" of the gravity vector, g n represents the "gravity vector defined in the navigation coordinate system"; g is the gravitational acceleration), through the acceleration components a x (x-axis) and a z (z-axis) in the carrier coordinate system, according to the formula = arctan(ax / a z )Calculate the roll angle , similarly, by a y (y-axis) and a z Calculate the pitch angle θ, so as to determine the spatial direction of "vertically downward", solve the angle offset problem of the carrier and the horizontal plane.

[0014] Magnetometer compensation and heading angle calculation: due to the pitch angle θ and the roll angle will cause the magnetometer measurement value to deviate from the horizontal plane, and the "rotation matrix around the x-axis and y-axis" (C x ( ) is the roll rotation matrix, (C y (θ) is the pitch rotation matrix) in the initial transformation matrix established in step S1, which converts the original magnetometer data m b (carrier coordinate system) to the horizontal coordinate system m h , that is, m h =C x ( ) T C y (θ) T m b ; again according to the formula Ψ=arctan(m x / m y )-δ; m x , m y is the magnetometer component in the horizontal coordinate system, and δ is the local magnetic deviation; calculate the heading angle Ψ, determine the "geomagnetic north" direction, and complete the spatial orientation calibration.

[0015] Initial transformation matrix construction: finally, through the "heading rotation matrix C z (Ψ) around the z-axis" and the aforementioned C x ( )、C y (θ) to construct the complete transformation matrix C b n =C z (Ψ)C y (θ)C x ( ), which directly defines the mapping relationship of "device wearing posture → navigation coordinate system" - for example, when the power station inspection personnel wear the device in front of the chest, on the side of the safety helmet or in the hand, the matrix will automatically adapt to different wearing angles, without the need for manual adjustment of the device orientation, truly realizing "wearing calibration".

[0016] This step solves the core problem of traditional positioning devices through the "two-vector self-calibration" logic of gravity and geomagnetism: traditional IMU positioning requires manual fixing of the device in a preset orientation (such as facing upwards) to ensure the accuracy of the coordinate system. This step, without any manual intervention, can automatically identify the device's arbitrary wearing posture through sensor data, significantly reducing the operation threshold of substation inspection personnel (no need to learn the calibration process), and improving the efficiency of device deployment (completed within a few seconds after starting, without additional preparation time). At the same time, based on the rigorous rotation matrix and angle calculation logic defined in the disclosure, the coordinate system error after self-calibration can be controlled within 0.5°, providing a high-precision "space reference" for subsequent IMU motion perception and RTK fusion, avoiding the accumulation of positioning deviation caused by coordinate system deviation.

[0017] Further, the IMU data further includes three-axis gyroscope data; and the initial transformation matrix is a matrix product after the carrier coordinate system rotates around the x-axis, the y-axis, and the z-axis; wherein the rotation around the x-axis corresponds to the roll angle, the rotation around the y-axis corresponds to the pitch angle, and the rotation around the z-axis corresponds to the yaw angle.

[0018] In this embodiment, the three-axis gyroscope data collected in the S1 stage does not directly participate in the calculation of the pitch angle, roll angle, and yaw angle (the first three are derived by the accelerometer and magnetometer), but plays a key role in "static state verification" and "initial posture stability guarantee": Static state validity judgment: The ideal output of the gyroscope in a static state is "zero angular velocity" (as there is no rotational motion), and the system will monitor the three-axis angular velocity value of the gyroscope in real time (the threshold is usually set to ±1° / s). If the angular velocity of an axis continuously exceeds the threshold, it is determined that the device is not in a stable static state (such as slight shaking when the inspection personnel holds the device), at which time the "recollect IMU data" process is triggered to avoid the identification deviation of the gravity vector caused by non-static data (such as mistaking the acceleration of hand shaking as a gravity component), ensuring the reliability of the basis data for subsequent angle calculation.

[0019] Initial posture consistency check: After calculating the pitch angle, roll angle, and yaw angle using the accelerometer and magnetometer, the system will use the short-term stability of the gyroscope (angular velocity drift is small in a static state) to monitor the continuity of the initial angle for 1-2 seconds. If the angle fluctuation range exceeds 0.3°, it is determined that the current calibration result has errors, and the calibration process is re-executed to further improve the accuracy of the initial transformation matrix, providing a "low-error initial reference" for subsequent S2 posture tracking.

[0020] The essence of the initial transformation matrix is ​​a mathematical mapping that transforms sensor data from the carrier coordinate system (b-frame, the device's own coordinate system) to the navigation coordinate system (n-frame, usually the north-north-sky coordinate system). Its construction completely follows the rotation sequence of "roll around the x-axis, pitch around the y-axis, and yaw around the z-axis," and the specific implementation is as follows: Step 1: Determine the angles and matrix forms corresponding to the rotation of each axis. Rotation around the x-axis corresponding to the roll angle (The device rotates around its own x-axis, such as flipping the device from front to side), the rotation matrix is ​​denoted as C. x ( Rotation around the y-axis corresponds to a pitch angle θ (the equipment rotates up and down around its own y-axis, such as tilting the equipment up or down), and the rotation matrix is ​​denoted as C. y (θ); the heading angle Ψ corresponding to rotation around the z-axis (the equipment rotates around its own z-axis, such as turning the equipment from north to east), the rotation matrix is ​​denoted as C. z (Ψ).

[0021] Rotate about the x-axis : ; For example: roll angle When the angle is 30°, cos30°≈0.866, sin30°=0.5. The matrix will convert the acceleration data of the y-axis and z-axis of the carrier to the navigation system to correct the component offset caused by the equipment rolling over.

[0022] Rotate θ about the y-axis: ; For example, when the pitch angle θ = 15°, cos15°≈0.966, sin15°≈0.259, the matrix will correct the x-axis and z-axis data offset caused by the device tilting up / down (for example, when the device tilts down, the accelerometer x-axis will be mixed with the gravity component, which can be separated by this matrix).

[0023] Rotate Ψ about the z-axis: ; For example, when the heading angle Ψ = 90° (eastward), cos90° = 0 and sin90° = 1, the matrix will convert the data of the carrier's x-axis (originally northward) into the eastward component of the navigation system, ensuring that the azimuth reference is consistent with the geomagnetic north.

[0024] Step 2: Determine the product order of the rotation matrices. The complete form of the initial transformation matrix is ​​C. b n =C z (Ψ)×C y (θ)×C x ( ) (the product order cannot be reversed), because the coordinate system conversion needs to follow the logic of "first correcting the device's own roll (roll), then correcting the tilt (pitch), and finally correcting the azimuth orientation (heading)" - if the order is wrong, it will cause the heading angle correction to be superimposed on the carrier data without eliminating the roll error, resulting in a coordinate system offset (such as the east direction of the navigation system deviating from the actual east direction by more than 5°).

[0025] For example, the inspection personnel of the substation wear the device on the side of the safety helmet (roll angle = 45°, pitch angle θ = 10°, heading angle Ψ = 30°), the system will first correct the roll error of the device by C x (45°), then correct the pitch error of the safety helmet by C y (10°), and finally correct the heading error of the personnel by C z (30°), and finally accurately map the acceleration and magnetometer data collected by the device to the "northeast sky" navigation system, achieving the effect of "no matter how it is worn, the data can correspond to the real space direction".

[0026] Multiply the matrices C b n = C z (Ψ) × C y (θ) × C x ( ) to get: ; In this embodiment, on the one hand, the addition of three-axis gyroscope data upgrades the "static calibration" of S1 from "passive collection" to "active verification", avoiding calibration errors caused by incomplete device stillness (such as personnel hand shaking and slight walking) - if the traditional solution is calibrated in a non-stationary state, the pitch angle and roll angle calculation error may exceed 2°, and the subsequent displacement increment error of S2 will accumulate to 0.5m / 10s, while the gyroscope angular velocity threshold judgment can control the initial angle error within 0.5°, providing a "highly reliable initial reference" for subsequent IMU motion perception and RTK fusion; on the other hand, the clear "rotation matrix product around x / y / z axis" logic changes the coordinate system conversion from "abstract principle" to "quantifiable mathematical operation", not only ensuring the consistency of conversion accuracy (error < 0.3°) under different wearing postures (chest, safety helmet side, hand-held), but also avoiding the "direction confusion" problem caused by matrix construction errors (such as misjudging the east direction as the north direction), laying a precise spatial mapping foundation for the motion acceleration extraction of subsequent step S2 and the dead reckoning compensation of S4, and finally improving the reliability and stability of the entire positioning method in the complex scene of the substation.

[0027] S2, the human motion sensing and posture tracking system based on IMU enters dynamic working mode, and the carrier posture is updated by integrating the gyroscope data in the IMU; the three-axis acceleration under the carrier coordinate system is converted to the navigation coordinate system through the initial transformation matrix, and the human motion acceleration is obtained by deducting the gravity acceleration component from the acceleration under the navigation coordinate system; the human motion acceleration is sequentially integrated to obtain the velocity and displacement increment under the navigation coordinate system.

[0028] Further, the quaternion is used for updating the carrier posture, and the real-time attitude quaternion is obtained by discretely calculating the angular velocity measured by the gyroscope and the quaternion differential equation.

[0029] In the embodiment, the system enters the dynamic working mode, the gyroscope data is integrated to update the carrier posture in real time, the attitude matrix is used to convert the three-axis acceleration under the carrier coordinate system to the acceleration under the navigation coordinate system , the gravity acceleration component is deducted from the acceleration under the navigation coordinate system to obtain the human motion acceleration, and the human motion acceleration is integrated once to obtain the velocity and twice to obtain the displacement increment under the navigation coordinate system.

[0030] In the static state, the accelerometer measurement value is: ; wherein the gravity vector under the navigation coordinate system is g n =[0,0,-g] T , wherein g is the gravity acceleration. Here, the skyward direction is defined as positive, so the gravity downward is negative.

[0031] The rotation matrix from the n system to the b system is .

[0032] ; Therefore, we have: ; ; The heading is calculated by using the geomagnetic vector: the magnetometer vector is converted to the horizontal coordinate system ; ; In the horizontal coordinate system, the heading angle can be calculated by the following formula: ; wherein, is the magnetic declination.

[0033] In this embodiment, "IMU-based human motion perception and attitude tracking" is the core link connecting "coordinate system self-calibration (S1)" and "RTK fusion compensation (S4)". The core objective is to acquire the human body's "attitude state" and "motion state" (velocity, displacement) in real time through the collaborative operation of the IMU's gyroscope, accelerometer, and magnetometer, providing high-precision dynamic data for subsequent positioning compensation. The entire process can be broken down into four key sub-processes, which are implemented in detail by combining coordinate system definition, sensor principles, and formula logic: I. Sub-process 1: Dynamic carrier attitude update (based on gyroscope integration) The three-axis gyroscope in the IMU is used to measure the angular velocity (unit: ° / s or rad / s) of the carrier (i.e., positioning device / human body) around its three axes (x-axis roll, y-axis pitch, z-axis yaw). Angular velocity reflects the rate of attitude change. By integrating the angular velocity over time, the carrier's attitude angle (roll angle) can be calculated. The real-time changes in pitch angle θ and heading angle Ψ are used to update the attitude matrix (i.e., the transformation matrix C established in step S1 from "vehicle coordinate system b to navigation coordinate system n"). b n ).

[0034] The specific implementation method is as follows: Data Input: After the system enters dynamic mode (e.g., when inspection personnel begin to move), the gyroscope outputs the three-axis angular velocity ω in a fixed time step (usually 10~20ms to ensure real-time performance). b =[ω x ,ω y ,ω z ] T (The superscript b indicates "measured value in the carrier coordinate system").

[0035] Attitude change is calculated by integrating angular velocity: The angular velocity around each axis is integrated over time to obtain the increment of the attitude angle. For example, the angular velocity ω around the x-axis... x After integrating for time Δt, the increment of the roll angle Δ =ω x ×Δt; Similarly, the pitch angle increment Δθ and the heading angle increment ΔΨ can be obtained.

[0036] Update the attitude matrix: Calculate the attitude angle increment (Δ) Δθ, ΔΨ) are superimposed on the initial attitude angle in step S1. On (θ0, Ψ0), the attitude angle at the current moment is obtained. = 0+Δ (θ = θ0 + Δθ, Ψ = Ψ0 + ΔΨ); then recalculate the attitude matrix C based on the new attitude angles. bn =C z (Ψ)×C y (θ)×C x ( ).

[0037] II. Sub-process 2: Acceleration conversion and motion acceleration extraction The triaxial accelerometer in the IMU measures the "specific force in the carrier coordinate system" (i.e., the acceleration felt by the device, which includes "human motion acceleration" and "the projection of gravitational acceleration"). Since we need "pure motion acceleration" (for calculating velocity and displacement), we must first transform the acceleration from the "carrier coordinate system b" to the "navigation coordinate system n" through the "attitude matrix" and then remove the gravitational acceleration component.

[0038] The specific implementation is as follows: Step 1: Carrier acceleration → navigation acceleration conversion utilizes the updated attitude matrix C from sub-process 1. b n The acceleration a of the carrier coordinate system measured by the accelerometer b =[a x b ,a y b ,a z b ] T Converted to acceleration a in the navigation coordinate system (northeast to the sky, with the sky direction being positive). n The formula is: a n =C b n c×a b .

[0039] Step 2: Remove gravitational acceleration to obtain the gravity vector g in the motion acceleration navigation coordinate system. n =[0,0,-g] T (g=9.8m / s) 2 (Since the upward direction is positive, gravity is negative downwards), the accelerometer measures a. n This includes the gravitational component. Therefore, the pure acceleration a n motion We need to obtain it by subtracting the gravity vector: a n motion =a n -g n .

[0040] III. Sub-process 3: Calculation of velocity and displacement increments (integral operation) Motion acceleration is the "rate of change of velocity", and velocity is the "rate of change of displacement" - by integrating the "motion acceleration once", we can obtain the real-time velocity, and by integrating the "velocity twice", we can obtain the displacement increment in the navigation coordinate system (i.e., the distance the human body moves in the three directions of east, north, and sky).

[0041] The specific implementation is as follows: Step 1: Integrate once to find the speed For the acceleration a n motion Integrating over a time step Δt (consistent with the gyroscope, 10~20ms), we obtain the velocity v at the current moment. n =[vE,vN,vU] T (E=East, N=North, U=Sky), the discretization formula is: v k n =v n k-1 +a n motion，k ×Δt; where v n k-1 It is the velocity at the previous moment, a n motion，k It is the acceleration of motion at the current moment.

[0042] Step 2: Calculate the displacement increment through double integration For velocity v n Integrating again by Δt, we obtain the displacement increment ΔS at the current time relative to the initial time. n =[ΔS E ,ΔS N , ΔS U ] T The discretization formula is: ΔS n k =ΔS n k-1 +v n k ×Δt.

[0043] Displacement increment ΔS n The core data for step S4, "RTK failure compensation," is as follows: when the RTK signal is poor (e.g., entering the substation interior), the system can use "historical reliable RTK position + ΔS" to determine the cause. n "Calculate the current location to avoid location interruption."

[0044] IV. Sub-process 4: Geomagnetic vector correction of heading angle (solving gyroscope drift) Gyroscope integration will cause the heading angle to drift in the long term (e.g. 2-3° drift in 1 minute, resulting in "north" deviation), while the earth's magnetic field is a "long-term stable direction reference" (geomagnetic north) - measure the geomagnetic vector through the three-axis magnetometer in the IMU, convert to the horizontal coordinate system, and calculate the heading angle to correct the gyroscopic heading drift in real time.

[0045] The specific implementation is as follows: Step 1: Carrier geomagnetic vector -> horizontal coordinate system conversion The magnetometer measures the "geomagnetic vector in the carrier coordinate system" m b = [m x x, m b y, m y z] b z b T But the pitch angle θ and the roll angle will cause the geomagnetic vector to "deviate from the horizontal plane" (the heading angle is the angle on the horizontal plane, and the effects of pitch and roll need to be offset). Therefore, first convert m b to the "horizontal coordinate system" (i.e. the x-y plane of the navigation system, ignoring the z axis) to get m h (horizontal geomagnetic vector), the formula is: m h = C x ( ) T × C y (θ) T × m b ; where C x ( ) T is the transpose of the roll rotation matrix, C y (θ) T is the transpose of the pitch rotation matrix, and the effect is to "offset" the effects of carrier roll and pitch on magnetic measurement, so that m h only reflects the geomagnetic direction on the horizontal plane.

[0046] Step 2: Calculate the heading angle and correct In the horizontal coordinate system, "geomagnetic north" is the reference direction (approximately north, but with a magnetic deviation δ), and the heading angle Ψ is defined as the angle between the carrier north (navigation system y axis) and the horizontal geomagnetic vector m h , the calculation formula is: ; where m x x h is the east component of the horizontal geomagnetic vector, and m y y his the north component; δ is the "magnetic deviation" (the deviation of local geomagnetic north from true north, which needs to be obtained in advance by map or device calibration, such as δ≈-6° in Beijing, i.e. the geomagnetic north is 6° west of the true north). The calculated Ψ will replace the heading angle obtained by gyroscopic integration to correct the drift error.

[0047] Step S2 realizes the full-chain perception of "attitude-acceleration-velocity-displacement" through the cooperation of the IMU three sensors, solves the "attitude dynamic tracking" problem-real-time capture of human motion, ensures the accuracy of acceleration conversion and heading calculation; provides "backup data when RTK fails"-displacement increment can be directly used for dead reckoning to avoid positioning blind area; balances "short-term accuracy" and "long-term stability"-gyroscope ensures short-term attitude accuracy, magnetometer corrects long-term heading drift, lays data foundation for subsequent fusion with RTK, and finally supports continuous positioning demand in complex scenes such as transformer substations.

[0048] S3, real-time receiving of positioning data output by the Beidou RTK module; the positioning data at least includes solving state, accuracy factor, and satellite number; based on the multi-dimensional indicators in the positioning data, the reliability factor a is calculated, the value range of the reliability factor a is 0 to 1, a=1 represents that the RTK data is completely reliable, and a=0 represents that the RTK data is unreliable.

[0049] Further, the multi-dimensional indicators include solving state, accuracy factor threshold, satellite number threshold, and position change rate; the reliability factor a is the product of the corresponding weight factors of each dimension indicator; wherein the solving state includes fixed solution, floating solution, and single-point solution, different solving states correspond to different weight factors.

[0050] Further, the accuracy factor includes horizontal accuracy factor HDOP and vertical accuracy factor VDOP, which are respectively used to calculate the reliability factor a in the horizontal direction and the vertical direction.

[0051] In this embodiment, the positioning data output by the Beidou RTK module is received in real time: longitude, latitude, height, position accuracy factor PDOP / HDOP / VDOP, satellite number, solving state such as floating solution and fixed solution, and an adaptive reliability factor a is designed, the value of which is between 0 and 1. a=1 represents that the RTK data is completely reliable, and a=0 represents that the RTK data is unreliable.

[0052] The calculation of the reliability factor a is based on the multi-dimensional RTK quality indicators: Solving state weight: fixed solution>floating solution>single-point solution. For example, the basic value of a is set to 0.99 when the fixed solution is floating solution, 0.5, and single-point solution is 0.1.

[0053] Precision factor threshold: when HDOP / VDOP exceeds a certain threshold, α is reduced.

[0054] Satellite number threshold: when the number of visible satellites is less than a certain number, α is reduced.

[0055] Rate of change detection: calculate the difference between the current RTK position and the historical position. If the rate of change exceeds the physiological limit of human movement (such as 10 m / s 2 ) in a short time, it is considered to be a jump, and α is greatly reduced or even set to zero.

[0056] In this embodiment, the system receives the positioning data output by the Beidou RTK module in real time (the update frequency is usually 1 Hz, which meets the real-time positioning requirements of substation personnel), "at least contains the solution state, precision factor, satellite number", and can also synchronously obtain latitude, longitude, height, position change rate and other data, and construct a credibility factor α (value 0-1) based on multi-dimensional indicators; the specific implementation is as follows: Solution state weight (α status ): RTK solution state is divided into fixed solution (highest precision, centimeter level), floating solution (decimeter level), and single-point solution (meter level), with corresponding weights of 1, 0.5, and 0.1, respectively - for example: in an open outdoor area of a substation, the satellite signal is not blocked, the RTK outputs a fixed solution, and α status =1; when entering the corridor of the main control building, the signal blocking is intensified, the solution state becomes a floating solution, and α status drops to 0.5; when completely entering the underground cable layer, the solution state becomes a single-point solution, and α status drops to 0.1.

[0057] Precision factor weight (α dop ): The precision factor includes horizontal precision factor HDOP (reflecting horizontal positioning precision) and vertical precision factor VDOP (reflecting elevation direction precision), and the preset threshold is usually HDOP≤1.5 and VDOP≤2.0, α dop =1 when HDOP / VDOP is below the threshold, and is reduced by a certain proportion when it exceeds the threshold - for example: outdoor HDOP=0.8 and VDOP=1.2, α dop =1; indoor HDOP=3.0 (1 times threshold), α dop drops to 0.5, and VDOP=4.0 (1 times threshold), α dop also drops to 0.5.

[0058] Satellite number weight (α sat ): The preset visible satellite number threshold is usually 8, α sat =1 when the number of satellites is ≥8, and is reduced by a certain proportion when it is less than 8 - for example: 15 satellites in outdoor, α sat= 1; the number of indoor satellites is reduced to 6 (2 less than the threshold), a sat = 0.75; the number of underground cable layer satellites is reduced to 3, a sat = 3 / 8 = 0.375.

[0059] Position change rate weight (a change ): calculate the difference between the current RTK position and the position 1 second ago. If the change rate exceeds the physiological limit of human movement (set to 10 m / s 2 in the disclosure, far exceeding the normal walking speed of 0.5 m / s 2 acceleration of a human being), it is determined that the RTK data jumps, a change is greatly reduced to 0.1 or even 0 (for example, due to signal interference, the RTK position suddenly jumps 5 meters, a change is directly set to 0); when there is no jump, a change = 1.

[0060] The final reliability factor a = a status x a dop x a sat x a change , for example: outdoor scene (fixed solution + HDOP = 0.8 + 15 satellites + no jump), a = 1 x 1 x 1 x 1 = 1; indoor scene (floating solution + HDOP = 3.0 + 6 satellites + no jump), a = 0.5 x 0.5 x 0.75 x 1 = 0.1875; clearly quantifying the reliability of RTK data.

[0061] This step solves the core defect of traditional RTK positioning "blindly trusting data" through the reliability evaluation logic of "multi-dimensional index weighting": the traditional scheme directly uses the position output by RTK, even if the data jumps or the accuracy drops due to obstruction or interference, it will still be used as the positioning result, which will cause the personnel trajectory of the substation to have "fly points" (such as suddenly jumping from the equipment area to outside the fence), which will seriously affect safety management and control; this step can identify the "degree of reliability" of RTK data in real time through the dynamic calculation of a - when a tends to 1, it is clear that the RTK data is reliable and can be used as a high-precision positioning basis; when a tends to 0, it is clear that the RTK data is not reliable and needs to be switched to the IMU compensation mode to avoid interference from incorrect data on the positioning result from the source. At the same time, the accuracy factors in the horizontal (HDOP) and vertical (VDOP) directions are evaluated respectively, which can also achieve "directional reliability judgment" (for example, indoor horizontal a = 0.3, vertical a = 0.1), providing more detailed decision basis for subsequent fusion compensation, further improving the positioning accuracy.

[0062] S4, a Kalman filter is established by adaptive Kalman filter fusion and compensation; the state variables of the filter at least include three-dimensional position, three-dimensional velocity, and sensor zero offset; the position output by the RTK is input as an observation value into the filter, and the confidence factor a obtained in step S3 is dynamically injected into the observation noise covariance matrix R of the filter; when a tends to 1, a first observation noise covariance matrix R1 is set, and the cumulative error of the IMU is corrected by the RTK observation value; when a tends to 0, a second observation noise covariance matrix R2 is set, the latest reliable RTK reference position in the historical reference library is called, and the dead reckoning is performed by combining the IMU displacement increment obtained in step S2 to obtain the compensated positioning result; the RTK signal is continuously monitored, when the RTK signal is reliable, the new RTK position is updated as the reliable reference and stored in the historical library, and the displacement tracking is restarted; R1 < R2.

[0063] Further, a preset small constant e is introduced into the calculation formula of R1 and R2 to avoid zero denominator; the observation value of the Kalman filter is the position component in the east direction, the north direction and the sky direction in the navigation coordinate system.

[0064] Further, the IMU displacement increment is calculated by discretization integration, and the integral process accumulates the contribution value of the velocity and acceleration at a fixed time step; the compensated positioning result is the vector superposition of the reliable RTK reference position and the IMU displacement increment.

[0065] This step is based on the Kalman filter to construct an “RTK-IMU fusion framework”, and the core is to dynamically adjust the filtering strategy through a to ensure the positioning accuracy and continuity in different scenes.

[0066] A Kalman filter is established, and the state variables include: three-dimensional position, three-dimensional velocity, attitude angle / quaternion, sensor zero offset, etc. Observation update: the position output by the RTK is input as an observation value into the filter; Adaptive observation noise covariance matrix R: the confidence factor a calculated in step three is dynamically injected into the observation noise covariance matrix R of the filter. The logic is: when the RTK reliability is high (a→1), a small R value is set, and the filter believes the RTK observation value, so as to correct the cumulative error of the IMU.

[0067] At any given time t2, when the RTK reliability decreases (α→0), a large R value is set. The system immediately retrieves the most recent reliable reference position PRTK(t1) from the historical reference database, where t1 << t2. Simultaneously, it acquires the total displacement increment ΔS(t2-t1) calculated by the IMU during the time interval from t1 to t2. This displacement increment is a vector, encompassing both planar displacement and elevation changes. Dead reckoning (DR) and compensation are performed using the displacement increment calculated by the IMU, thus preventing erroneous RTK data from "biasing" the filter. Assume there are k time steps from t1 to t2, with a time step size of Δt, and the human motion acceleration a... motion : Integrating the acceleration twice yields the displacement increment from t1 to t2 (time step Δt, k steps in total, k = (t2 - t1) / Δt): Velocity increment: v(t) = ∫(t1 to t2)a motion dt ; Displacement increment: ΔS(t2-t1)=∫(t1 to t2)v(t)dt= ; The discretization calculation formula is: ; The reference position at time t1 is superimposed with the IMU displacement increment vector to obtain the compensated positioning result at time t2: p compensated (t2)=p RTK (t1)+ΔS(t2-t1); This compensated position P compensated (t2) is the final output of the system at time t2. Continuous monitoring continues until the RTK signal is recovered to a reliable level. The system continuously updates the position estimation reference using IMU displacement increments, using t1 as the baseline. Once the RTK signal is recovered, the system will output the new RTK position PRTK(t). new It is stored in the history library as a new reliable benchmark and a new round of displacement tracking begins.

[0068] 1. Posture update Aspect update is performed using quaternions. The quaternion differential equation is: ; in, It is the angular velocity measured by the gyroscope. This represents quaternion multiplication.

[0069] After discretization, we use a first-order approximation: ; in, ; 2. Velocity and position estimation Convert the acceleration in the body frame to the navigation frame: ; In the navigation frame, the motion acceleration is: ; And g n = [0, 0, -g] T , then: ; Integrate the motion acceleration to get the velocity, and integrate again to get the position.

[0070] 3. Adaptive Kalman filter Define the state vector as: x = [p n , v n , q, b a , b g ] T , where: p n = [p E , p N , p U ] T : position in the navigation frame; v n = [v E , v N , v U ] T : velocity in the navigation frame; q = [q0, q1, q2, q3] T : attitude quaternion; b a = [b ax , b ay , b az ] T : accelerometer bias; b g = [b gx , b gy , b gz ] T : gyroscope bias.

[0071] The state prediction model is: ; The observation is the RTK position: ; The adaptive observation noise covariance matrix is: ; where, is the credibility factor, is a small constant.

[0072] For the horizontal and elevation directions, respectively, then: ; The Kalman gain is: ; Credibility factor calculation: α = α status ×α dop ×α sat ×α change ; α status The fixed solution is 1, the floating solution is 0.5, and the single-point solution is 0.1.

[0073] α dop The value is 1 when HDOP and VDOP are less than the threshold; otherwise, it decreases proportionally.

[0074] α sat The value is 1 when the number of satellites exceeds the threshold; otherwise, it is reduced proportionally.

[0075] α change If the rate of change of position exceeds the threshold, then decrease.

[0076] When α approaches 1 (RTK is reliable): R1 is small, the Kalman filter will "highly trust" the RTK observation value, and fuse the RTK position with the IMU-estimated position through the observation update stage, correcting the cumulative error of the IMU (such as attitude drift caused by gyroscope bias, velocity error caused by accelerometer bias), and finally outputting a positioning result with centimeter-level accuracy - for example, when inspecting outdoors, the system is mainly based on RTK, and the IMU only assists in correcting short-term drift, with a positioning error of <5cm.

[0077] When α approaches 0 (RTK is unreliable): R² is large, the filter will "low trust" RTK observations. At this time, the system retrieves the most recent reliable RTK reference position p from the historical reference library. RTK (t1) (usually the fixed solution position when α>0.8), combined with the IMU displacement increment ΔS(t2-t1) from t1 to the current time t2 calculated in step S2, according to formula p compensated (t2)=p RTK Dead reckoning is performed using (t1) + ΔS(t2-t1) – for example, when an inspection crew departs from outdoors at time p1. RTK (t1 is the fixed solution position) enters the underground cable layer at time t2), the IMU calculates ΔS = 3m (eastward) + 2m (northward) + 0m (elevation), then p compensated (t2) is p RTK (t1) With this increment, the positioning error is less than 1 meter within 15 minutes.

[0078] RTK signal recovery baseline update: The system continuously monitors α changes. When α rises above 0.8 (RTK recovery is reliable, such as personnel returning outdoors from underground cable layers), the new RTK position p is immediately updated. RTK (t new) as a new reliable benchmark into the history library, restart the IMU displacement tracking, and realize seamless switching between "compensation mode" and "high-precision RTK mode".

[0079] This step completely solves the "positioning blind area" and "precision fault" problems of traditional positioning technology in complex scenes through the fusion logic of "adaptive filtering + dead reckoning": on the one hand, when RTK is reliable, use RTK to correct IMU error, combining the "long-term no drift" of RTK and the "short-term high precision" of IMU, avoiding the defects of single RTK affected by shielding or single IMU error accumulation; on the other hand, when RTK is unreliable, through historical reliable benchmark and IMU displacement increment compensation, ensure the positioning continuity of substation indoor and outdoor, ground and underground, etc. The sub-millimeter accuracy within 15 minutes fully meets the personnel safety control (such as electronic fence, track tracing) requirements. At the same time, compared with the "multi-sensor fusion scheme" mentioned in the disclosure (which relies on additional hardware such as barometer, and has high computing power requirements), this step is based on RTK and IMU dual modules only, without additional hardware, greatly reducing the device cost and computing power consumption, and is more suitable for large-scale deployment of substations; in addition, the logic of "scene-based dynamic adjustment R" avoids the "deviation" of the filtering algorithm by false RTK data, further improves the stability and reliability of the positioning result, and provides core technical support for power operation safety control.

[0080] In addition, although the various steps of the method in the present disclosure are described in a specific order in the accompanying drawings, this does not require or imply that the steps must be performed in this specific order, or that all the steps shown must be performed to achieve the desired results. Additionally or alternatively, certain steps can be omitted, multiple steps can be combined into one step, and / or one step can be divided into multiple steps, etc.

[0081] Although some specific embodiments of the present application have been described in detail by examples, those skilled in the art should understand that the above examples are only for illustration, not for limiting the scope of the present application. Those skilled in the art should also understand that various modifications can be made to the embodiments without departing from the scope and spirit of the present application.

Claims

1. A positioning method based on BeiDou RTK and inertial navigation adaptive compensation, characterized in that, Includes the following steps: S1. After the device is powered on, it collects IMU data in a stationary state. The IMU data includes at least three-axis accelerometer data and three-axis magnetometer data. The gravity vector is determined using the three-axis accelerometer data, and the pitch and roll angles of the carrier coordinate system relative to the horizontal plane are calculated. The magnetometer data is compensated based on the pitch and roll angles to calculate the heading angle. Based on the pitch, roll, and heading angles, an initial transformation matrix from the carrier coordinate system to the navigation coordinate system is established to complete self-calibration. S2, the IMU-based human motion perception and attitude tracking system enters the dynamic working mode, and uses the gyroscope data in the IMU to integrate and update the carrier attitude; The three-axis acceleration in the carrier coordinate system is transformed to the navigation coordinate system using the initial transformation matrix. After deducting the gravitational acceleration component, the human motion acceleration is obtained. The human motion acceleration is then integrated sequentially to obtain the velocity and displacement increment in the navigation coordinate system, respectively. S3, Real-time RTK performance adaptive evaluation and reliability factor calculation: The positioning data output by the Beidou RTK module is received in real time; the positioning data includes at least the solution status, accuracy factor, and number of satellites; based on the multi-dimensional indicators in the positioning data, the reliability factor α is calculated, and the value of the reliability factor α ranges from 0 to 1, where α=1 indicates that the RTK data is completely reliable, and α=0 indicates that the RTK data is unreliable. S4, Adaptive Kalman filter fusion and compensation to establish a Kalman filter; The state variables of the filter include at least three-dimensional position, three-dimensional velocity, and sensor bias. The position output by RTK is used as the observation value and input into the filter. The confidence factor α obtained in step S3 is dynamically injected into the observation noise covariance matrix R of the filter. When α approaches 1, the first observation noise covariance matrix R1 is set, and the cumulative error of the IMU is corrected by the RTK observation value. When α approaches 0, the second observation noise covariance matrix R2 is set, the most recent reliable RTK reference position in the historical reference library is retrieved, and dead reckoning is performed in combination with the IMU displacement increment obtained in step S2 to obtain the compensated positioning result. Continuously monitor the RTK signal. When the RTK signal becomes reliable again, update the new RTK position as a reliable reference and store it in the historical database, then restart displacement tracking; R1 < R2.

2. The positioning method according to claim 1, characterized in that, In step S1, the IMU data also includes three-axis gyroscope data; the initial transformation matrix is ​​the matrix product of the carrier coordinate system after rotation around the x-axis, y-axis, and z-axis; wherein, rotation around the x-axis corresponds to the roll angle, rotation around the y-axis corresponds to the pitch angle, and rotation around the z-axis corresponds to the yaw angle.

3. The positioning method according to claim 1, characterized in that, In step S3, the multi-dimensional indicators include solution status, accuracy factor threshold, satellite number threshold, and position change rate; the credibility factor α is the product of the weight factors corresponding to each dimension indicator; wherein, the solution status includes fixed solution, floating solution, and single-point solution, and different solution statuses correspond to different weight factors.

4. The positioning method according to claim 3, characterized in that, In step S3, the accuracy factor includes a horizontal accuracy factor HDOP and a vertical accuracy factor VDOP, which are confidence factors α calculated independently in the horizontal and vertical directions, respectively.

5. The positioning method according to claim 1, characterized in that, In step S4, a preset small constant ε is introduced into the calculation formulas of R1 and R2 to avoid the denominator being zero; the observation values ​​of the Kalman filter are the position components in the east, north, and sky directions in the navigation coordinate system.

6. The positioning method according to claim 1, characterized in that, In step S2, quaternions are used to update the carrier attitude. The real-time attitude quaternion is obtained by discretizing the angular velocity measured by the gyroscope and the quaternion differential equation.

7. The positioning method according to claim 1, characterized in that, In step S4, the IMU displacement increment is calculated by discretization integration. The integration process accumulates the contribution values ​​of velocity and acceleration at a fixed time step. The compensated positioning result is the vector superposition of the reliable RTK reference position and the IMU displacement increment.

Images