Fusion positioning method and apparatus, aerial vehicle, medium, and program product

Abstract

The application provides a fusion positioning method and device, an aircraft, a medium and a program product, and relates to the technical field of navigation positioning. The method comprises the following steps: constructing and initializing a state vector to be estimated; the state vector comprises a position in a VINS coordinate system, a position drift term, and a yaw conversion angle of a coordinate system to be fused; performing a prediction update based on acceleration data to obtain a current predicted state vector and an error covariance thereof; performing a correction update on the current predicted state vector and the error covariance thereof by using position observation data; and determining fusion navigation positioning information based on the current predicted state vector or the state vector after the correction update. By explicitly introducing the VINS position drift term and the coordinate system yaw conversion angle into the EKF state vector and performing online estimation, the cumulative error of the VINS can be modeled and compensated, and the conversion relationship between the two coordinate systems can be adaptively calibrated, so that the accuracy and robustness of the fusion positioning are effectively improved.

Description

Technical Field

[0001] This application relates to the field of navigation and positioning technology, and more specifically, to a fusion positioning method, device, aircraft, medium, and program product. Background Technology

[0002] Visual-Inertial Navigation Systems (VINS) can estimate the six-DOF pose of a vehicle in real time through data fusion between a camera and an inertial measurement unit (IMU). However, this system exhibits significant cumulative drift during prolonged operation or in environments with poor texture and drastic lighting changes. Furthermore, its performance is highly sensitive to the accuracy of time synchronization between sensors, initial state estimation, and extrinsic parameter calibration errors between the camera and the IMU.

[0003] Visual marker navigation systems (such as AprilTag) can calculate a high-precision, error-free six-DOF pose relative to a single frame of an image by recognizing specific visual markers (such as QR codes) in the environment. While robust and with mature open-source implementations, their proper functioning depends entirely on the visual markers remaining continuously within the camera's field of view. Furthermore, they are sensitive to the pixel size, distance, and occlusion of the markers; at long distances, with small pixels, or with partial occlusion, the observed data becomes noisier or may be completely lost.

[0004] The Extended Kalman Filter (EKF) fusion framework is a classic method for integrating multi-source sensor data. EKF uses a nonlinear state estimation framework to fuse data from visual-inertial navigation systems and visual marker navigation systems, thereby improving the accuracy and robustness of pose estimation. Its core lies in constructing a unified state-space model to coordinate data from multiple heterogeneous sensors.

[0005] Existing localization schemes based on EKF and VINS and visual marker fusion lack modeling, estimation and compensation for the cumulative drift of VINS. The fusion filter treats the drift error contained in the VINS output as reliable observation information, causing this error to be included in the system state estimation and cannot be eliminated in the long term.

[0006] In summary, existing EKF-based fusion positioning schemes suffer from low accuracy and robustness. Summary of the Invention

[0007] The purpose of this application is to provide a fusion positioning method, device, aircraft, medium, and program product to improve the accuracy and robustness of the fusion positioning scheme.

[0008] In a first aspect, embodiments of this application provide a fusion localization method applied to an extended Kalman filter, comprising:

[0009] Construct and initialize the state vector to be estimated; wherein, the state vector includes the position in the visual inertial navigation system coordinate system. Visual inertial navigation system position drift item and the yaw transformation angle from the visual inertial navigation system coordinate system to the visual marker navigation coordinate system. ;

[0010] The state vector to be estimated is predicted and updated based on the acceleration data collected by the inertial measurement unit, so as to obtain the current predicted state vector and its error covariance.

[0011] Upon receiving location observation data, the current predicted state vector and its error covariance are corrected and updated using the location observation data.

[0012] The fused navigation and positioning information is determined based on the currently predicted state vector or the corrected and updated state vector.

[0013] In this embodiment, by explicitly introducing the VINS position drift term and coordinate system yaw transformation angle into the EKF state vector and estimating them online, the cumulative error of VINS can be modeled and compensated, and the transformation relationship between the two coordinate systems can be adaptively calibrated. This effectively suppresses positioning drift under long-term operation and reduces the dependence on the accuracy of fixed external parameter calibration, thereby improving the accuracy and robustness of fusion positioning.

[0014] In some embodiments, the position observation data includes first position observation data provided by a visual inertial navigation system in its own coordinate system and second position observation data provided by a visual marker navigation system in its own coordinate system.

[0015] In this embodiment, by clearly distinguishing and integrating the relative position observation data provided by VINS and the absolute position observation data provided by visual markers, the advantages of the former being continuous and the latter being accurate can be comprehensively utilized to provide the system with multi-source observation information that balances smoothness and global consistency.

[0016] In some embodiments, the step of correcting and updating the currently predicted state vector and its error covariance using the location observation data includes:

[0017] When the first position observation data and the second position observation data are received simultaneously within the same prediction period, the current predicted state vector and its error covariance are corrected and updated for the first time according to the preset first observation model, and the state vector after the first correction and update is corrected and updated for the second time according to the preset second observation model.

[0018] In the embodiments of this application, by specifying that the two types of observations are updated in a step-by-step manner, first VINS and then visual marking, within the same prediction period, it is possible to prioritize stabilizing short-term motion estimation and then perform absolute calibration in scenarios where VINS observations are high-frequency and continuous and visual marking observations may arrive intermittently, thereby improving the numerical stability of the filter and its robustness to asynchronous observations.

[0019] In some embodiments, the first observation model is represented as follows: the first position observation data is obtained by calculating the position in the visual inertial navigation system coordinate system, the visual inertial navigation system position drift term, and the position observation noise in the visual inertial navigation system coordinate system.

