A multi-sensor fusion alignment and collision avoidance protection linkage method for high dynamic maneuvering aircraft

Abstract

The application discloses a multi-sensor fusion alignment and collision avoidance protection linkage method for high dynamic maneuvering aircraft, comprising the following steps: acquiring IMU sampling data, radar and visual observation data and time stamps of the aircraft; based on a pre-constructed state variable definition and error propagation basic model, completing IMU sampling data bias correction and state deduction, solving state and covariance prior; combining the right invariant error dynamics to linearize covariance prior propagation, obtaining posterior state and covariance; calculating observation residual and constructing an adjoint matrix, realizing data time sequence alignment, and then completing state updating through extended Kalman filtering; extracting the position component of the updated covariance, solving the maximum eigenvalue square root and normalizing mapping, combining a double-threshold hysteresis mechanism to output a collision avoidance warning, and simultaneously issuing a linkage degradation instruction to a protection mechanism; the application improves filtering consistency and convergence, avoids frequent false triggering, and improves flight safety.

Description

Technical Field

[0001] This invention relates to a multi-sensor fusion alignment and collision avoidance protection linkage method for highly dynamic maneuvering aircraft, belonging to the field of avionics and detection and collision avoidance technology. Background Technology

[0002] In high-speed, high-tilt-rate maneuvering flight scenarios, aircraft state estimation and collision avoidance face a series of severe challenges. Traditional multi-sensor fusion and collision avoidance methods are insufficient to simultaneously meet the requirements for state estimation accuracy, stability, and safety response under highly dynamic conditions.

[0003] Existing methods often parameterize attitude, velocity, and position in Euclidean space using Euler angles, quaternions, or rotation vectors, and then perform state estimation using the Extended Kalman Filter (EKF). However, under large-angle, high-angular-velocity maneuvers, Euler angles exhibit gimbal-locked singularities, and the linearization of the standard EKF on the manifold cannot guarantee kinematic consistency. Furthermore, modeling IMU biases typically employs simple random walks or additive methods, neglecting the geometric coupling between the bias and the rotation group structure, leading to decreased bias estimation accuracy and state estimation divergence during severe maneuvers. The IMU, millimeter-wave radar, and visual camera have different sampling frequencies, and the observation time is asynchronous with the filter update time. Traditional methods use linear interpolation or time extrapolation based on the uniform velocity assumption to force asynchronous observations to the same moment. However, under high angular velocity variations, linear extrapolation disrupts the rigidity of coordinate transformations, resulting in significant projection errors in the observation residuals in the tangent space, causing the Kalman filter update to deviate from the true state. Summary of the Invention

[0004] The technical problem to be solved by this invention is the problem of easy divergence in state estimation and large spatiotemporal alignment error in the prior art.

[0005] To solve the above-mentioned technical problems, the present invention is implemented using the following technical solution.

[0006] On one hand, the present invention provides a multi-sensor fusion alignment and collision avoidance protection linkage method for highly dynamic maneuvering aircraft, comprising:

[0007] Acquire IMU sampling data, radar and visual observation data, and timestamps from the aircraft;

[0008] Based on the pre-built state variable definition and error propagation basic model, the IMU sampled data is biased and corrected, the state evolution is performed on the biased and corrected IMU sampled data, and the predicted state and covariance prior are calculated.

[0009] Based on the predicted state, a right-invariant error is defined, and the covariance prior is linearized and propagated based on the right-invariant error dynamics to obtain the posterior state and posterior covariance.

[0010] Based on the radar and visual observation data, timestamps and the predicted state, the observation residuals are calculated and an adjoint matrix is ​​constructed. The observation residuals are aligned to the unified filtering time through the adjoint matrix. At the unified filtering time, the extended Kalman filter is performed to update the posterior state and posterior covariance.

[0011] Extract the location covariance component from the updated posterior covariance, calculate the square root of the largest eigenvalue of the location covariance component, normalize the square root of the largest eigenvalue and map it, combine the mapping result with dual threshold hysteresis management, a pre-set conflict risk intensity index and risk intensity duration to generate an alarm confirmation output, and simultaneously output a linkage degradation flag to the execution protection agency.

[0012] Linearization propagation through right-invariant error dynamics avoids the dependence of traditional error linearization on state estimation, improves filtering consistency and convergence, and uses the adjoint matrix to unify asynchronous observation data such as radar and vision to the same filtering time. The position covariance is mapped to the safe distance and confirmation time. Combined with dual-threshold hysteresis management, alarm and linkage degradation signals are generated to avoid frequent false triggers and improve flight safety.

[0013] The IMU sampling data includes: triaxial angular velocity and triaxial acceleration collected by the airborne inertial measurement unit;

[0014] The radar and visual observation data include:

[0015] Radar observation data, including target point cloud data acquired by radar;

[0016] Visual observation data, including the target bounding box coordinates acquired by the camera module;

[0017] The timestamp includes the system time at which the radar observation data and visual observation data were actually collected.

[0018] The steps for defining the state variables and constructing the basic model for error propagation include:

[0019] An extended Lie group is used to construct the state matrix, which includes the rotation matrix, velocity vector, and position vector, as shown in the following formula:

[0020] ,

[0021] in, The state matrix, For rotation matrix, , It is a special orthogonal group in three dimensions. For velocity vector, , For three-dimensional Euclidean space For position vectors, , To expand Lie groups;

[0022] Gyroscope bias and accelerometer bias The bias is processed using a semi-direct product method, including: biasing the gyroscope. Exponentially mapped embedded rotation group The accelerometer bias Added in an additive form;

[0023] The state variables are defined as 15-dimensional state variables, including 9-dimensional nominal states and 6-dimensional bias error states. The nominal states include a three-dimensional rotation matrix, a three-dimensional velocity vector, and a three-dimensional position vector. The bias error states include a three-dimensional gyroscope bias and a three-dimensional accelerometer bias.

