SINS-based tightly coupled filter positioning method under line-of-sight constraint

Abstract

This invention relates to the fields of aviation and remote sensing mapping, and particularly to a tightly coupled filtering positioning method based on line-of-sight constraints under a strapdown-linked base. The method includes: constructing a system consisting of an UAV platform, a strapdown-linked optoelectronic pod, and a positioning processor to collect multi-source data and unify it into a navigation coordinate system; constructing an augmented state Kalman filter that includes the target's geographical location, IMU zero-bias error, and sensor installation deviation; defining a process model to achieve state-time updates, while simultaneously establishing three types of measurement models—laser ranging, angle encoding, and IMU line-of-sight angular velocity—to generate residuals; and finally, using the residuals to calculate the Kalman gain and update the state, achieving synchronous optimization of the target position and IMU zero-bias. The advantages are: no expensive hardware is required; through algorithmic innovation, online self-calibration of a low-cost IMU is achieved, significantly improving the positioning accuracy and robustness for any stationary target on the ground, and has significant application value in fields such as aerial reconnaissance and geographic mapping.

Description

Technical Field

[0001] This invention relates to the fields of aviation and remote sensing mapping technology, and in particular to a tightly coupled filtering positioning method based on line-of-sight constraints under a traction linkage base. Background Technology

[0002] Unmanned Aerial Vehicles (UAVs), equipped with electro-optical pods, perform reconnaissance, surveillance, and location tasks on stationary ground targets, representing one of the core application scenarios in the UAV field. In this application, target location must follow a standardized process, with the specific steps as follows:

[0003] 1. The UAV uses the BeiDou satellite navigation system receiver it carries to collect and calculate its own geographic coordinates (including longitude, latitude, and altitude information) in real time, providing reference position data for subsequent target positioning;

[0004] 2. After the camera built into the electro-optical pod locks onto a stationary ground target using image recognition and tracking technology, the laser rangefinder on the pod starts working, emitting laser signals toward the target and receiving reflected signals, and calculating the straight-line slant distance between the UAV and the target based on the laser propagation time difference;

[0005] 3. Utilize a high-precision encoder to measure the rotation angle of the optoelectronic pod relative to the UAV body (including the azimuth angle in the horizontal direction and the pitch angle in the vertical direction). Simultaneously, combine this with the UAV's own attitude data (such as roll angle, pitch angle, and yaw angle) provided by the UAV's built-in inertial measurement unit (IMU) in the UAV flight control system to calculate the line-of-sight vector from the UAV's center of mass to the stationary ground target. This vector contains the target's orientation information relative to the UAV.

[0006] 4. The UAV's reference position is fused with the line-of-sight vector, and combined with the target slant range, the final geographic coordinates of the stationary ground target are calculated through spatial geometric operations to complete the positioning process.

[0007] The accuracy of the above-mentioned classic positioning link is constrained by multiple error sources. These error sources can be divided into three categories according to their source, as follows: (1) UAV platform error: The positioning error of the Beidou positioning system and the attitude measurement error output by the flight control IMU directly affect the accuracy of the positioning reference; (2) Pod mechanical and calibration error: This includes the measurement error of the high-precision encoder on the pod rotation axis, the installation misalignment angle error generated during the installation of the pod and the UAV body, and the aiming line (boresight) error between the camera optical axis and the laser rangefinder axis. This type of error will cause the target pointing and the measurement data to deviate; (3) Sensor measurement error: The ranging error generated by the laser rangefinder during the distance measurement process directly affects the accuracy of the slant range data.

[0008] To improve the attitude stability (i.e., image stabilization capability) and enhance attitude measurement accuracy of pods, the engineering field commonly employs a "straight-through" mounting technique on the pod's camera base. However, in practical applications, due to strict limitations on equipment cost, size, and power consumption, these pods typically only accommodate low-cost microelectromechanical systems inertial measurement units (MEMS-IMUs). Low-cost MEMS-IMUs have significant technical drawbacks: their zero-bias error and noise change continuously over time. If their output data is directly used for pod attitude calculation through integration, errors accumulate rapidly and diverge, causing the attitude calculation results to deviate significantly from reality within seconds to tens of seconds, failing to provide a reliable attitude reference for high-precision positioning. Therefore, in traditional solutions, the IMU is only used to provide high-frequency attitude stabilization feedback signals to ensure the pod's image stabilization function, without being incorporated into the attitude angle calculation stage of the positioning process, thus failing to fully realize its potential value.

[0009] In summary, designing effective technical solutions for low-cost MEMS-IMUs, which suffer from significant noise and drift issues, to fully utilize their data and enable them to participate more deeply in the positioning calculation process, while overcoming their inherent defects and effectively improving the final target positioning accuracy, is a key technical challenge that urgently needs to be addressed in the field of UAV optoelectronic pod positioning technology. Summary of the Invention