[0020] The second observation model is expressed as follows: the second position observation data is obtained by calculation based on the two-dimensional rotation matrix constructed by the yaw conversion angle, the position in the visual inertial navigation system coordinate system, and the position observation noise in the visual marker navigation coordinate system.

[0021] In the embodiments of this application, by establishing corresponding mathematical models for the two types of observations, the constraint relationship between each observation and the state vector can be clearly defined from a mechanistic perspective, thereby guiding EKF to correctly allocate the observation error to the drift, position and yaw angle states respectively, and achieving accurate error compensation and coordinate alignment.

[0022] In some embodiments, the initial value of the yaw conversion angle is obtained by calculating the difference in azimuth angle between the position output by the visual inertial navigation system at the initialization time and the position observation data in the visual marker navigation coordinate system in the horizontal plane.

[0023] In this embodiment of the application, by directly calculating the initial value of the yaw conversion angle using the position observation data of the two systems at the initial moment, the initial azimuth alignment relationship between the two coordinate systems can be quickly established, thereby providing accurate initial heading constraints for the filter and accelerating the subsequent state convergence process.

[0024] In some embodiments, the initial value of the position drift term of the visual inertial navigation system is determined based on the difference between the position output by the visual inertial navigation system at the initialization time and the corresponding position observation data.

[0025] In this embodiment of the application, by setting the difference between the initial position observation data of VINS and its output position as the initial value of the drift term, the initial zero bias of VINS can be calibrated when the system starts up, thereby providing a reasonable starting point for subsequent online drift estimation.

[0026] In some embodiments, the state vector further includes velocity and accelerometer zero bias in the visual inertial navigation system coordinate system.

[0027] In this embodiment, by further including the velocity and accelerometer zero-bias state in the state vector, the motion state of the carrier and the system error of the IMU can be more comprehensively modeled and estimated, thereby improving the accuracy of state prediction and providing richer output information for the navigation system.

[0028] In some embodiments, determining the fused navigation and positioning information based on the currently predicted state vector or the corrected and updated state vector includes:

[0029] Based on the position in the visual-inertial navigation system coordinate system in the corrected and updated state vector. and yaw conversion angle Determine the fusion position in the visual marker navigation coordinate system The fused navigation and positioning information is used as the fused position in the visual marker navigation coordinate system. The fused position is calculated based on a two-dimensional rotation matrix constructed from the yaw conversion angle and the position in the visual inertial navigation system coordinate system.

[0030] In this embodiment, by rotating the corrected VINS position to the visual marker navigation system via an online estimated yaw angle, the fused positioning result with compensated drift in the global coordinate system can be directly output, thereby providing users or upper-level control systems with ready-to-use, accurate and globally consistent navigation information.

[0031] Secondly, embodiments of this application provide a fusion positioning device applied to an extended Kalman filter, comprising:

[0032] An initialization module is used to construct and initialize the state vector to be estimated; wherein the state vector includes the position in the visual inertial navigation system coordinate system, the visual inertial navigation system position drift term, and the yaw transformation angle from the visual inertial navigation system coordinate system to the visual marker navigation coordinate system;

[0033] The prediction update module is used to predict and update the state vector to be estimated based on the acceleration data collected by the inertial measurement unit, so as to obtain the current predicted state vector and its error covariance.

[0034] The state correction module is used to correct and update the currently predicted state vector and its error covariance using the received position observation data.

[0035] The fusion positioning module is used to determine fusion navigation and positioning information based on the currently predicted state vector or the corrected and updated state vector.

[0036] Thirdly, embodiments of this application provide an aircraft including a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor, when executing the program, can implement the method described in any embodiment of the first aspect.

[0037] Fourthly, embodiments of this application provide a computer-readable storage medium storing a computer program, which, when executed by a processor, can implement the method described in any embodiment of the first aspect.

[0038] Fifthly, embodiments of this application provide a computer program product, the computer program product including a computer program, wherein when the computer program is executed by a processor, it can implement the method described in any embodiment of the first aspect. Attached Figure Description

[0039] To more clearly illustrate the technical solutions of the embodiments of this application, the accompanying drawings used in the embodiments of this application will be briefly introduced below. It should be understood that the following drawings only show some embodiments of this application and should not be regarded as a limitation of the scope. For those skilled in the art, other related drawings can be obtained based on these drawings without creative effort.

[0040] Figure 1 A flowchart illustrating a fusion positioning method provided in an embodiment of this application;

[0041] Figure 2 This is a schematic diagram of the structure of a fusion positioning device provided in an embodiment of this application;

[0042] Figure 3 This is a schematic diagram of the structure of an aircraft provided in an embodiment of this application. Detailed Implementation

[0043] The technical solutions in the embodiments of this application will now be described with reference to the accompanying drawings.

[0044] It should be noted that similar reference numerals and letters in the following figures indicate similar items; therefore, once an item is defined in one figure, it does not need to be further defined and explained in subsequent figures. Furthermore, in the description of this application, terms such as "first," "second," etc., are used only to distinguish descriptions and should not be construed as indicating or implying relative importance.

[0045] It should be noted that the Extended Kalman Filter (EKF) fusion framework can filter and fuse the Visual Inertial Navigation System (VINS) and the Visual Marking Navigation System to improve the accuracy and robustness of pose estimation.