[0024] Based on the state matrix, the gyroscope is biased. and accelerometer bias The basic model of error propagation is jointly constructed by using the semi-direct product form as the extended error term.

[0025] By embedding the gyroscope bias index mapping into the rotation group and adding additive bias to the accelerometer bias, the nominal state and bias error are separated, which facilitates filtering and updating.

[0026] It also includes: if the location covariance component is unavailable or its value is abnormal, switch to a conservative parameter pair and output a linkage degradation flag; otherwise, according to the monotonic constraint calibration, map the mapping parameters after normalization of the square root of the largest eigenvalue, record the mapping parameters, covariance prior, alarm confirmation output and linkage degradation flag corresponding to each linkage decision, and form a traceable evidence package.

[0027] When the location covariance is unavailable or its value is abnormal, switch to conservative parameters and output a degradation flag to prevent protection failure due to data failure; record the parameters, covariance, alarm and degradation flag of each linkage decision to form an evidence package for easy post-event analysis and system authentication.

[0028] The steps of bias correction of the IMU sampled data, performing state evolution on the bias-corrected IMU sampled data, and calculating the predicted state and covariance prior include:

[0029] The bias correction for triaxial angular velocity and triaxial acceleration is as follows:

[0030] ,

[0031] ,

[0032] in, The triaxial angular velocities after bias correction. The gyroscope is used to measure the angular velocity of its three axes. For gyroscope bias The triaxial acceleration after bias correction. For the original measurement of triaxial acceleration by the accelerometer, For accelerometer bias;

[0033] The triaxial angular velocity after bias correction and triaxial acceleration The formula for converting to a Lie algebra vector is as follows:

[0034] ,

[0035] in, For Lie algebra vectors, This is the vector transpose of the triaxial angular velocities after bias correction. This is the vector transpose of the triaxial accelerations after bias correction. The transpose of the aircraft's velocity vector. To expand Lie groups The corresponding Lie algebra space;

[0036] The Lie algebra vector is converted using the hat mapping. The matrix undergoes state evolution according to the following formula:

[0037] ,

[0038] in, for The updated state matrix at each time step. for Extended Lie Groups of Time State matrix, For the Lie group exponential mapping operator, The IMU sampling time interval This is the Lie algebra vector transformed by the hat mapping;

[0039] The formula for calculating the predicted state is as follows:

[0040] ,

[0041] in, To predict the state, for The updated state matrix at each time step;

[0042] Based on the aforementioned error propagation model, the covariance prior is calculated using the following formula:

[0043] ,

[0044] in, Let covariance be the prior matrix. The right-invariant error state transition Jacobian matrix is... for The covariance posterior matrix at time t. This is the transpose of the right-invariant error state transition Jacobian matrix. Inject matrix for process noise. The diagonal matrix of IMU noise covariance. Transpose of the process noise injection matrix.

[0045] The original measurements are corrected by using online estimated gyroscope and accelerometer biases to improve the accuracy of the predicted state. Angular velocity, acceleration, and velocity are combined into a Lie algebra vector, and discrete-time state evolution is realized through exponential mapping, which preserves the manifold structure and is more accurate than Euler integral.

[0046] The steps of defining a right-invariant error based on the predicted state, performing linearization propagation of the covariance prior based on right-invariant error dynamics, and obtaining the posterior state and posterior covariance include:

[0047] The right-invariant error is defined by the following formula:

[0048] ,

[0049] in, The error is right-invariant. The inverse of the true state value. Estimate the current state;

[0050] Linearizing the error dynamics at the manifold identity element yields the state transition Jacobian matrix.

[0051] Based on the state transition Jacobian matrix, covariance propagation is performed on the covariance prior to obtain the posterior state and posterior covariance.

[0052] The covariance propagation formula is as follows:

[0053] ,

[0054] in, Let covariance be the prior matrix. The right-invariant error state transition Jacobian matrix is... for The covariance posterior matrix at time t. This is the transpose of the right-invariant error state transition Jacobian matrix. Inject matrix for process noise. The diagonal matrix of IMU noise covariance. Transpose of the process noise injection matrix.

[0055] The steps include: calculating the observation residual and constructing the adjoint matrix based on the radar and visual observation data, timestamps, and the predicted state; aligning the observation residual to the unified filtering time using the adjoint matrix; and performing extended Kalman filtering updates on the posterior state and posterior covariance at the unified filtering time.

[0056] The time difference of the timestamps is calculated using the following formula:

[0057] ,

[0058] in, Due to time difference, For the current step time, The original timestamp of the observation data;

[0059] The predicted state is mapped to the observation space using an observation function to obtain the predicted observation value. The deviation between the radar and visual observation data and the predicted observation value is calculated to obtain the observation residual.

[0060] The manifold evolution increment from the observation time to the filter step time is calculated based on the aforementioned time difference, using the following formula:

[0061] ,

[0062] in, For manifold evolution increments, Due to time difference, For the Lie group exponential mapping operator, This is the Lie algebra vector transformed by the hat mapping;

[0063] The extended Lie group adjoint matrix is ​​calculated based on the manifold evolution increment, and the observation residual is shifted from the tangent space at the observation time to the tangent space at the filter step time.

[0064] An extended Kalman filter is performed to update the posterior state and posterior covariance under a uniform time scale.

[0065] The time difference between radar and visual observation times and filter step times is calculated. An adjoint matrix is ​​constructed through manifold evolution increments. The observation residuals are spatially shifted from the observation time to the filter time to achieve updates under a unified time scale.

[0066] The extended Kalman filter update includes:

[0067] The Kalman gain is calculated using the following formula:

[0068] ,

[0069] in, Here is the Kalman gain matrix. The posterior covariance matrix is... Let be the transpose of the Jacobian matrix of the observation model. To observe the noise covariance matrix;

[0070] The posterior state is updated using the following formula:

[0071] ,

[0072] in, for Posterior state estimation after time-lapse updates for Prior state estimation at time 10:00 For the Lie group exponential mapping operator, Here is the Kalman gain matrix. The observation residuals after time alignment compensation;

[0073] The updated posterior covariance is calculated using the following formula:

[0074] ,

[0075] in, for The posterior covariance matrix updated at each time step. It is the identity matrix. Let be the Jacobian matrix of the observation model.

[0076] The step of calculating the square root of the largest eigenvalue of the location covariance component is as follows:

[0077] ,

[0078] in, The square root of the largest eigenvalue. The operation is performed to find the largest eigenvalue of the position covariance component matrix. The position covariance component matrix;

[0079] The step of normalizing the square root of the largest eigenvalue is as follows:

[0080] ,

[0081] in, The square root of the largest eigenvalue after normalization. For the amplitude limiting function, The square root of the largest eigenvalue. For low uncertainty threshold, This represents a high uncertainty threshold.

[0082] In the step of normalizing the square root of the largest eigenvalue and then mapping, the mapping includes spatial channel mapping and temporal channel mapping, and the formula for spatial channel mapping is as follows:

[0083] ,

[0084] in, The adaptively adjusted safety protection distance Based on the safe distance, This is the adjustment coefficient for the spatial channel. The term is the power of the square root of the largest eigenvalue after normalization. The preset power adjustment coefficient, This is the lower limit of the safe distance. This is the upper limit of the safe distance;

[0085] The time channel mapping formula is as follows:

[0086] ,

[0087] in, The adaptively adjusted state confirmation time. Based on the confirmation time, This is the adjustment factor for the time channel. The term is the power of the square root of the largest eigenvalue after normalization. The preset power adjustment coefficient, To confirm the lower limit of the time, This is the upper limit for the confirmation time.

[0088] The step of generating an alarm confirmation output by combining the mapping results with dual-threshold hysteresis management, a pre-set conflict risk intensity index, and the duration of risk intensity includes:

[0089] The pre-set conflict risk intensity indicators include the entry enhanced protection threshold and the exit enhanced protection threshold;

[0090] The dual-threshold hysteresis management includes: entering the enhanced protection state only when the square root of the normalized maximum eigenvalue is not less than the threshold for entering enhanced protection and the cumulative time is not less than the duration of the risk intensity; and exiting the enhanced protection state only when the square root of the normalized maximum eigenvalue is not greater than the threshold for exiting enhanced protection and the cumulative time is not less than the duration of the risk intensity.

[0091] The conditions for alarm confirmation are as follows:

[0092] ,

[0093] in, This is an alarm confirmation flag. This is a collision risk indicator calculated in real time. Based on the adaptively adjusted safety protection distance Dynamically adjusted alarm thresholds The adaptively adjusted safety protection distance The duration of the current alarm status. This is the adaptively adjusted state confirmation time.

[0094] Different entry and exit thresholds are used, and the duration is required to be no less than the duration of the risk intensity, so as to avoid frequent switching of protection status due to noise or instantaneous fluctuations.

[0095] Compared with the prior art, the beneficial effects achieved by the present invention are as follows:

[0096] This invention improves the accuracy and consistency of state estimation in high-dynamic scenarios by biasing IMU sampled data and linearizing the propagation of state and covariance based on right-invariant error dynamics. It utilizes the adjoint matrix to shift the residuals of asynchronous observation data such as radar and vision from the original time to the unified filtering time, realizing tightly coupled real-time filtering updates. It extracts the position covariance component from the posterior covariance and calculates its largest eigenvalue square root. After normalization mapping, it adaptively adjusts the safety protection distance and state confirmation time. Combined with dual-threshold hysteresis management and risk intensity duration, it generates alarm confirmation output and linkage degradation flags, thereby reliably triggering the execution protection mechanism while avoiding frequent false triggers. This significantly improves the active collision avoidance capability and flight safety of the aircraft in uncertain environments. Attached Figure Description

[0097] Figure 1 This is a flowchart illustrating the multi-sensor fusion alignment and collision avoidance protection linkage method shown in Embodiment 1 of the present invention;

[0098] Figure 2 This is a flowchart of the dual-channel mapping and criterion process shown in Embodiment 1 of the present invention. Detailed Implementation

[0099] The technical solution of the present invention will be described in detail below with reference to the accompanying drawings and specific embodiments. It should be understood that the embodiments of the present invention and the specific features in the embodiments are detailed descriptions of the technical solution of the present invention, rather than limitations thereof. In the absence of conflict, the embodiments of the present invention and the technical features in the embodiments can be combined with each other.

[0100] The term "and / or" simply describes the relationship between related objects, indicating that three relationships can exist. For example, A and / or B can represent: A alone, A and B simultaneously, or B alone. Additionally, the character " / " generally indicates that the preceding and following related objects have an "or" relationship.

[0101] Example 1

[0102] like Figure 1 As shown in the figure, this embodiment introduces a multi-sensor fusion alignment and collision avoidance protection linkage method for highly dynamic maneuvering aircraft, including:

[0103] Acquire IMU sampling data, radar and visual observation data, and timestamps from the aircraft;

[0104] Based on the pre-built state variable definition and error propagation basic model, bias correction is performed on the IMU sampled data, state evolution is performed on the bias-corrected IMU sampled data, and the predicted state and covariance prior are calculated.

[0105] Based on the right-invariant error defined by the predicted state, the covariance prior is linearized and propagated based on the dynamics of the right-invariant error to obtain the posterior state and posterior covariance.