[0010] To address the problem that low-cost MEMS-IMUs mounted in UAV optoelectronic pods cannot be directly used for high-precision target positioning due to their own zero-bias and drift errors, this invention provides a tightly coupled filtering positioning method based on line-of-sight constraints under a fast-linkage base.

[0011] The purpose of this invention is to provide a tightly coupled filtering positioning method based on line-of-sight constraints under a fast-linkage base, which specifically includes the following steps:

[0012] S1. Build a target positioning system, including a UAV platform equipped with a Beidou receiver, a strapdown optoelectronic pod, and a positioning calculation processor; collect the UAV's Beidou position, body attitude, raw angular velocity and raw linear acceleration, laser rangefinder range measurement value, and angle encoder reading, and unify them to the navigation coordinate system through coordinate transformation;

[0013] S2. Design an extended Kalman filter to simultaneously estimate the target position and system error by augmenting the state vector;

[0014] S3. Define a process model based on the augmented state vector in step S2. The process model includes a target position model, an IMU zero bias error model, and a static error model. Update the state vector and its covariance matrix over time according to the process model and output the predicted state.

[0015] S4. Based on the predicted state, construct three types of measurement models: laser ranging, angle encoding, and line-of-sight angular velocity. Compare the actual sensor measurements with the theoretical values ​​to generate laser ranging residuals, angle residuals, and line-of-sight angular velocity residuals, respectively.

[0016] S5. Calculate the Kalman gain using the laser ranging residual, angle residual, and line-of-sight angular velocity residual generated in step S4, update the augmented state vector and error covariance matrix, output the optimized target position, and complete the high-precision positioning of the target.

[0017] Preferably, the drone platform is also equipped with a flight control IMU to output the aircraft's attitude matrix;

[0018] The strapdown-type optoelectronic pod integrates a MEMS-IMU, an image sensor, a laser rangefinder, and an angle encoder. The MEMS-IMU outputs the raw angular velocity and raw linear acceleration; the laser rangefinder outputs the measured slant distance from the UAV to the target; and the angle encoder outputs the measured azimuth angle α and pitch angle β of the pod relative to the aircraft.

[0019] The transformation formula for unifying coordinates to the navigation coordinate system is as follows:

[0020] ;

[0021] In the formula: This represents the three-dimensional position vector of the UAV in the northeastern sky navigation coordinate system; The rotation transformation matrix from the geocentric Earth coordinate system to the northeast celestial coordinate system is represented by a 3×3 dimensionless matrix. This represents the three-dimensional position vector of the UAV in the geocentric fixed coordinate system. This represents the reference position vector of the origin of the local navigation coordinate system in the geocentric fixed coordinate system.

[0022] Preferably, the augmented state vector includes the target's geographical location to be determined and the system error term that needs to be estimated online; the expression is:

[0023] ;

[0024] In the formula: These are the three-dimensional geographic coordinates of the target, representing the position of the stationary target to be estimated; and These are the real-time zero-bias error vectors of the gyroscope and accelerometer of the pod IMU, respectively; It is the static error vector between the image sensor and the laser rangefinder, used to correct installation deviations.

[0025] Preferably, the target location model is used to simplify prediction, i.e., the target at time... k With time k The position of -1 satisfies the following equation:

[0026] ;

[0027] The IMU zero-bias error model describes the IMU's zero-bias error as a first-order Markov process or a random walk process; wherein, the real-time zero-bias error vector of the gyroscope... The expression is as follows:

[0028] ;

[0029] In the formula: express k The zero-bias three-dimensional estimation vector of the gyroscope at time t; This represents a three-dimensional vector of angular random walk coefficients, with components corresponding to the XYZ axes; Indicates the Kalman filter update time interval; This represents a three-dimensional Gaussian white noise vector that follows a standard normal distribution.

[0030] Real-time zero bias error vector of accelerometer The expression is as follows:

[0031] ;

[0032] In the formula: This represents the zero-bias three-dimensional estimation vector of the accelerometer at time k; This represents the three-dimensional estimation vector of the accelerometer zero bias at time k-1; Represents the three-dimensional vector of acceleration random walk coefficients; Indicates the Kalman filter update time interval; Represents a three-dimensional Gaussian white noise vector;

[0033] The equations for the static error model are expressed as follows:

[0034] .

[0035] Preferably, in step S3, a process model is used, utilizing time... k -1 predicts the next time step. k state:

[0036] ;

[0037] Prediction error covariance matrix The formula is:

[0038] ;

[0039] In the formula: f It is a process function; It is the state transition matrix, obtained by linearizing the Jacobian matrix of the process model; Indicates time k When -1, the posterior error covariance matrix obtained after measurement update; express The transpose of the matrix; It is the process noise covariance matrix, containing and . contributions.

[0040] Preferably, the actual measured value of the laser rangefinder z range The difference between the theoretical slope distance and the actual slope distance is the laser ranging residual. r range The expression is as follows:

[0041] r range = z range - ;

[0042] ;