[0046] However, in existing EKF fusion positioning schemes, the lack of modeling, estimation, and compensation for the cumulative drift of VINS leads to the gradual accumulation of drift errors in long-term positioning scenarios, which cannot be eliminated. At the same time, if fixed coordinate transformation parameters (extrinsic parameters) that may contain errors are used to associate the VINS coordinate system with the world coordinate system, installation deviations and calibration errors will be systematically introduced, further reducing the long-term accuracy and consistency of the fusion results.

[0047] Specifically, existing EKF fusion positioning solutions have at least one of the following problems:

[0048] 1. When using only VINS output as observation for fusion, the drift is not explicitly modeled (the drift-related parameters are explicitly added to the equation model), making it difficult to suppress long-term errors;

[0049] 2. If a fixed VINS <---> world coordinate extrinsic parameter is used for pose correlation transformation, the uncertainty of the coordinates and the deviation of equipment installation or calibration will be transmitted to the fusion solution, resulting in the inability to guarantee positioning accuracy;

[0050] 3. When visual markers (such as AprilTag) are intermittently visible, joint updates are susceptible to anomalies in timestamps and information matrices.

[0051] To address at least one problem in the existing technology, this application provides a fusion positioning method. By explicitly introducing and estimating the VINS position drift term online into the EKF state vector (explicitly introducing the drift term refers to explicitly adding a parameter term reflecting the drift characteristics to the solution model), the method can dynamically identify, separate, and attribute the time-varying cumulative error in the VINS output to the change of this drift term, thereby achieving online real-time compensation for the inherent drift of VINS and significantly suppressing long-term positioning errors. Furthermore, by estimating the yaw conversion angle from the VINS frame to the world (navigation) frame online as part of the state vector and utilizing April... The absolute observation model, such as Tag, is continuously corrected to adaptively calibrate the rotation relationship between the two coordinate systems, thereby avoiding systematic errors caused by inaccurate fixed external parameters or installation deviations, and ensuring the uniformity and accuracy of global coordinates. In addition, by adopting a sequential step-by-step update strategy of VINS first and then AprilTag, when AprilTag is intermittently visible, the short-term state can be stabilized first by using high-frequency continuous VINS observations, and then the low-frequency absolute AprilTag observations can be used for calibration. This effectively reduces the instantaneous impact on the filter state caused by asynchronous observation, timestamp mismatch or single observation anomaly, and improves the robustness of the system in degradation scenarios.

[0052] like Figure 1 As shown, this application provides a fusion localization method applied to an extended Kalman filter (EKF). The method may include the following steps:

[0053] S1. Construct and initialize the state vector to be estimated; where the state vector includes the position in the visual inertial navigation system coordinate system. Visual inertial navigation system position drift item And the yaw transformation angle from the visual inertial navigation system coordinate system to the visual marker navigation coordinate system. .

[0054] In some embodiments, the state vector also includes velocity in the visual inertial navigation system coordinate system. and accelerometer zero bias .

[0055] First, it should be noted that the embodiments of this application are schemes for optimizing and fusing a visual inertial navigation system (VINS) and a visual marker navigation system (such as AprilTag) through an extended Kalman filter (EKF). The visual inertial navigation system corresponds to the local coordinates of VINS (i.e., the coordinate system where the VINS output is located, referred to as the V system), and the visual marker navigation system corresponds to the visual marker navigation coordinate system (such as the AprilTag coordinate system, referred to as the A system). The visual marker navigation coordinate system can adopt the North-East-Earth (NED) coordinate system or its approximate coordinate system.

[0056] It is understandable that the coordinate systems of the two are transformed by the yaw angle. Related: The 2D rotation matrix from the visual inertial navigation system coordinate system to the visual marker navigation coordinate system:

[0057] ,

[0058] If 3D expansion is required, you can Treating it as the heading angle (Z-Yaw), it degenerates into the above equation under the assumption of planar motion. The Jacobian of rotation with respect to angle and the error state treatment follow the standard derivation.

[0059] For example, the state vector (2D plane model, exemplarily a 9-dimensional vector) to be estimated by the extended Kalman filter is first defined:

[0060] ;

[0061] in, The accelerometer has zero bias in the visual inertial navigation system coordinate system. The velocity is in the coordinate system of the visual inertial navigation system. The position in the visual inertial navigation system coordinate system; For position drift in visual inertial navigation systems; This represents the yaw transformation angle from the visual inertial navigation system coordinate system to the visual marker navigation coordinate system. This example explicitly models this drift term to suppress drift in state prediction. By using explicit drift and yaw as states, consistency can be improved during observation gaps / mismatches.

[0062] For example, the differential equation of the above state vector is as follows:

[0063] ,

[0064] ,

[0065] ,

[0066] ,

[0067] ,

[0068] in, It is a velocity differential equation. Represents the velocity vector in the VINS local coordinate system (V-frame). The derivative with respect to time, i.e., acceleration; This represents the acceleration of motion of the carrier in the V-frame. This indicates that the accelerometer has zero bias in the V-frame. Indicates noise during the speed process;

[0069] It is a position differential equation. Represents the position vector in the V frame. The derivative with respect to time;

[0070] It is a zero partial differential equation. This indicates the rate of change of the accelerometer's zero bias. Indicates noise during the zero bias process;

[0071] It is a drift differential equation. Indicates VINS position drift term The rate of change of the drift is set to zero in the equation, meaning that in the continuous-time model, this will be... The drift is modeled as a constant. This does not mean the drift is absolutely constant, but rather that it is a slowly varying variable, its rate of change being much slower than that of dynamic processes (such as velocity and position). Therefore, its rate of change is set to zero at the differential equation level. Its actual change will be driven and corrected by the discretized process noise covariance and subsequent observation updates.