[0106] Based on radar and visual observation data, timestamps, and predicted states, the observation residuals are calculated and an adjoint matrix is ​​constructed. The observation residuals are aligned to the unified filtering time through the adjoint matrix. At the unified filtering time, the extended Kalman filter is performed to update the posterior state and posterior covariance.

[0107] Extract the location covariance component from the updated posterior covariance, calculate the square root of the largest eigenvalue of the location covariance component, normalize the square root of the largest eigenvalue and map it, combine the mapping result with dual threshold hysteresis management, pre-set conflict risk intensity index and risk intensity duration to generate alarm confirmation output, and simultaneously output linkage degradation flag to the execution protection agency.

[0108] The IMU sampling data includes: triaxial angular velocity and triaxial acceleration collected by the airborne inertial measurement unit;

[0109] Radar and visual observation data include:

[0110] Radar observation data, including target point cloud data acquired by radar;

[0111] Visual observation data, including the target bounding box coordinates acquired by the camera module;

[0112] The timestamp includes the system time at which radar and visual observation data were actually acquired.

[0113] Specifically, the sampling frequency of IMU data is 200-400Hz, the radar observation frequency is 15-20Hz, and the visual observation frequency is 30-60Hz.

[0114] The steps for defining state variables and constructing the basic error propagation model include:

[0115] An extended Lie group is used to construct the state matrix, which includes the rotation matrix, velocity vector, and position vector, as shown in the following formula:

[0116] ,

[0117] in, The state matrix, For rotation matrix, , It is a special orthogonal group in three dimensions. For velocity vector, , For three-dimensional Euclidean space For position vectors, , To expand Lie groups;

[0118] Gyroscope bias and accelerometer bias The bias is handled using a semi-direct product method, including: biasing the gyroscope. Exponentially mapped embedded rotation group accelerometer bias Added in an additive form;

[0119] The state variables are defined as 15-dimensional state variables, including 9-dimensional nominal states and 6-dimensional bias error states. The nominal states include a three-dimensional rotation matrix, a three-dimensional velocity vector, and a three-dimensional position vector. The bias error states include a three-dimensional gyroscope bias and a three-dimensional accelerometer bias.

[0120] Based on the state matrix, the gyroscope bias is... and accelerometer bias The basic model of error propagation is jointly constructed by using the semi-direct product form as the extended error term.

[0121] It also includes: if the location covariance component is unavailable or its value is abnormal, switch to conservative parameter pair and output linkage degradation flag; otherwise, according to the monotonic constraint calibration, map parameters are normalized to the square root of the largest eigenvalue and the mapping parameters, covariance prior, alarm confirmation output and linkage degradation flag corresponding to each linkage decision are recorded to form a traceable evidence package.

[0122] Specifically, according to the monotonicity constraint and Calibrate the mapping parameters. The sign of the partial derivative. The adaptively adjusted safety protection distance The adaptively adjusted state confirmation time. It is the square root of the largest eigenvalue.

[0123] The steps of bias correction on IMU sampled data, state evolution on the bias-corrected IMU sampled data, and calculation of the predicted state and covariance prior include:

[0124] The bias correction for triaxial angular velocity and triaxial acceleration is as follows:

[0125] ,

[0126] ,

[0127] in, The triaxial angular velocities after bias correction. The gyroscope is used to measure the angular velocity of its three axes. For gyroscope bias The triaxial acceleration after bias correction. For the original measurement of triaxial acceleration by the accelerometer, For accelerometer bias;

[0128] The triaxial angular velocity after bias correction and triaxial acceleration The formula for converting to a Lie algebra vector is as follows:

[0129] ,

[0130] in, For Lie algebra vectors, This is the vector transpose of the triaxial angular velocities after bias correction. This is the vector transpose of the triaxial accelerations after bias correction. The transpose of the aircraft's velocity vector. To expand Lie groups The corresponding Lie algebra space;

[0131] Transform the Lie algebra vector into a hat map. The matrix undergoes state evolution according to the following formula:

[0132] ,

[0133] in, for The updated state matrix at each time step. for Extended Lie Groups of Time State matrix, For the Lie group exponential mapping operator, The IMU sampling time interval This is the Lie algebra vector transformed by the hat mapping;

[0134] The formula for calculating the predicted state is as follows:

[0135] ,

[0136] in, To predict the state, for The updated state matrix at each time step;

[0137] Based on the basic error propagation model, the prior covariance is calculated using the following formula:

[0138] ,

[0139] in, Let covariance be the prior matrix. The right-invariant error state transition Jacobian matrix is... for The covariance posterior matrix at time t. This is the transpose of the right-invariant error state transition Jacobian matrix. Inject matrix for process noise. The diagonal matrix of IMU noise covariance. Transpose of the process noise injection matrix.

[0140] The steps for obtaining the posterior state and posterior covariance by linearizing the prior covariance based on the right-invariant error dynamics, according to the right-invariant error definition of the predicted state, include:

[0141] The right-invariant error is defined by the following formula:

[0142] ,

[0143] in, The error is right-invariant. The inverse of the true state value. Estimate the current state;

[0144] Linearizing the error dynamics at the manifold identity element yields the state transition Jacobian matrix.

[0145] Based on the state transition Jacobian matrix, covariance propagation is performed on the covariance prior to obtain the posterior state and posterior covariance.

[0146] The formula for covariance propagation is as follows:

[0147] ,

[0148] in, Let covariance be the prior matrix. The right-invariant error state transition Jacobian matrix is... for The covariance posterior matrix at time t. This is the transpose of the right-invariant error state transition Jacobian matrix. Inject matrix for process noise. The diagonal matrix of IMU noise covariance. Transpose of the process noise injection matrix.