[0043] In the formula: This represents the theoretical slant range, which is the actual straight-line distance from the UAV to the target; This represents the predicted target position vector output in step S3; P UAV This represents the measured position vector of the UAV's BeiDou receiver; x t , y t , z t ( ) represents the predicted target location coordinates; x u , y u , z u ( ) represents the BeiDou location coordinates of the drone;

[0044] Actual measurement value of angle encoder z angle The difference between the theoretical angle and the actual angle is the angular residual. r angle The expression is as follows:

[0045] r angle = z angle -[α,β] T ;

[0046] α= ;

[0047] β= ;

[0048] In the formula: [α,β] T Indicates the theoretical angle; α represents the theoretical azimuth angle; β represents the theoretical elevation angle; This function represents the arctangent function in four quadrants and outputs the complete circular angle. x t , y t , z t ( ) represents the predicted target location coordinates; x u , y u , z u ( ) represents the BeiDou location coordinates of the drone;

[0049] The difference between the theoretical line-of-sight angular velocity and the measured line-of-sight angular velocity is the line-of-sight angular velocity residual. The formula is as follows:

[0050] ;

[0051] In the formula: This indicates the measured line-of-sight angular velocity, obtained from the readings of the pod's IMU gyroscope. Subtract the gyroscope bias estimate output in step S3 The formula is as follows:

[0052] ;

[0053] The theoretical line-of-sight angular velocity is expressed by the formula:

[0054] ;

[0055] in, L This is the line-of-sight vector from the drone to the target. L = - P UAV , Indicates a stationary target point. P UAV Indicates the BeiDou location of the drone; v UAVThis represents the velocity vector of the UAV relative to the navigation system; v target Represents the velocity vector of the target; Represents the square of the magnitude of the line-of-sight vector. L T L ; This is the rotation transformation matrix from the machine body to the line-of-sight coordinate system; This is the angular velocity vector of the UAV body.

[0056] Preferably, in step S4, the Kalman gain is calculated using the following formula:

[0057] ;

[0058] In the formula: It is the Kalman gain matrix; It is a measurement matrix or Jacobian matrix; It is the noise covariance matrix of the measurement, which includes the noise situation of the laser rangefinder, angle encoder, and IMU; It is the prediction error covariance matrix of step S3;

[0059] Based on Kalman gain and residuals, the predicted state and error covariance matrix are updated synchronously; the formula for the updated predicted state is: ;

[0060] In the formula: It is the updated state estimate at time k; This represents the predicted value at time k; It is the residual vector, which is composed of the laser ranging residual, angle residual and line-of-sight angular velocity residual generated in step S4;

[0061] The formula for the updated error covariance matrix is: ;

[0062] In the formula: It is the updated error covariance; I Represents the identity matrix; It is the Kalman gain matrix; It is a measurement matrix or Jacobian matrix; It is the prediction error covariance matrix of step S3;

[0063] Output target position The results were used for this location analysis.

[0064] Compared with the prior art, the present invention can achieve the following beneficial effects:

[0065] (1) Improved accuracy: This invention transforms a low-cost IMU, which was originally difficult to use directly for positioning due to severe drift, into a sensor that accurately measures line-of-sight dynamics. By constraining line-of-sight dynamics, the core error (zero bias) of the IMU is effectively estimated and compensated, thereby significantly improving the accuracy of the entire positioning link.

[0066] (2) Real-time online self-calibration: The system does not require complex offline IMU calibration in advance. During flight to lock onto a stationary target, the filter can automatically and in real time estimate the error terms such as the zero bias of the IMU, which greatly simplifies the deployment and maintenance of the system.

[0067] (3) Strong robustness: By tightly coupling, it integrates information from multiple heterogeneous sensors such as Beidou, laser rangefinder, encoder and IMU. The instantaneous error of a single sensor will not cause the system to fail, and the overall robustness is high.

[0068] (4) High cost-effectiveness: There is no need to upgrade the expensive high-performance fiber optic IMU. The performance of the system based on low-cost hardware can be greatly improved through algorithm innovation alone, which has extremely high cost-effectiveness.

[0069] (5) Model advancement: Compared with the traditional "loosely coupled" or "open-loop" method that first calculates the attitude and then performs positioning, the "tightly coupled" method of this invention, which estimates the target state and the sensor error state under the same framework, is theoretically superior and can achieve higher accuracy. Attached Figure Description

[0070] Figure 1 This is an algorithm flowchart of a tightly coupled filtering positioning method based on line-of-sight constraints under a fast-linkage base, provided by an embodiment of the present invention. Detailed Implementation

[0071] In the following description, embodiments of the invention will be described with reference to the accompanying drawings. In the description below, the same modules are denoted by the same reference numerals. Where the same reference numerals are used, their names and functions are also the same. Therefore, their detailed description will not be repeated.

[0072] To make the objectives, technical solutions, and advantages of this invention clearer, the invention will be further described in detail below with reference to the accompanying drawings and specific embodiments. It should be understood that the specific embodiments described herein are merely illustrative of the invention and do not constitute a limitation thereof.