[0072] It is the differential equation of yaw angle. Indicates yaw angle The derivative with respect to time, i.e., the yaw rate, This indicates the noise during the yaw angle process.

[0073] It should be noted that, , , For zero-mean white noise, their covariances are respectively , , .

[0074] The process model composed of the above differential equations is the standard starting point for the Discrete EKF. The discretized system process model is obtained by discretizing the above differential equations using the first-order Euler method:

[0075] (2.1)

[0076] (2.2)

[0077] (2.3)

[0078] (2.4)

[0079] (2.5)

[0080] Wherein, the subscripts k and k+1 represent the current time and the next prediction time, respectively; Indicates the sampling period or prediction step size; Represents a two-dimensional rotation matrix; This represents the acceleration at time k in the global navigation coordinate system (A frame, such as NED). Represents the discrete process noise at time k; This represents the noise that drives the change in yaw angle.

[0081] In the discretized system process model, equation (2.1) is the velocity update equation, which represents the predicted velocity in the V coordinate system at the next time step (k+1); equation (2.2) is the position update equation, which represents the predicted position in the V coordinate system at the next time step (k+1); equation (2.3) is the zero bias update equation, which represents the predicted accelerometer zero bias at the next time step (k+1); equation (2.4) is the drift update equation, which represents the predicted VINS position drift at the next time step (k+1); and equation (2.5) is the yaw angle update equation, which represents the predicted yaw transformation angle between coordinate systems at the next time step (k+1).

[0082] In some embodiments, the yaw conversion angle initial value It is based on the position output by the visual inertial navigation system at the initialization time. Position observation data in visual marker navigation coordinate system It is obtained by calculating the difference in azimuth angle between the two in the horizontal plane.

[0083] In some embodiments, the position drift term of a visual inertial navigation system initial value It is based on the position output by the visual inertial navigation system at the initialization time. and its corresponding location observation data The difference is determined.

[0084] Specifically, during the initialization of the state vector to be estimated, the accelerometer bias in the V frame can be zeroed. and speed Initialized to zero; yaw conversion angle from V-series to A-series. Initialization: Take the first segment and observe simultaneously. ,Right now It is based on the position output by the visual inertial navigation system at the initialization time. Position observation data in visual marker navigation coordinate system The position drift term of the visual-inertial navigation system is obtained by calculating the difference in azimuth angles between the two in the horizontal plane. initial value It is based on the position output by the visual inertial navigation system at the initialization time. and its corresponding location observation data The difference is determined, that is: .

[0085] S2. Based on the acceleration data collected by the inertial measurement unit, the estimated state vector is predicted and updated to obtain the current predicted state vector and its error covariance.

[0086] For example, the system inputs include acceleration acquired by the inertial measurement unit (IMU) (converted to A-frame representation after gravity compensation / low-pass filtering): .

[0087] Step S2 can be based on the prediction steps of standard Kalman filtering, updating the state variables and error covariance matrix through prediction:

[0088] (3.1)

[0089] (3.2)