[0149] Based on radar and visual observation data, timestamps, and predicted states, the observation residuals are calculated and an adjoint matrix is ​​constructed. The observation residuals are aligned to the unified filtering time using the adjoint matrix. At the unified filtering time, an extended Kalman filter is performed to update the posterior state and posterior covariance, including the following steps:

[0150] The time difference for each timestamp is calculated using the following formula:

[0151] ,

[0152] in, Due to time difference, For the current step time, The original timestamp of the observation data;

[0153] The predicted state is mapped to the observation space using an observation function to obtain the predicted observation value. The deviation between the radar and visual observation data and the predicted observation value is calculated to obtain the observation residual.

[0154] The manifold evolution increment from the observation time to the filter step time is calculated based on the time difference, as shown in the following formula:

[0155] ,

[0156] in, For manifold evolution increments, Due to time difference, For the Lie group exponential mapping operator, This is the Lie algebra vector transformed by the hat mapping;

[0157] The extended Lie group adjoint matrix is ​​calculated based on the manifold evolution increment, and the observation residuals are shifted from the tangent space at the observation time to the tangent space at the filter step time.

[0158] Extended Kalman filtering is performed to update the posterior state and posterior covariance under a uniform time scale.

[0159] Extended Kalman filter updates include:

[0160] The Kalman gain is calculated using the following formula:

[0161] ,

[0162] in, Here is the Kalman gain matrix. The posterior covariance matrix is... Let be the transpose of the Jacobian matrix of the observation model. To observe the noise covariance matrix;

[0163] The updated posterior state is calculated using the following formula:

[0164] ,

[0165] in, for Posterior state estimation after time-lapse updates for Prior state estimation at time 10:00 For the Lie group exponential mapping operator, Here is the Kalman gain matrix. The observation residuals after time alignment compensation;

[0166] The updated post-hoc covariance is calculated using the following formula:

[0167] ,

[0168] in, for The posterior covariance matrix updated at each time step. It is the identity matrix. Let be the Jacobian matrix of the observation model.

[0169] The steps for calculating the square root of the largest eigenvalue of the location covariance component are as follows:

[0170] ,

[0171] in, The square root of the largest eigenvalue. The operation is performed to find the largest eigenvalue of the position covariance component matrix. The location covariance component matrix;

[0172] The steps for normalizing the square root of the largest eigenvalue are as follows:

[0173] ,

[0174] in, The square root of the largest eigenvalue after normalization. For the amplitude limiting function, The square root of the largest eigenvalue. For low uncertainty threshold, This represents a high uncertainty threshold.

[0175] The mapping step after normalizing the square root of the largest eigenvalue includes spatial channel mapping and temporal channel mapping. The formula for spatial channel mapping is as follows:

[0176] ,

[0177] in, The adaptively adjusted safety protection distance Based on the safe distance, This is the adjustment coefficient for the spatial channel. The term is the power of the square root of the largest eigenvalue after normalization. The preset power adjustment coefficient, This is the lower limit of a safe distance. This is the upper limit of the safe distance;

[0178] The time channel mapping formula is as follows:

[0179] ,

[0180] in, The adaptively adjusted state confirmation time. Based on the confirmation time, This is the adjustment factor for the time channel. The term is the power of the square root of the largest eigenvalue after normalization. The preset power adjustment coefficient, To confirm the lower limit of the time, This is the upper limit for the confirmation time.

[0181] The steps for generating alarm confirmation output by combining the mapping results with dual-threshold hysteresis management, pre-defined conflict risk intensity indicators, and risk intensity duration include:

[0182] The pre-defined conflict risk intensity indicators include the entry threshold for enhanced protection and the exit threshold for enhanced protection;

[0183] The dual-threshold hysteresis management includes: entering the enhanced protection state only when the square root of the normalized maximum eigenvalue is not less than the threshold for entering enhanced protection and the cumulative time is not less than the duration of the risk intensity; and exiting the enhanced protection state only when the square root of the normalized maximum eigenvalue is not greater than the threshold for exiting enhanced protection and the cumulative time is not less than the duration of the risk intensity.

[0184] The conditions for alarm confirmation are:

[0185] ,

[0186] in, This is an alarm confirmation flag. This is a collision risk indicator calculated in real time. Based on the adaptively adjusted safety protection distance Dynamically adjusted alarm thresholds The adaptively adjusted safety protection distance The duration of the current alarm status. This is the adaptively adjusted state confirmation time.

[0187] Specifically, such as Figure 2 As shown, the location covariance component is extracted from the posterior covariance, and its largest eigenvalue square root is calculated as the main uncertainty index. After normalizing the largest eigenvalue square root, it is mapped to the spatial channel to obtain the dynamic protection distance and mapped to the time channel to obtain the dynamic confirmation window time. Combined with dual-threshold hysteresis management and conflict risk intensity index and its duration count, alarm confirmation output is generated, and linkage degradation flag is output at the same time.

[0188] Example 2

[0189] This embodiment introduces a multi-sensor fusion alignment and collision avoidance protection linkage method for highly dynamic maneuvering aircraft, which is deployed in the flight control computer of a tilt-configuration aircraft.

[0190] The flight control computer reads the initial values ​​from the IMU and integrated navigation system, and constructs an extended Lie group:

[0191] ,

[0192] in, The state matrix, Let be a rotation matrix. , It is a special orthogonal group in three dimensions. For velocity vectors, , For three-dimensional Euclidean space For position vectors, .

[0193] Simultaneously bias the gyroscope Through exponential mapping Embedded rotation group, accelerometer bias Added additively, forming a 15-dimensional extended state variable. This state space serves as the mathematical foundation for subsequent predictions and updates.