[0073] This invention provides a tightly coupled filtering positioning method based on line-of-sight constraints under a fast-linkage base, specifically including the following steps:

[0074] S1. Data Acquisition and Coordinate Unification

[0075] S101. Hardware deployment of the target positioning system: The hardware includes a UAV platform, a strapdown optoelectronic pod, and a positioning calculation processor, as follows: The UAV platform is equipped with a Beidou receiver (three-dimensional position measurement accuracy better than 10m, data output frequency 10Hz) and a flight control IMU (body attitude measurement accuracy: Euler angle / quaternion attitude angle error less than 0.5°, data output frequency 10Hz).

[0076] The strapdown electro-optical pod is mounted under the drone's fuselage via a gimbal structure, allowing for free rotation. The pod integrates a MEMS-IMU, image sensors (such as cameras for locking onto stationary ground targets), and a laser rangefinder (for measuring slant distance). R ) and angle encoder; a low-cost MEMS-IMU fixed to the camera base measures the angular velocity of the pod in real time;

[0077] In a specific embodiment: a low-cost MEMS-IMU is strapped down to the camera base, with an angular velocity measurement range of ±200° / s and an initial zero-bias drift rate of 5° / h, used to collect pod angular velocity and linear acceleration data; an image sensor with a resolution of 1920×1080 and a frame rate of 30fps is used to lock onto and track stationary ground targets (such as buildings and landmarks); a laser rangefinder with a ranging range of 50~2000m, a ranging error of ±0.5m, and an output frequency of 5Hz is used to collect slant distance data from the UAV to the target; an angle encoder with an azimuth range of 0~360°, a pitch range of -90°~90°, an angle error of ±0.1°, and an output frequency of 20Hz is used to collect the azimuth angle α and pitch angle β of the pod relative to the UAV body;

[0078] The positioning and calculation processor is an embedded industrial computer with a main frequency of 2.0 GHz and 4 GB of memory. It is connected to the UAV flight control and various sensors of the optoelectronic pod via CAN bus, and the algorithm running cycle (Kalman filter update interval Δt) has been set to 0.1 s.

[0079] S102. Multivariate Data Acquisition and Coordinate Unification

[0080] The positioning processor synchronously collects the following raw data according to the output frequency of each sensor: the UAV's BeiDou position output by the BeiDou receiver. P UAV The airframe attitude matrix output by the flight control IMU The raw angular velocity and raw linear acceleration output by the MEMS-IMU; the measured slant distance from the UAV to the target output by the laser rangefinder; and the measured azimuth angle α and pitch angle β of the pod relative to the aircraft output by the angle encoder.

[0081] The collected data is unified to the navigation coordinate system (taking the Northeast-Sky coordinate system as an example) through coordinate transformation. The transformation formula is as follows:

[0082] ;

[0083] In the formula: This represents the three-dimensional position vector of the UAV in the Northeastern Sky Navigation Coordinate System (NED), with the unit being meters (m). The rotation transformation matrix from the geocentric Earth-fixed coordinate system (ECEF) to the northeast-sky coordinate system (NED) is a 3×3 dimensionless matrix. This represents the three-dimensional position vector of the UAV in the geocentric fixed coordinate system (ECEF), with the unit being meters (m). This represents the reference position vector of the origin of the local navigation coordinate system in the ECEF coordinate system, in meters (m).

[0084] S2. Constructing an Extended Kalman Filter Based on Augmented State Vectors: Design an Extended Kalman Filter (EKF) to simultaneously estimate the target position and system error using augmented state vectors, thereby achieving high-precision real-time estimation of the target position and synchronously compensating for system errors; the specific operation is as follows:

[0085] Define the augmented state vector of the extended Kalman filter (EKF) x It not only includes the target's geographical location to be determined, but also the system error term that needs to be estimated online; the augmented state vector allows the filter to estimate the target location and system error simultaneously, avoiding the separate processing of error compensation in traditional methods and improving the overall estimation accuracy; in the extended Kalman filter (EKF), the augmented state (such as adding IMU zero bias) can effectively handle the dynamic characteristics of sensor errors, and the definition of the state vector directly affects the construction of subsequent process models and measurement models; the expression of the augmented state vector is:

[0086] ;

[0087] In the formula: These are the target's three-dimensional geographic coordinates (such as latitude, longitude, altitude, or ECEF coordinates), representing the position of the stationary target to be estimated; and These are the real-time zero-bias error vectors of the gyroscope and accelerometer of the pod IMU (units are rad / s and m / s, respectively). 2 These errors change slowly over time and need to be estimated online to compensate for sensor drift; It is the static error vector between the image sensor and the laser rangefinder, used to correct installation deviations.

[0088] The augmented state vector of this step x It forms the basis of the process model in step S3, and is an element of the state vector. and It will be modeled as a stochastic process in S3.