[0090] Equation (3.1) is the state vector prediction formula, which utilizes the corrected optimal state estimate from the previous time step (the posterior estimate at time k-1) and the dynamic model of the system (based on the state transition matrix). F (Description), to calculate the current time ( k Prior estimates of the state at time (time).

[0091] Equation (3.2) is the error covariance matrix prediction formula, which is used to predict the uncertainty (error) of the current state estimate. Wherein, This represents the error covariance matrix after correction and update at the previous time step, i.e., the output result of the filter at the previous time step, for the current time step ( k In terms of time, It is a known, specific numerical matrix; its initial time ( k =1) initial value It is set by engineers based on prior knowledge.

[0092] This step first updates the nine state variables defined in step S1, where the input is the motion acceleration in frame A. And the yaw conversion angle from V frame to A frame is used to obtain the acceleration in V frame: Then, substitute it into Equations 2.1 to 2.5 to predict and update each state variable, and obtain the current predicted state vector.

[0093] The system's state transition Jacobian matrix and process noise covariance matrix as follows:

[0094]

[0095]

[0096] Among them, in the state transition Jacobian matrix, "A" represents a 2x2 identity matrix located on the diagonal block, indicating that the state component is mainly affected by its previous value; "0" represents a zero matrix, which indicates that there is no direct linear relationship between the two state components corresponding to the row and column during the prediction process. and This indicates the negative impact of accelerometer zero bias on velocity and position prediction; This indicates the direct impact of velocity on position prediction; and This indicates that it quantifies how the yaw angle estimation error affects the prediction of speed and position.

[0097] In the process noise covariance matrix This indicates the intensity of the random walk at zero bias of the accelerometer; Indicates the noise intensity during the speed process; Indicates the noise intensity during the position process; This represents the random walk intensity of the VINS position drift term; This indicates the noise intensity during the yaw angle process.

[0098] Then, the above and Substitute these equations into equation (3.2) to complete the prediction update of the error covariance matrix.

[0099] S3. Upon receiving location observation data, use the location observation data to correct and update the currently predicted state vector and its error covariance.

[0100] In some embodiments, the position observation data includes first position observation data provided by the visual inertial navigation system in its own coordinate system and second position observation data provided by the visual marker navigation system in its own coordinate system.

[0101] In some embodiments, the current predicted state vector and its error covariance are corrected and updated using location observation data, including:

[0102] When first position observation data and second position observation data are received simultaneously within the same prediction period, the current predicted state vector and its error covariance are corrected and updated for the first time according to the preset first observation model, and the state vector after the first correction and update is corrected and updated for the second time according to the preset second observation model.

[0103] In some embodiments, the first observation model is represented as follows: the first position observation data is obtained by calculating the position in the visual inertial navigation system coordinate system, the visual inertial navigation system position drift term, and the position observation noise in the visual inertial navigation system coordinate system; it is exemplarily represented as follows: ,in, This indicates the observation data at the first location. This represents the position observation noise in the coordinate system of a visual-inertial navigation system.

[0104] The second observation model is represented as follows: the second position observation data is obtained by calculating the position in the visual inertial navigation system coordinate system and the position observation noise in the visual marker navigation coordinate system based on the two-dimensional rotation matrix constructed from the yaw transformation angle; it is exemplarily represented as follows: ,in, This indicates the observation data at the second location. Indicates the change angle from yaw. The constructed two-dimensional rotation matrix, This represents the noise in position observations within the visual marker navigation coordinate system.

[0105] It should be noted that step S3 will use other sensor data to correct the state vector X and error covariance matrix P updated in step S2, thereby obtaining a more accurate state estimate.

[0106] The system's observations may include: 1. VINS position observation data in the V system: 2. Observational data of AprilTag location in the A system: .in, The two-dimensional real number space is a set consisting of all ordered pairs of real numbers.

[0107] For any available observation received by the system, the corresponding state variables can be corrected according to the standard Kalman filter update steps:

[0108] (4.1)

[0109] (4.2)

[0110] (4.3)

[0111] (4.4)

[0112] (4.5)

[0113] in, In y It is the innovation, or observation residual, which is the formula representing the difference between the observation and the prediction. It calculates the actual sensor observations. z Compared with the predicted value based on the current state The "predicted observations" The difference between them.

[0114] Specifically, regarding VINS updates, z This is the position observation data directly output by VINS (first position observation data). , H It is a Jacobian matrix The predicted observations are Therefore, y measures the deviation between the actual output position of the VINS and the true position "as perceived" by the filter after drift correction.

[0115] Regarding AprilTag updates: z It is the absolute position calculated by AprilTag (second position observation data). , H It is a Jacobian matrix The predicted observations are Therefore, y measures the deviation between the absolute position provided by AprilTag and the position "perceived" by the filter after rotating its V-frame position to the A-frame.

[0116] In this context, S represents the innovation covariance, which is the uncertainty of the observation gap calculated in the previous step. y The reliability of the S matrix. A larger S matrix means that the difference between the observation and the prediction is more likely to be caused by noise rather than state error. The observation noise covariance matrix is ​​represented by the variables described below. and .

[0117] In K The Kalman gain, a formula representing the optimal corrected weights, is a matrix that determines the proportion by which the information should be weighted. y The correction amount is applied to the state prediction value superior.

[0118] It should be noted that the Kalman gainK Automatic adjustment of "step-by-step update" and "adaptive calibration" is achieved. For example, when the AprilTag observation quality is high, K It will assign a larger weight to correct the yaw angle. and location To achieve coordinate system calibration; when VINS observations are continuous but have drift, K It will intelligently transmit new information y Part of the explanation is positional drift. The change is partly explained by position. The changes in [the data] enable online estimation and compensation of drift.

[0119] This is the state vector update (correction) formula, which represents the formula for predicting the state. Add the optimal weight K Scaled-up news y To obtain the optimal posterior state estimate at the current time. .

[0120] This is the formula for updating (correcting) the error covariance matrix. It represents the updated system's confidence in the state estimate (a reduction in uncertainty), meaning that the uncertainty of the state estimate should decrease after correction due to the new information brought by the observations. In the formula... Meaning: gain K The larger the value (i.e., the more confident the observation), the greater the uncertainty of the state. P The more it is cut, the more it will be cut.

[0121] For example, since the data frequency of VINS and AprilTag is generally lower than the data acquisition frequency of IMU, step S3 can be executed only when VINS or AprilTag data is refreshed.

[0122] Because the coordinate systems of the two location observation data are different, their observation Jacobian matrices are also different. For example, the observation models and their corresponding observation Jacobian matrices for VINS location observation data (first location observation data) and AprilTag location observation data (second location observation data) are shown below:

[0123] 1. VINS location observation data (first observation model):

[0124]

[0125] This location observation model represents the location observation data acquired by the VINS sensor in the V-frame. It should be equal to the actual position of the carrier in the V system. Subtract a systematic positional drift In addition, there is random observation noise. ;in, express It follows a normal distribution. The 0 in the text indicates that the long-term average of the noise is zero. In It is the observation noise covariance matrix, which fully describes the intensity of the noise and the correlation between its components.

[0126] The corresponding Jacobian for the first position observation is:

[0127]

[0128] The state order corresponding to this matrix is: , which means The VINS observations were quantified. For the state vector X The sensitivity to changes in each component. It is an identity matrix, representing VINS observations. For the actual location The changes are extremely sensitive; It is a negative identity matrix, representing VINS observations. Position drift Equally sensitive, but changing in the opposite direction. The zero vector represents the yaw angle between the VINS position observation data (in the V-frame) and the coordinate system. It's irrelevant because the VINS output is defined in its own local frame and does not involve rotation with the global frame.

[0129] 2. AprilTag location observation data (second observation model):

[0130]

[0131] This location observation model represents the absolute position of the AprilTag system calculated in the global coordinate system (A-frame). It should be equal to the actual position of the carrier in the V system. After a yaw angle conversion Determined rotation matrix The values ​​after transformation to the A-system, plus random observation noise. .

[0132] The corresponding observation Jacobian for the second position observation data is:

[0133]

[0134] The state order corresponding to this matrix is: ,in yes right The derivative of; the matrix represents The AprilTag observations were quantified. For the state vector X The sensitivity to changes in each component. Represents AprilTag observations For V-series positions It is sensitive to changes, but its sensitivity is affected by the current heading estimate. Modulation, The direction of change needs to be The rotation is what is reflected in the observations of the A system. superior; This is the mathematical core of adaptive calibration; this term represents the yaw angle. Errors will directly affect the predicted A-series positions An error has occurred, and the size of the error is related to the current position. Proportional (the farther from the center of rotation, the greater the position deviation caused by the heading error); the zero matrix represents the drift between the AprilTag observation (in the A-frame) and the VINS. There is no direct relationship because drift is an error within the V-series, while AprilTag provides an absolute reference for the A-series.

[0135] It should be noted that step S3 can be summarized as follows: after the VINS location observation data is refreshed, the VINS location observation data is... Observation of the Jacobian matrix and the observation noise covariance matrix Substituting these values ​​into equations (4.1) to (4.5) completes the correction of the state variables and the error covariance matrix; similarly, after the AprilTag location observation data is refreshed, the AprilTag location observation data... Observation of the Jacobian matrix and the observation noise covariance matrix Substituting these into equations (4.1) to (4.5) completes the correction of the state variables and the error covariance matrix.

[0136] It should be noted that if two position observation data (first position observation data and second position observation data) exist simultaneously in the same EKF prediction period, the correction is performed in the order of VINS first, then AprilTag. This is because VINS is available most of the time (high frequency, continuous), and can be used first for correction to stabilize short-term motion; while AprilTag, as a low-frequency / intermittent "absolute position" calibration source, is corrected and updated later, thus allowing for "calibration" within the same period. and .

[0137] If observation data for a certain prediction period is missing (usually referring to missing AprilTag observation data, because VINS data is high-frequency and continuous, while AprilTag may not be detected in some periods due to occlusion, distance, etc.), the corresponding correction steps can be omitted, thus not compromising the consistency of filtering.

[0138] S4. Determine the fused navigation and positioning information based on the currently predicted state vector or the corrected and updated state vector.

[0139] In some embodiments, step S4 may include:

[0140] Based on the position in the visual-inertial navigation system coordinate system in the corrected and updated state vector. and yaw conversion angle Determine the fusion position in the visual marker navigation coordinate system As a fusion of navigation and positioning information, its transformation relationship is as follows:

[0141] ,

[0142] in, Indicates the change angle from yaw. Construct a two-dimensional rotation matrix.

[0143] It is understandable that the above conversion relationship can be used to convert the optimal state estimate obtained after correction and update by EKF prediction and position observation data into navigation position information with global consistency that can be directly used by users or upper-level control systems.

[0144] Specifically, first, after completing the EKF sequence measurement update at the current time, the corrected posterior state vector estimate is obtained. And extract two core state components from them:

[0145] 1. Position estimation in the local coordinate system of a visual inertial navigation system (VINS): This estimate has been updated through observations, effectively suppressing the inherent cumulative drift of VINS.

[0146] 2. Yaw transformation angle estimation from VINS coordinate system to visual marker navigation coordinate system (A-frame, approximate NED coordinate system is used in this embodiment). This angle value has been adaptively calibrated online using absolute position observation data such as AprilTag.

[0147] Subsequently, based on the yaw conversion angle Construct the corresponding two-dimensional rotation matrix This matrix rotates a two-dimensional vector in frame V to frame A, and its specific form is:

[0148] ;

[0149] In practical embedded deployments, to reduce real-time computation, lookup tables or hardware accelerators can be used to pre-calculate or quickly retrieve the data. and The value of .

[0150] Next, coordinate system transformation calculations are performed. The position vector in the V frame is... Left multiplication by rotation matrix This allows us to obtain the fused position in the global visual marker navigation coordinate system (A-frame). :

[0151] .

[0152] It should be noted that, by explicitly introducing the VINS position drift term and coordinate system yaw transformation angle into the EKF state vector and estimating them online, this embodiment of the application can model and compensate for the cumulative error of VINS and adaptively calibrate the transformation relationship between the two coordinate systems. This effectively suppresses positioning drift under long-term operation and reduces the dependence on the accuracy of fixed external parameter calibration, thereby improving the accuracy and robustness of fusion positioning.

[0153] Compared with the prior art, the embodiments of this application have the following beneficial effects:

[0154] 1. By explicitly modeling in the EKF state vector and estimating the VINS position drift term online ( This allows the system to dynamically identify, separate, and absorb the inherent, time-varying cumulative error in the VINS output into an independent state variable, thereby achieving proactive compensation for long-term VINS drift at the algorithmic level. This enables the fused positioning system to maintain the stability of its position estimation even under the challenge of the absolute observation source (AprilTag) being invisible for extended periods, effectively suppressing the divergence of positioning errors caused by long-term operation.

[0155] 2. By transforming the yaw angle from the VINS local coordinate system to the global navigation coordinate system ( This is used as a key state variable for online estimation, and the absolute position observation data provided by AprilTag is used for continuous correction, which can dynamically calibrate the rotational transformation relationship between the two coordinate systems. This avoids the systematic error transmission caused by installation deviation, calibration error or carrier deformation in the traditional solution that uses fixed external parameters, reduces the dependence on manual precision calibration, and can still ensure the uniformity and accuracy of global coordinates in complex usage scenarios.

[0156] 3. By adopting a sequential step-by-step update strategy of VINS first, followed by AprilTag, this approach prioritizes stabilizing short-term motion states using high-frequency continuous VINS observations, followed by low-frequency but absolute AprilTag observations for calibration, even in real-world scenarios where AprilTags are intermittently visible, observations are asynchronous, or data quality is unstable. This strategy significantly reduces the transient impact caused by single observation anomalies or mismatched timestamp filters, improving system stability and reliability in visually degraded environments (such as weak textures, motion blur, and occlusion).

[0157] Please refer to Figure 2 , Figure 2 A block diagram illustrating the composition of a fusion positioning device provided in some embodiments of this application is shown. It should be understood that this fusion positioning device is similar to the one described above. Figure 1 Corresponding to the method embodiments, it is able to perform each step involved in the above method embodiments. The specific functions of the fusion positioning device can be found in the description above. To avoid repetition, detailed descriptions are appropriately omitted here.

[0158] Figure 2 The fusion positioning device includes at least one software functional module that can be stored in memory or embedded in the fusion positioning device in the form of software or firmware. The fusion positioning device is applied to an extended Kalman filter and includes:

[0159] Initialization module 210 is used to construct and initialize the state vector to be estimated; wherein, the state vector includes the position in the visual inertial navigation system coordinate system. Visual inertial navigation system position drift item And the yaw transformation angle from the visual inertial navigation system coordinate system to the visual marker navigation coordinate system. ;

[0160] The prediction update module 220 is used to predict and update the state vector to be estimated based on the acceleration data collected by the inertial measurement unit, so as to obtain the current predicted state vector and its error covariance.

[0161] The state correction module 230 is used to correct and update the currently predicted state vector and its error covariance using the received position observation data.

[0162] The fusion positioning module 240 is used to determine fusion navigation and positioning information based on the currently predicted state vector or the corrected and updated state vector.

[0163] It is understood that the above-described device embodiments correspond to the method embodiments of the present invention. The fusion positioning device provided by the embodiments of the present invention can implement the fusion positioning method provided by any one of the method embodiments of the present invention.

[0164] Those skilled in the art will understand that, for the sake of convenience and brevity, the specific working process of the device described above can be referred to the corresponding process in the aforementioned method, and will not be elaborated further here.

[0165] like Figure 3 As shown, some embodiments of this application provide an aircraft 300, which includes a memory 310, a processor 320, and a computer program stored in the memory 310 and executable on the processor 320. When the processor 320 reads the program from the memory 310 via a bus 330 and executes the program, it can implement the method of any embodiment of the above-described fusion positioning method.

[0166] Processor 320 can process digital signals and may include various computing architectures. For example, it may be a complex instruction set computer architecture, a reduced instruction set computer architecture, or an architecture that implements multiple instruction set combinations. In some examples, processor 320 may be a microprocessor.

[0167] The memory 310 can be used to store instructions executed by the processor 320 or data related to the execution of instructions. These instructions and / or data may include code used to implement some or all of the functions of one or more modules described in the embodiments of this application. The processor 320 of this disclosure embodiment can be used to execute the instructions in the memory 310 to implement the methods shown above. The memory 310 includes dynamic random access memory, static random access memory, flash memory, optical memory, or other memories well known to those skilled in the art.

[0168] Some embodiments of this application also provide a computer-readable storage medium storing a computer program that, when executed by a processor, describes the method described in the method embodiments.

[0169] Some embodiments of this application also provide a computer program product that, when run on a computer, causes the computer to perform the methods described in the method embodiments.

[0170] It should be noted that the various embodiments in this specification are described in a progressive manner, with each embodiment focusing on the differences from other embodiments. Similar or identical parts between embodiments can be referred to interchangeably. For apparatus embodiments, since they are basically similar to method embodiments, the description is relatively simple; relevant parts can be referred to the descriptions in the method embodiments.

[0171] It should be understood, in the several embodiments provided in this application, that the disclosed apparatus and methods can also be implemented in other ways. The apparatus embodiments described above are merely illustrative; for example, the flowcharts and block diagrams in the accompanying drawings illustrate the architecture, functionality, and operation of possible implementations of apparatus, methods, and computer program products according to various embodiments of this application. In this regard, each block in a flowchart or block diagram may represent a module, segment, or portion of code containing one or more executable instructions for implementing a specified logical function. It should also be noted that in some alternative implementations, the functions marked in the blocks may occur in a different order than those marked in the drawings. For example, two consecutive blocks may actually be executed substantially in parallel, and they may sometimes be executed in reverse order, depending on the functions involved. It should also be noted that each block in a block diagram and / or flowchart, and combinations of blocks in block diagrams and / or flowcharts, can be implemented using a dedicated hardware-based system that performs the specified function or action, or using a combination of dedicated hardware and computer instructions.

[0172] In addition, the functional modules in the various embodiments of this application can be integrated together to form an independent part, or each module can exist independently, or two or more modules can be integrated to form an independent part.

[0173] If the aforementioned functions are implemented as software functional modules and sold or used as independent products, they can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of this application, in essence, or the part that contributes to the prior art, or a portion of the technical solution, can be embodied in the form of a software product. This computer software product is stored in a storage medium and includes several instructions to cause a computer device (which may be a personal computer, server, or network device, etc.) to execute all or part of the steps of the methods described in the various embodiments of this application. The aforementioned storage medium includes various media capable of storing program code, such as USB flash drives, portable hard drives, read-only memory (ROM), random access memory (RAM), magnetic disks, or optical disks.

[0174] The above description is merely an embodiment of this application and is not intended to limit the scope of protection of this application. Various modifications and variations can be made to this application by those skilled in the art. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of this application should be included within the scope of protection of this application. It should be noted that similar reference numerals and letters in the following figures indicate similar items; therefore, once an item is defined in one figure, it does not need to be further defined and explained in subsequent figures.

[0175] The above description is merely a specific embodiment of this application, but the scope of protection of this application is not limited thereto. Any variations or substitutions that can be easily conceived by those skilled in the art within the technical scope disclosed in this application should be included within the scope of protection of this application. Therefore, the scope of protection of this application should be determined by the scope of the claims.

[0176] It should be noted that, in this document, relational terms such as "first" and "second" are used only to distinguish one entity or operation from another, and do not necessarily require or imply any such actual relationship or order between these entities or operations. Furthermore, the terms "comprising," "including," or any other variations thereof are intended to cover non-exclusive inclusion, such that a process, method, article, or apparatus that comprises a list of elements includes not only those elements but also other elements not expressly listed, or elements inherent to such a process, method, article, or apparatus. Without further limitations, an element defined by the phrase "comprising one..." does not exclude the presence of other identical elements in the process, method, article, or apparatus that includes said element.

Claims

1. A fusion positioning method, characterized in that, Applied to extended Kalman filters, including: Construct and initialize the state vector to be estimated; wherein, the state vector includes the position in the visual inertial navigation system coordinate system, the visual inertial navigation system position drift term, and the yaw transformation angle from the visual inertial navigation system coordinate system to the visual marker navigation coordinate system; the initial value of the yaw transformation angle is obtained by calculating the difference between the position output by the visual inertial navigation system at the initialization time and the position observation data in the visual marker navigation coordinate system in the horizontal plane; the initial value of the visual inertial navigation system position drift term is determined based on the difference between the position output by the visual inertial navigation system at the initialization time and its corresponding position observation data; The state vector to be estimated is predicted and updated based on the acceleration data collected by the inertial measurement unit, so as to obtain the current predicted state vector and its error covariance. Upon receiving location observation data, the current predicted state vector and its error covariance are corrected and updated using the location observation data. The fused navigation and positioning information is determined based on the currently predicted state vector or the corrected and updated state vector.

2. The fusion positioning method according to claim 1, characterized in that, The position observation data includes first position observation data provided by the visual inertial navigation system in its own coordinate system and second position observation data provided by the visual marker navigation system in its own coordinate system.

3. The fusion positioning method according to claim 2, characterized in that, The step of correcting and updating the currently predicted state vector and its error covariance using the location observation data includes: When the first position observation data and the second position observation data are received simultaneously within the same prediction period, the current predicted state vector and its error covariance are corrected and updated for the first time according to the preset first observation model, and the state vector after the first correction and update is corrected and updated for the second time according to the preset second observation model.

4. The fusion positioning method according to claim 3, characterized in that, The first observation model is expressed as follows: the first position observation data is obtained by calculating the position in the visual inertial navigation system coordinate system, the position drift term of the visual inertial navigation system, and the position observation noise in the visual inertial navigation system coordinate system. The second observation model is expressed as follows: the second position observation data is obtained by calculation based on the two-dimensional rotation matrix constructed by the yaw conversion angle, the position in the visual inertial navigation system coordinate system, and the position observation noise in the visual marker navigation coordinate system.

5. The fusion positioning method according to claim 1, characterized in that, The state vector also includes velocity and accelerometer zero bias in the visual inertial navigation system coordinate system.

6. The fusion positioning method according to any one of claims 1 to 5, characterized in that, The process of determining the fused navigation and positioning information based on the currently predicted state vector or the corrected and updated state vector includes: The fused position in the visual marker navigation coordinate system is determined based on the position in the visual inertial navigation system coordinate system and the yaw transformation angle in the corrected and updated state vector, and is used as the fused navigation positioning information; wherein, the fused position in the visual marker navigation coordinate system is obtained by calculation based on the two-dimensional rotation matrix constructed by the yaw transformation angle and the position in the visual inertial navigation system coordinate system.

7. A fusion positioning device, characterized in that, Applications to extended Kalman filters include: An initialization module is used to construct and initialize the state vector to be estimated. The state vector includes the position in the visual-inertial navigation system (VIS) coordinate system, the VIS position drift term, and the yaw transformation angle from the VIS coordinate system to the visual marker navigation coordinate system. The initial value of the yaw transformation angle is obtained by calculating the difference in azimuth angle in the horizontal plane between the position output by the VIS at the initialization time and the position observation data in the visual marker navigation coordinate system. The initial value of the VIS position drift term is determined based on the difference between the position output by the VIS at the initialization time and its corresponding position observation data. The prediction update module is used to predict and update the state vector to be estimated based on the acceleration data collected by the inertial measurement unit, so as to obtain the current predicted state vector and its error covariance. The state correction module is used to correct and update the currently predicted state vector and its error covariance using the received position observation data. The fusion positioning module is used to determine fusion navigation and positioning information based on the currently predicted state vector or the corrected and updated state vector.

8. An aircraft, characterized in that, It includes a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor, when executing the program, can implement the fusion positioning method according to any one of claims 1-6.

9. A computer-readable storage medium, characterized in that, The computer-readable storage medium stores a computer program, which, when executed by a processor, performs the fusion positioning method as described in any one of claims 1-6.

10. A computer program product, characterized in that, The computer program product includes a computer program that, when executed by a processor, implements the fusion positioning method according to any one of claims 1-6.

Images