[0194] Perform bias correction on the raw IMU data: , ,in, The triaxial angular velocities after bias correction. The gyroscope is used to measure the angular velocity of its three axes. For gyroscope bias The triaxial acceleration after bias correction. For the original measurement of triaxial acceleration by the accelerometer, For accelerometer bias;

[0195] Constructing Lie algebra vectors ,in, For Lie algebra vectors, This is the vector transpose of the triaxial angular velocities after bias correction. This is the vector transpose of the triaxial accelerations after bias correction. This is the transpose of the aircraft's velocity vector;

[0196] And update the nominal state via exponential mapping: ,in, for The updated state matrix at each time step. for Extended Lie Groups of Time State matrix, For the Lie group exponential mapping operator, The IMU sampling time interval The Lie algebra vector is transformed by the hat mapping; simultaneously, the Jacobian is obtained by linearization based on the right-invariant error, and the covariance is propagated: ,in, Let covariance be the prior matrix. The right-invariant error state transition Jacobian matrix is... for The covariance posterior matrix at time t. This is the transpose of the right-invariant error state transition Jacobian matrix. Inject matrix for process noise. The diagonal matrix of IMU noise covariance. Transpose of the process noise injection matrix.

[0197] When radar or visual observation arrives, the system records its timestamp and calculates the time difference: ,in, Due to time difference, For the current step time, Use the original timestamps of the observation data. Calculate the manifold evolution increment. ,in, For manifold evolution increments, Due to time difference, For the Lie group exponential mapping operator, The Lie algebra vector is transformed by the hat mapping; then the adjoint matrix is ​​constructed to align the flight data to the unified filtering time, and at the unified filtering time, the extended Kalman filter is performed to update the posterior state and posterior covariance: ,in, Here is the Kalman gain matrix. The posterior covariance matrix is... Let be the transpose of the Jacobian matrix of the observation model. To observe the noise covariance matrix; ,in, for Posterior state estimation after time-lapse updates for Prior state estimation at time 10:00 For the Lie group exponential mapping operator, Here is the Kalman gain matrix. The observation residuals after time alignment compensation; ,in, for The posterior covariance matrix updated at each time step. It is the identity matrix. Let be the Jacobian matrix of the observation model.

[0198] The flight control computer extracts position sub-blocks from the posterior covariance and calculates the square root of the largest eigenvalue. ,

[0199] in, The square root of the largest eigenvalue. The operation is performed to find the largest eigenvalue of the position covariance component matrix. The location covariance component matrix is ​​given; and then normalized. ,in, The square root of the largest eigenvalue after normalization. For the amplitude limiting function, The square root of the largest eigenvalue. For low uncertainty threshold, For high uncertainty threshold, calibration parameters , ;

[0200] Perform spatial channel mapping: ,in, The adaptively adjusted safety protection distance Based on the safe distance, This is the adjustment coefficient for the spatial channel. The term is the power of the square root of the largest eigenvalue after normalization. The preset power adjustment coefficient, This is the lower limit of a safe distance. This is the upper limit of the safe distance;

[0201] Perform time channel mapping: ,in, The adaptively adjusted state confirmation time. Based on the confirmation time, This is the adjustment factor for the time channel. The term is the power of the square root of the largest eigenvalue after normalization. The preset power adjustment coefficient, To confirm the lower limit of the time, This is the upper limit for the confirmation time.

[0202] Set the enhanced protection threshold. Enhance protection threshold Risk intensity and duration The period is only when the square root of the normalized largest eigenvalue is... It enters the enhanced protection state only when the normalized maximum eigenvalue square root is maintained for more than 3 filtering cycles. It exits the enhanced protection state when it lasts for more than 3 filtering cycles.

[0203] The conditions for alarm confirmation are: ,in, This is an alarm confirmation flag. This is a collision risk indicator calculated in real time. Based on the adaptively adjusted safety protection distance Dynamically adjusted alarm thresholds The adaptively adjusted safety protection distance The duration of the current alarm status. For adaptively adjusted state confirmation time; threshold Follow Linear increase: .

[0204] During the tilt transition, when the aircraft suddenly and rapidly approaches an obstacle, the square root of the maximum eigenvalue increases due to a brief GPS obstruction, and the normalized square root of the maximum eigenvalue also increases accordingly. The system automatically adjusts... Increase, and at the same time Extend the calculation of risk intensity If the threshold is exceeded and the duration exceeds the preset period, an alarm will be triggered. The flight control system executes automatic collision avoidance maneuvers.

[0205] Those skilled in the art will understand that embodiments of the present invention can be provided as methods, systems, or computer program products. Therefore, the present invention can take the form of a completely hardware embodiment, a completely software embodiment, or an embodiment combining software and hardware aspects. Furthermore, the present invention can take the form of a computer program product embodied on one or more computer-usable storage media (including, but not limited to, disk storage, CD-ROM, optical storage, etc.) containing computer-usable program code.

[0206] This invention is described with reference to flowchart illustrations and / or block diagrams of methods, apparatus (systems), and computer program products according to embodiments of the invention. It will be understood that each block of the flowchart illustrations and / or block diagrams, and combinations of blocks in the flowchart illustrations and / or block diagrams, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, special-purpose computer, embedded processor, or other programmable data processing apparatus to produce a machine, such that the instructions, which execute via the processor of the computer or other programmable data processing apparatus, generate instructions for implementing the flowchart illustrations and / or block diagrams. Figure 1 One or more processes and / or boxes Figure 1 A device that provides the functions specified in one or more boxes.

[0207] These computer program instructions may also be stored in a computer-readable storage medium that can direct a computer or other programmable data processing device to function in a particular manner, such that the instructions stored in the computer-readable storage medium produce an article of manufacture including instruction means, which are implemented in a process Figure 1 One or more processes and / or boxes Figure 1 The function specified in one or more boxes.