[0089] In a specific embodiment, filter initialization includes: obtaining a rough initial position of the target using a conventional open-loop calculation method (without using IMU angular velocity information). Initialize augmented state vector And error covariance matrix ; The initial value of the IMU error term can be set to zero, but a large initial covariance is given, indicating that the uncertainty of the initial zero bias is very high.

[0090] S3. Based on the augmented state vector defined in step S2 x Define the process model, including the target position model, the IMU zero-bias error model, and the static error model; update the state vector and its covariance matrix over time based on the process model; specifically as follows:

[0091] Target position model (static constraint): Since the ground target is stationary, its position state does not change over time; that is, the target remains stationary at time 10:00. k With time k The position of -1 satisfies:

[0092] ;

[0093] The target location model simplifies prediction and reduces state uncertainty;

[0094] IMU zero-bias error model: This refers to the zero-bias error of the IMU. and The model is a first-order Markov process or a random walk process, reflecting its slowly time-varying characteristics; among them, the real-time zero-bias error vector of the gyroscope... The expression is as follows:

[0095] ;

[0096] In the formula: express k The zero-bias three-dimensional estimation vector of the gyroscope at time t, in radians per second (rad / s). denoted as a three-dimensional vector of angular random walk coefficients, in units of radians per square second (rad / √s), with components corresponding to the XYZ axes; This indicates the Kalman filter update time interval, in seconds (s). This represents a three-dimensional Gaussian white noise vector that follows a standard normal distribution.

[0097] Real-time zero bias error vector of accelerometer Similarly, but using the acceleration random walk coefficient, it is expressed as follows:

[0098] ;

[0099] In the formula: This represents the three-dimensional zero-bias estimate vector of the accelerometer at time k, in meters per second squared (m / s²). 2 ); This represents the three-dimensional accelerometer zero-bias estimate vector at time k-1, in meters per second squared (m / s²). 2 ); This represents a three-dimensional vector of acceleration random walk coefficients, with units of meters per square second (m² / sq.) The components correspond to the XYZ axes respectively; This indicates the Kalman filter update time interval, in seconds (s). This represents a three-dimensional Gaussian white noise vector that follows a standard normal distribution with a mean of 0 and a covariance of the identity matrix I.

[0100] Static error model: The installation deviation between the image sensor and the laser rangefinder is static and does not change over time. Therefore, the static error vector (used to correct for installation deviation) is represented as follows: .

[0101] Using a process model, through process functions f (Equations based on the process model), utilizing time... k -1 predicts the next time step. k state:

[0102] ;

[0103] Prediction error covariance matrix The formula is:

[0104] ;

[0105] In the formula: It is the state transition matrix (obtained by linearizing the Jacobian matrix of the process model); Indicates time k When -1, the posterior error covariance matrix obtained after measurement update; express The transpose of the matrix; It is the process noise covariance matrix, containing and . contributions.

[0106] This step's principle can be briefly described as follows: The process model describes how the state evolves over time, with the core being state prediction and error covariance prediction. In this step, the target is stationary, and the IMU is modeled as a first-order Markov process or a random walk process to reflect its slow time-varying characteristics. The input comes from the augmented state vector definition in step S2, and the output is (predicted state). and the predicted covariance matrix This is directly used for the measurement update in step S4; for example, the predicted target location. This will be used to calculate theoretical measurements (such as slope distance and angle). The specific logic for time updates is as follows: In k -1 to k At any given moment, due to the target location Assuming to be at rest, its state prediction is equal to the estimate from the previous time step; IMU zero bias. When propagating over time using its random walk model, the uncertainty (covariance) will increase slightly.

[0107] S4. Define the multi-source measurement model (measurement update): based on the predicted state output from step S3. Models for three types of measurements—laser ranging, angle encoding, and line-of-sight angular velocity—are constructed. The actual sensor measurements are compared with theoretical values ​​to generate residuals, providing a basis for the state update in step S5. Specifically:

[0108] (1) Laser ranging measurement model

[0109] Theoretical slant range calculation: combining UAV BeiDou positioning P UAV and the predicted target location output in step S3 The theoretical slope distance is calculated using the following formula:

[0110] ;

[0111] In the formula: The theoretical slant range is the actual straight-line distance from the UAV to the target, expressed in meters (m). This represents the predicted target position vector output in step S3, in three-dimensional coordinates. x t , y t , z t ), unit: meter (m); P UAV This represents the measured position vector and three-dimensional coordinates of the UAV's BeiDou receiver. x u , y u , z u ), unit: meter (m);

[0112] Residual calculation: Actual measured value of laser rangefinder z range The difference between the slope distance and the theoretical slope distance is the residual. r range ,Right now rrange = z range - .

[0113] In a specific embodiment, the precise formula for calculating the theoretical value of laser ranging is:

[0114] .

[0115] (2) Angle coding measurement model