[0208] These computer program instructions may also be loaded onto a computer or other programmable data processing apparatus to cause a series of operational steps to be performed on the computer or other programmable apparatus to produce a computer-implemented process, thereby providing instructions that execute on the computer or other programmable apparatus for implementing the process. Figure 1 One or more processes and / or boxes Figure 1 The steps of the function specified in one or more boxes.

[0209] The embodiments of the present invention have been described above with reference to the accompanying drawings. However, the present invention is not limited to the specific embodiments described above. The specific embodiments described above are merely illustrative and not restrictive. Those skilled in the art can make many other forms under the guidance of the present invention without departing from the spirit and scope of the claims. All of these forms are within the protection scope of the present invention.

Claims

1. A multi-sensor fusion alignment and collision avoidance protection linkage method for highly dynamic maneuvering aircraft, characterized in that, include: Acquire IMU sampling data, radar and visual observation data, and timestamps from the aircraft; Based on the pre-built state variable definition and error propagation basic model, the IMU sampled data is biased and corrected, the state evolution is performed on the biased and corrected IMU sampled data, and the predicted state and covariance prior are calculated. Based on the predicted state, a right-invariant error is defined, and the covariance prior is linearized and propagated based on the right-invariant error dynamics to obtain the posterior state and posterior covariance. Based on the radar and visual observation data, timestamps and the predicted state, the observation residuals are calculated and an adjoint matrix is ​​constructed. The observation residuals are aligned to the unified filtering time through the adjoint matrix. At the unified filtering time, the extended Kalman filter is performed to update the posterior state and posterior covariance. The location covariance component is extracted from the updated posterior covariance. The square root of the maximum eigenvalue of the location covariance component is calculated. The square root of the maximum eigenvalue is normalized and mapped. The mapping result is combined with dual-threshold hysteresis management, a pre-set conflict risk intensity index and risk intensity duration to generate an alarm confirmation output. At the same time, a linkage degradation flag is output to the execution protection agency.

2. The multi-sensor fusion alignment and collision avoidance protection linkage method for highly dynamic maneuvering aircraft according to claim 1, characterized in that, The IMU sampling data includes: triaxial angular velocity and triaxial acceleration collected by the airborne inertial measurement unit; The radar and visual observation data include: Radar observation data, including target point cloud data acquired by radar; Visual observation data, including the target bounding box coordinates acquired by the camera module; The timestamp includes the system time at which the radar observation data and visual observation data were actually collected.

3. The multi-sensor fusion alignment and collision avoidance protection linkage method for highly dynamic maneuvering aircraft according to claim 1, characterized in that, The steps for defining the state variables and constructing the basic model for error propagation include: An extended Lie group is used to construct the state matrix, which includes the rotation matrix, velocity vector, and position vector, as shown in the following formula: ， in, The state matrix, For rotation matrix, , It is a special orthogonal group in three dimensions. For velocity vectors, , For three-dimensional Euclidean space For position vectors, , To expand Lie groups; Gyroscope bias and accelerometer bias The bias is processed using a semi-direct product method, including: biasing the gyroscope. Exponentially mapped embedded rotation group The accelerometer bias Added in an additive form; The state variables are defined as 15-dimensional state variables, including 9-dimensional nominal states and 6-dimensional bias error states. The nominal states include a three-dimensional rotation matrix, a three-dimensional velocity vector, and a three-dimensional position vector. The bias error states include a three-dimensional gyroscope bias and a three-dimensional accelerometer bias. Based on the state matrix, the gyroscope is biased. and accelerometer bias The basic model of error propagation is jointly constructed by using the semi-direct product form as the extended error term.

4. The multi-sensor fusion alignment and collision avoidance protection linkage method for highly dynamic maneuvering aircraft according to claim 1, characterized in that, Also includes: If the location covariance component is unavailable or its value is abnormal, switch to conservative parameter pair and output linkage degradation flag; otherwise, according to the monotonic constraint calibration, map the mapping parameters after normalization of the square root of the largest eigenvalue, record the mapping parameters, covariance prior, alarm confirmation output and linkage degradation flag corresponding to each linkage decision, and form a traceable evidence package.

5. The multi-sensor fusion alignment and collision avoidance protection linkage method for highly dynamic maneuvering aircraft according to claim 1, characterized in that, The steps of bias correction of the IMU sampled data, performing state evolution on the bias-corrected IMU sampled data, and calculating the predicted state and covariance prior include: The bias correction for triaxial angular velocity and triaxial acceleration is as follows: ， ， in, The triaxial angular velocities after bias correction. The gyroscope is used to measure the angular velocity of its three axes. For gyroscope bias The triaxial acceleration after bias correction. For the original measurement of triaxial acceleration by the accelerometer, For accelerometer bias; The triaxial angular velocity after bias correction and triaxial acceleration The formula for converting to a Lie algebra vector is as follows: ， in, For Lie algebra vectors, This is the vector transpose of the triaxial angular velocities after bias correction. This is the vector transpose of the triaxial accelerations after bias correction. The transpose of the aircraft's velocity vector. To expand Lie groups The corresponding Lie algebra space; The Lie algebra vector is converted using the hat mapping. The matrix undergoes state evolution according to the following formula: ， in, for The updated state matrix at each time step. for Extended Lie Groups of Time State matrix, For the Lie group exponential mapping operator, The IMU sampling time interval This is the Lie algebra vector transformed by the hat mapping; The formula for calculating the predicted state is as follows: ， in, To predict the state, for The updated state matrix at each time step; Based on the aforementioned error propagation model, the covariance prior is calculated using the following formula: ， in, Let covariance be the prior matrix. The right-invariant error state transition Jacobian matrix is... for The covariance posterior matrix at time t. This is the transpose of the right-invariant error state transition Jacobian matrix. Inject matrix for process noise, The diagonal matrix of IMU noise covariance. Transpose of the process noise injection matrix.