[0116] Theoretical calculation: based on the UAV's BeiDou position P UAV The UAV attitude (such as quaternions or Euler angles) and the predicted target position output in step S3. The theoretical azimuth and elevation angles that the pod should point to are calculated in reverse.

[0117] Residual calculation: Actual measured values ​​from the pod angle encoder z angle The difference from the theoretical angle is the residual. r angle ,Right now r angle = z angle -[α,β] T ;

[0118] α= ;

[0119] β= ;

[0120] In the formula: [α,β] T Indicates the theoretical angle; α represents the theoretical azimuth angle; β represents the theoretical elevation angle; This function represents the arctangent function in four quadrants and outputs the complete circular angle. x t , y t , z t ( ) represents the predicted target location coordinates; x u , y u , z u () represents the BeiDou location coordinates of the drone.

[0121] In a specific embodiment, the predicted pod angle (azimuth angle) is... Pitch angle The calculation requires a transformation from the body coordinate system to the navigation coordinate system. With the BeiDou positioning of drones P UAV ( k ), body attitude matrix The inverse calculation theory points the line of sight, ultimately yielding the corresponding angle.

[0122] (3) IMU line-of-sight angular velocity measurement model (core)

[0123] Theoretical line-of-sight angular velocity calculation: Theoretical line-of-sight angular velocity Calculations using purely geometric methods describe the drone's velocity. V UAV During flight, continuously point towards a stationary target point. line-of-sight vector L The angular velocity is given by the formula:

[0124] ;

[0125] in, This is the theoretical line-of-sight angular velocity vector (unit: rad / s, in the line-of-sight coordinate system). L This is the line-of-sight vector from the UAV to the target (unit: m). L = - P UAV , Indicates a stationary target point. P UAV Indicates the BeiDou location of the drone; v UAV This represents the velocity vector of the UAV relative to the navigation system; v target Represents the velocity vector of the target; Represents the square of the magnitude of the line-of-sight vector. L T L ; The rotation transformation matrix from the machine body to the line-of-sight coordinate system is 3×3. This is the angular velocity vector of the UAV body (unit: rad / s, from the flight control main inertial navigation).

[0126] Calculation of line-of-sight angular velocity: Measurement of line-of-sight angular velocity Readings from the pod's IMU gyroscope Subtract the gyroscope bias estimate output in step S3 The formula is as follows: ;

[0127] Residual calculation: The difference between the theoretical line-of-sight angular velocity and the measured line-of-sight angular velocity is the residual. The formula is as follows:

[0128] ;

[0129] The residual is a key innovation because it is associated with both the target position and the estimation error of the IMU zero bias.

[0130] Principle summary: The input of this step depends entirely on the output of step S3 to predict the state. (in particular and ) is used to calculate all theoretical measurements; for example, Used to calculate slope distance and angle Used to correct gyroscope readings The residual of this step This is the core principle. In EKF, the line-of-sight angular velocity residual effectively couples position and IMU errors because geometry is sensitive to position changes, while IMU zero bias directly affects angular velocity measurement. The multi-source measurement fusion (laser, angle, angular velocity) in this step improves robustness, and multi-modal sensor fusion reduces uncertainty through redundancy. The residual output will be used for Kalman gain calculation in step S5.

[0131] S5. Filtering and State Update: Utilize the three sets of residuals (laser ranging residuals) generated in step S4. r range Angular residual r angle and line-of-sight angular velocity residual Calculate the Kalman gain and update the augmented state vector. x Sum the error covariance matrix and output the optimized target position. calibrated IMU zero bias error and To achieve high-precision positioning of the target;

[0132] The filter optimizes state estimation by minimizing the residuals, with particular emphasis on How to simultaneously adjust the target position and IMU zero bias;

[0133] The formula for calculating the Kalman gain matrix is ​​as follows:

[0134] ;

[0135] In the formula: It is the Kalman gain matrix, which balances the weights of prediction and measurement; It is the measurement matrix (or Jacobian matrix), which is obtained by linearizing the measurement model in step S4 (reflecting the linear relationship between measurement and state). It is a measurement noise covariance matrix, which includes the noise status of sensors such as laser rangefinders, angle encoders, and IMUs; It is the prediction error covariance matrix of step S3, which describes the degree of uncertainty of the predicted state;

[0136] Based on Kalman gain and residuals, the predicted state and error covariance matrix are updated synchronously.

[0137] The updated prediction state formula is: ;

[0138] In the formula: It is the updated state estimate at time k; This represents the predicted value at time k; It is the residual vector, generated from the laser ranging residual in step S4. r range Angular residual r angle and line-of-sight angular velocity residual The composition reflects the deviation between the actual measured value of the sensor and the theoretical value calculated based on the prior state (the larger the residual, the greater the gap between the predicted value and the measured value, and the more significant the correction is required).

[0139] drive and Common adjustments: for example, if Larger, gain Weights will be assigned to correct position or zero bias;

[0140] The formula for the updated error covariance matrix is: ;

[0141] In the formula, It is the updated error covariance, reflecting the reduction in estimation uncertainty; I Represents the identity matrix; It is the Kalman gain matrix; It is a measurement matrix (or Jacobian matrix); It is the prediction error covariance matrix of step S3;

[0142] Output target position calibrated IMU zero bias error and The results of this positioning are used, or fed back to step S3 of the next iteration, where and IMU zero-bias prediction for the next time step As a basis for the target's static constraint, this ensures continuous optimization of the filter. The final output of this step is that the filter not only optimizes the target position but also provides online IMU bias estimation, reducing reliance on high-cost sensors.

[0143] In a specific embodiment, the expressions for updating the state vector and covariance matrix are as follows:

[0144] ;

[0145] ;

[0146] Updated state vector This includes the current optimal target position estimate. and IMU zero bias estimation The former is output as the final result, while the latter is used for IMU data compensation in the next cycle. Through this iterative process, the system utilizes the prior information that the target is stationary, so that the IMU measurement data can be used to calibrate its own error and correct the target position.

[0147] In summary, the method of this invention does not directly use IMU integration to calculate attitude, but instead uses it as a sensor to measure the line-of-sight angular velocity between the UAV and a stationary target. By establishing an augmented state Kalman filter that includes the target's geographical location and the IMU's dynamic error term, the actual line-of-sight angular velocity measured by the IMU is compared with the theoretical line-of-sight angular velocity calculated based on the UAV's kinematics and target position estimation. Utilizing the residual between the two, the filter can simultaneously perform optimal estimation and real-time calibration of the target position and the IMU's own zero bias. This invention requires no expensive hardware and, through algorithmic innovation, achieves online self-calibration of a low-cost IMU, significantly improving the positioning accuracy and robustness for any stationary target on the ground.

[0148] It should be understood that the various forms of processes shown above can be used to reorder, add, or delete steps. For example, the steps described in this invention disclosure can be executed in parallel, sequentially, or in different orders, as long as the desired result of the technical solution disclosed in this invention can be achieved, and this is not limited herein.

[0149] The specific embodiments described above do not constitute a limitation on the scope of protection of this invention. Those skilled in the art should understand that various modifications, combinations, sub-combinations, and substitutions can be made according to design requirements and other factors. Any modifications, equivalent substitutions, and improvements made within the spirit and principles of this invention should be included within the scope of protection of this invention.

Claims

1. A tightly coupled filtering positioning method based on line-of-sight constraints under a fast-linkage base, characterized in that: Specifically, the steps include the following: S1. Build a target positioning system, including a UAV platform equipped with a Beidou receiver, a strapdown optoelectronic pod, and a positioning calculation processor; collect the UAV's Beidou position, body attitude, raw angular velocity and raw linear acceleration, laser rangefinder range measurement value, and angle encoder reading, and unify them to the navigation coordinate system through coordinate transformation; The drone platform is also equipped with a flight control IMU, which outputs the body attitude matrix; The strapdown-type optoelectronic pod integrates a MEMS-IMU, an image sensor, a laser rangefinder, and an angle encoder. The MEMS-IMU outputs the raw angular velocity and raw linear acceleration; the laser rangefinder outputs the measured slant distance from the UAV to the target; and the angle encoder outputs the measured azimuth angle α and pitch angle β of the pod relative to the aircraft. The transformation formula for unifying coordinates to the navigation coordinate system is as follows: ； In the formula: This represents the three-dimensional position vector of the UAV in the northeastern sky navigation coordinate system; The rotation transformation matrix from the geocentric Earth coordinate system to the northeast celestial coordinate system is represented by a 3×3 dimensionless matrix. This represents the three-dimensional position vector of the UAV in the geocentric fixed coordinate system. This represents the reference position vector of the origin of the local navigation coordinate system in the geocentric fixed coordinate system. S2. Design an extended Kalman filter to simultaneously estimate the target position and system error by augmenting the state vector; S3. Define a process model based on the augmented state vector in step S2. The process model includes a target position model, an IMU zero bias error model, and a static error model. Update the state vector and its covariance matrix over time according to the process model and output the predicted state. S4. Based on the predicted state, construct three types of measurement models: laser ranging, angle encoding, and line-of-sight angular velocity. Compare the actual sensor measurements with the theoretical values ​​to generate laser ranging residuals, angle residuals, and line-of-sight angular velocity residuals, respectively. S5. Calculate the Kalman gain using the laser ranging residual, angle residual, and line-of-sight angular velocity residual generated in step S4, update the augmented state vector and error covariance matrix, output the optimized target position, and complete the high-precision positioning of the target.

2. The tightly coupled filtering positioning method based on line-of-sight constraints under a fast-linkage base according to claim 1, characterized in that: The augmented state vector includes the target's geographical location to be determined and the system error term that needs to be estimated online; the expression is: ； In the formula: These are the three-dimensional geographic coordinates of the target, representing the position of the stationary target to be estimated; and These are the real-time zero-bias error vectors of the gyroscope and accelerometer of the pod IMU, respectively; It is the static error vector between the image sensor and the laser rangefinder, used to correct installation deviations.