6. The multi-sensor fusion alignment and collision avoidance protection linkage method for highly dynamic maneuvering aircraft according to claim 1, characterized in that, The steps of defining a right-invariant error based on the predicted state, performing linearization propagation of the covariance prior based on right-invariant error dynamics, and obtaining the posterior state and posterior covariance include: The right-invariant error is defined by the following formula: ， in, The error is right-invariant. The inverse of the true state value. Estimate the current state; Linearizing the error dynamics at the manifold identity element yields the state transition Jacobian matrix. Based on the state transition Jacobian matrix, covariance propagation is performed on the covariance prior to obtain the posterior state and posterior covariance. The covariance propagation formula is as follows: ， in, Let covariance be the prior matrix. The right-invariant error state transition Jacobian matrix is... for The covariance posterior matrix at time t. This is the transpose of the right-invariant error state transition Jacobian matrix. Inject matrix for process noise, The diagonal matrix of IMU noise covariance. Transpose of the process noise injection matrix.

7. The multi-sensor fusion alignment and collision avoidance protection linkage method for highly dynamic maneuvering aircraft according to claim 1, characterized in that, The steps of calculating the observation residual and constructing the adjoint matrix based on the radar and visual observation data, timestamps, and the predicted state, aligning the observation residual to the unified filtering time using the adjoint matrix, and performing extended Kalman filtering updates on the posterior state and posterior covariance at the unified filtering time include: The time difference of the timestamps is calculated using the following formula: ， in, Due to time difference, For the current step time, The original timestamp of the observation data; The predicted state is mapped to the observation space using an observation function to obtain the predicted observation value. The deviation between the radar and visual observation data and the predicted observation value is calculated to obtain the observation residual. The manifold evolution increment from the observation time to the filter step time is calculated based on the aforementioned time difference, using the following formula: ， in, For manifold evolution increments, Due to time difference, For the Lie group exponential mapping operator, This is the Lie algebra vector transformed by the hat mapping; The extended Lie group adjoint matrix is ​​calculated based on the manifold evolution increment, and the observation residual is shifted from the tangent space at the observation time to the tangent space at the filter step time. An extended Kalman filter is performed to update the posterior state and posterior covariance under a uniform time scale.

8. The multi-sensor fusion alignment and collision avoidance protection linkage method for highly dynamic maneuvering aircraft according to claim 1, characterized in that, The extended Kalman filter update includes: The Kalman gain is calculated using the following formula: ， in, Here is the Kalman gain matrix. The posterior covariance matrix is... Let be the transpose of the Jacobian matrix of the observation model. To observe the noise covariance matrix; The posterior state is updated using the following formula: ， in, for Posterior state estimation after time-lapse updates for Prior state estimation at time 10:00 For the Lie group exponential mapping operator, Here is the Kalman gain matrix. The observation residuals after time alignment compensation; The updated posterior covariance is calculated using the following formula: ， in, for The posterior covariance matrix updated at each time step. It is the identity matrix. Let be the Jacobian matrix of the observation model.

9. The multi-sensor fusion alignment and collision avoidance protection linkage method for highly dynamic maneuvering aircraft according to claim 1, characterized in that, The step of calculating the square root of the largest eigenvalue of the location covariance component is as follows: ， in, The square root of the largest eigenvalue. The operation is performed to find the largest eigenvalue of the position covariance component matrix. The position covariance component matrix; The step of normalizing the square root of the largest eigenvalue is as follows: ， in, The square root of the largest eigenvalue after normalization. For the amplitude limiting function, The square root of the largest eigenvalue. For low uncertainty threshold, This represents a high uncertainty threshold. In the step of normalizing the square root of the largest eigenvalue and then mapping, the mapping includes spatial channel mapping and temporal channel mapping, and the formula for spatial channel mapping is as follows: ， in, The adaptively adjusted safety protection distance Based on the safe distance, This is the adjustment coefficient for the spatial channel. The term is the power of the square root of the largest eigenvalue after normalization. The preset power adjustment coefficient, This is the lower limit of the safe distance. This is the upper limit of the safe distance; The time channel mapping formula is as follows: ， in, The adaptively adjusted state confirmation time. Based on the confirmation time, This is the adjustment factor for the time channel. The term is the power of the square root of the largest eigenvalue after normalization. The preset power adjustment coefficient, To confirm the lower limit of the time, This is the upper limit for the confirmation time.

10. The multi-sensor fusion alignment and collision avoidance protection linkage method for highly dynamic maneuvering aircraft according to claim 1, characterized in that, The step of generating an alarm confirmation output by combining the mapping results with dual-threshold hysteresis management, a pre-set conflict risk intensity index, and the duration of risk intensity includes: The pre-set conflict risk intensity indicators include the entry enhanced protection threshold and the exit enhanced protection threshold; The dual-threshold hysteresis management includes: entering the enhanced protection state only when the square root of the normalized maximum eigenvalue is not less than the threshold for entering enhanced protection and the cumulative time is not less than the duration of the risk intensity; and exiting the enhanced protection state only when the square root of the normalized maximum eigenvalue is not greater than the threshold for exiting enhanced protection and the cumulative time is not less than the duration of the risk intensity. The conditions for alarm confirmation are as follows: ， in, This is an alarm confirmation flag. This is a collision risk indicator calculated in real time. Based on the adaptively adjusted safety protection distance Dynamically adjusted alarm thresholds The adaptively adjusted safety protection distance The duration of the current alarm status. This is the adaptively adjusted state confirmation time.

Images