3. The tightly coupled filtering positioning method based on line-of-sight constraints under a fast-linkage base according to claim 2, characterized in that: The target location model is used to simplify prediction, i.e., the target at time... k With time k The position of -1 satisfies the following equation: ； The IMU zero-bias error model describes the IMU's zero-bias error as a first-order Markov process or a random walk process; wherein, the real-time zero-bias error vector of the gyroscope... The expression is as follows: ； In the formula: express k The zero-bias three-dimensional estimation vector of the gyroscope at time t; This represents a three-dimensional vector of angular random walk coefficients, with components corresponding to the XYZ axes; Indicates the Kalman filter update time interval; This represents a three-dimensional Gaussian white noise vector that follows a standard normal distribution. Real-time zero bias error vector of accelerometer The expression is as follows: ； In the formula: This represents the zero-bias three-dimensional estimation vector of the accelerometer at time k; This represents the three-dimensional estimation vector of the accelerometer zero bias at time k-1; Represents a three-dimensional vector of acceleration random walk coefficients; Indicates the Kalman filter update time interval; Represents a three-dimensional Gaussian white noise vector; The equations for the static error model are expressed as follows: 。 4. The tightly coupled filtering positioning method based on line-of-sight constraints under a fast-linkage base according to claim 3, characterized in that: In step S3, a process model is used, utilizing time... k -1 predicts the next time step. k state: ； Prediction error covariance matrix The formula is: ； In the formula: f It is a process function; It is the state transition matrix, obtained by linearizing the Jacobian matrix of the process model; Indicates time k At time 1, the posterior error covariance matrix obtained after measurement update; express The transpose of the matrix; It is the process noise covariance matrix, containing and . contributions.

5. The tightly coupled filtering positioning method based on line-of-sight constraints under a fast-linkage base according to claim 1, characterized in that: Actual measurement value of laser rangefinder z range The difference between the theoretical slope distance and the actual slope distance is the laser ranging residual. r range The expression is as follows: r range = z range ； ； In the formula: This represents the theoretical slant range, which is the actual straight-line distance from the UAV to the target; This represents the predicted target position vector output in step S3; P UAV This represents the measured position vector of the UAV's BeiDou receiver; x t , y t , z t ( ) represents the predicted target location coordinates; x u , y u , z u ( ) represents the BeiDou location coordinates of the drone; Actual measurement value of angle encoder z angle The difference between the theoretical angle and the actual angle is the angular residual. r angle The expression is as follows: r angle = z angle [α,β] T ； α= ； β= ； In the formula: [α,β] T Indicates the theoretical angle; α represents the theoretical azimuth angle; β represents the theoretical elevation angle; This function represents the arctangent function in four quadrants and outputs the complete circular angle. x t , y t , z t ( ) represents the predicted target location coordinates; x u , y u , z u ( ) represents the BeiDou location coordinates of the drone; The difference between the theoretical line-of-sight angular velocity and the measured line-of-sight angular velocity is the line-of-sight angular velocity residual. The formula is as follows: ； In the formula: This indicates the measured line-of-sight angular velocity, obtained from the readings of the pod's IMU gyroscope. Subtract the gyroscope bias estimate output in step S3 The formula is as follows: ； The theoretical line-of-sight angular velocity is expressed by the formula: ； in, L This is the line-of-sight vector from the drone to the target. L = P UAV , Indicates a stationary target point. P UAV Indicates the drone's BeiDou location; v UAV This represents the velocity vector of the UAV relative to the navigation system; v target Represents the velocity vector of the target; Represents the square of the magnitude of the line-of-sight vector. L T L ; This is the rotation transformation matrix from the machine body to the line-of-sight coordinate system; This is the angular velocity vector of the UAV body.

6. The tightly coupled filtering positioning method based on line-of-sight constraints under a fast-linkage base according to claim 1, characterized in that: In step S4, the Kalman gain is calculated using the following formula: ； In the formula: It is the Kalman gain matrix; It is a measurement matrix or Jacobian matrix; It is the noise covariance matrix of the measurement, which includes the noise situation of the laser rangefinder, angle encoder, and IMU; It is the prediction error covariance matrix of step S3; Based on Kalman gain and residuals, the predicted state and error covariance matrix are updated synchronously; the formula for the updated predicted state is: ; In the formula: It is the updated state estimate at time k; This represents the predicted value at time k; It is the residual vector, which is composed of the laser ranging residual, angle residual and line-of-sight angular velocity residual generated in step S4; The formula for the updated error covariance matrix is: ; In the formula: It is the updated error covariance; I Represents the identity matrix; It is the Kalman gain matrix; It is a measurement matrix or Jacobian matrix; It is the prediction error covariance matrix of step S3; Output target position The results were used for this location analysis.

Images