High-reliability aerial robot sensor failure navigation switching and recovery method

Abstract

This invention discloses a highly reliable method for navigation switching and recovery in aerial robots due to sensor failures. The method includes: constructing a sliding window for the temporary health flag of each sensor; comparing the number of consecutive frames of the temporary health flag within the sliding window with the length of the sliding window to obtain the final health flag of each sensor; constructing an error state Kalman filter; continuously recursively extrapolating the nominal state through the output of the inertial measurement unit; and predictively propagating the covariance matrix of the error state; updating the sensor's error state by observation when the final health flag corresponding to the sensor is a healthy state flag; and compensating the updated error state to the nominal state to obtain the optimal estimated positioning state. Independent health state detection and fault isolation enable real-time identification and isolation of hardware-level faulty sensors that continuously output erroneous data, achieving accurate navigation for aerial robots.

Description

Technical Field

[0001] This invention relates to the field of aerial robot control, and more specifically to a highly reliable aerial robot sensor failure navigation switching and recovery method. Background Technology

[0002] With the widespread application of aerial robots in various mission scenarios such as power line inspection, indoor pipe gallery inspection, and emergency rescue, higher requirements are placed on the continuity and safety of aerial robots' positioning in complex environments. During flight, aerial robots may experience sudden hardware failures of sensors or environmental degradation that cause the positioning source to fail, thus forcing them to make an emergency landing and interrupting their mission.

[0003] Current aerial robots mostly use a single sensor as the positioning source or employ tight-coupled fusion of multiple sensors. Single-sensor solutions lack redundancy; in the event of a sensor hardware failure, they cannot switch to other sensors as the positioning source, leading to inaccurate navigation and potentially forced landings. Tight-coupled fusion solutions, on the other hand, indiscriminately couple all sensor data together, lacking the ability to isolate the failure of a single sensor. If a sensor or a link in the fusion framework fails, the resulting erroneous constraints directly contaminate the joint optimization process, causing the pose estimation of the entire fusion system to deviate from the true value, making accurate navigation difficult for the aerial robot. Summary of the Invention

[0004] To overcome the shortcomings of existing technologies, this invention aims to provide a highly reliable method for navigation switching and recovery in aerial robots when sensor failures occur, in order to solve the problem that aerial robots are difficult to navigate accurately when sensor failures occur.

[0005] To achieve the above objectives, the technical solution adopted by the present invention is as follows: Firstly, this application provides a highly reliable method for navigation switching and recovery in the event of sensor failure in an aerial robot. The aerial robot includes an inertial measurement unit and at least two sensors. The highly reliable method for navigation switching and recovery in the event of sensor failure in an aerial robot includes: Perform health checks on the output of each sensor to obtain a temporary health flag for each sensor; A sliding window is constructed for the temporary health flag of each sensor. The final health flag of each sensor is obtained by comparing the number of consecutive frames of the temporary health flag within the sliding window with the length of the sliding window. The sliding window is used to store the history of the temporary health flag of the sensor. An error state Kalman filter is constructed, which continuously recursively calculates the nominal state through the output of the inertial measurement unit and predicts and propagates the covariance matrix of the error state. For any sensor, if the final health flag corresponding to the sensor is a health status flag, the error status of the sensor is observed and updated to obtain the updated error status. The updated error state is compensated to the nominal state, and the error state is cleared. External sources are prohibited from resetting the nominal state to obtain the optimal estimated positioning state. The attitude and position of the aerial robot are controlled by the optimal estimated positioning state, which includes the position, velocity and attitude of the aerial robot.

[0006] In embodiments of this application, the method further includes: If the duration of the final health flag for each sensor being an abnormal status flag exceeds the first duration threshold, an independent hovering state command is triggered, and the altitude reference value corresponding to the moment the independent hovering state command is triggered is latched. A virtual damping term is applied to the output velocity, and the horizontal attitude of the aerial robot is maintained by attitude estimation from the inertial measurement unit. The aerial robot's altitude is maintained in an open loop by integrating the altitude reference value with the vertical direction of the inertial measurement unit accelerometer.

[0007] In embodiments of this application, the method further includes: Health checks are performed on the inertial measurement unit to obtain its health indicators; If the duration of the abnormal status indicator of the inertial measurement unit exceeds the second duration threshold and an independent hovering state command is triggered, a runaway protection logic command is triggered to control the aerial robot to make an emergency landing.

[0008] In the embodiments of this application, health detection includes data integrity anomaly detection, data quality anomaly detection, and data activity anomaly detection; The nominal state includes the three-dimensional position of the aerial robot, the three-dimensional velocity of the aerial robot, the four-dimensional attitude quaternion of the aerial robot, and the zero bias of the six-dimensional inertial measurement unit; The error states include the three-dimensional position error of the aerial robot, the three-dimensional velocity error of the aerial robot, the four-dimensional attitude quaternion error of the aerial robot, and the zero bias error of the six-dimensional inertial measurement unit.

[0009] In the embodiments of this application, the final health flag of each sensor is obtained by comparing the number of consecutive frames of the temporary health flag within the sliding window with the length of the sliding window, including: If the first ratio is greater than or equal to the first ratio threshold, the final health flag is switched to a health status flag, wherein the first ratio is the ratio between the number of frames in which the temporary health flag is continuously a health status flag within the sliding window and the length of the sliding window. If the second ratio is greater than or equal to the second ratio threshold, the final health flag is switched to the abnormal state flag, wherein the first ratio threshold is greater than the second ratio threshold, and the second ratio is the ratio between the number of frames in which the temporary health flag is continuously an abnormal state flag within the sliding window and the length of the sliding window.

[0010] In the embodiments of this application, at least two sensors include at least one lidar and at least one visual acquisition device; For any given sensor, if the final health flag corresponding to the sensor is a health status flag, the sensor's error status is updated through observation, resulting in the updated error status, including: When the final health indicator corresponding to the lidar is the health status indicator, the lidar error status is updated by laser observation based on the distance residual from the point cloud to the line and the distance residual from the point cloud to the surface, and the updated error status is obtained. When the final health indicator corresponding to the visual acquisition device is a health status indicator, the visual observation of the error status of the visual acquisition device is updated based on the feature point reprojection residual, and the updated error status is obtained.

[0011] In embodiments of this application, the method further includes: For any sensor, if the final health flag corresponding to the sensor is an abnormal flag, skip the observation update of the sensor's error state and obtain the duration of skipping the observation update. If the duration of all sensors skipping observation updates exceeds the third duration threshold, upper limit saturation processing is performed on the covariance matrix, or prediction propagation of the covariance matrix is ​​stopped. Upper limit saturation processing restricts each diagonal element of the covariance matrix to within a preset upper limit of covariance.

[0012] In the embodiments of this application, for any sensor, when the final health flag corresponding to the sensor is a health status flag, the sensor's error status is observed and updated to obtain the updated error status, including: For any sensor, if the final health flag corresponding to the sensor is a health status flag and the number of health status flag frames does not exceed a preset number, perform a chi-square test on the observation residuals of the sensor to obtain the chi-square test value. If the chi-square test values ​​of multiple consecutive frames do not have a preset chi-square critical value, then the error state of the sensor is updated by observation, and the updated error state is obtained. The method also includes: If the chi-square test value exceeds the preset chi-square threshold, the sensor's observation update will be interrupted.

[0013] Secondly, this application provides a computing device, including... The memory is configured to store instructions; The processor is configured to retrieve instructions from memory and, when executing instructions, implement the aforementioned highly reliable aerial robot sensor failure navigation switching and recovery method.

[0014] Thirdly, this application provides an aerial robot, including an inertial measurement unit, at least two sensors, and the aforementioned computing device.

[0015] Fourthly, this application provides a machine-readable storage medium storing instructions for causing a machine to execute the aforementioned highly reliable aerial robot sensor failure navigation switching and recovery method.

[0016] This application provides a highly reliable method for navigation switching and recovery in aerial robot sensor failures, comprising: performing health checks on the output of each sensor to obtain a temporary health flag for each sensor; constructing a sliding window for the temporary health flag of each sensor, comparing the number of consecutive frames of the temporary health flag within the sliding window with the length of the sliding window to obtain the final health flag of each sensor; constructing an error state Kalman filter, continuously recursively extrapolating the nominal state through the output of the inertial measurement unit, and performing predictive propagation on the covariance matrix of the error state; for any sensor, when the final health flag corresponding to the sensor is a health state flag, observing and updating the sensor's error state to obtain the updated error state; compensating the updated error state to the nominal state, clearing the error state, prohibiting external sources from resetting the nominal state, obtaining the optimal estimated positioning state, and controlling the attitude and position of the aerial robot through the optimal estimated positioning state.

[0017] Compared with the prior art, the beneficial effects of the present invention are: Each sensor requires a final health flag-driven gating decision. Only when the final health flag indicates a healthy state will the observation be included in the error state measurement update; otherwise, the error layer skips the observation update. The continuous recursion of the nominal state remains unaffected, thus mapping the online access and exit of sensors to whether the error state has undergone a small correction, resulting in a continuous and smooth pose for the aerial robot's localization. This fundamentally eliminates the positional step caused by hard navigation switching, achieving measurement-level isolation of faulty sensors and stepless navigation switching, ensuring accurate navigation of the aerial robot even with the failure of any sensor.

[0018] Before the data from various sensors enters the fusion backend, independent health status detection and fault isolation nodes are set up. These nodes can identify and isolate hardware-level faulty sensors that continuously output erroneous data in real time, preventing erroneous observations from contaminating the global state estimation. This enables the aerial robot's multi-sensor fusion system to operate under degraded conditions for both fault and safety. When a sensor recovers from an anomaly to a healthy state, it is only necessary to re-gated the sensor observations at the error layer. The correction amount is naturally injected into the nominal state in the form of a small amount, resulting in a continuous and smooth output pose, thereby enabling accurate navigation of the aerial robot. Attached Figure Description

[0019] The accompanying drawings are provided to further illustrate embodiments of the present invention and form part of the specification. They are used together with the following detailed description to explain the embodiments of the present invention, but do not constitute a limitation thereof. In the drawings: Figure 1 This is a flowchart of a highly reliable aerial robot sensor failure navigation switching and recovery method according to an embodiment of the present invention; Figure 2 This is a schematic diagram of a highly reliable aerial robot sensor fault navigation switching and recovery device according to an embodiment of the present invention. Detailed Implementation

[0020] The specific embodiments of the present invention will now be described in detail with reference to the accompanying drawings. It should be understood that the specific embodiments described herein are for illustrative and explanatory purposes only and are not intended to limit the scope of the present invention.

[0021] The components of the embodiments of the invention described and illustrated herein can typically be arranged and designed in various different configurations. Therefore, the following detailed description of the embodiments of the invention provided in the accompanying drawings is not intended to limit the scope of the claimed invention, but merely to illustrate selected embodiments of the invention. All other embodiments obtained by those skilled in the art based on the embodiments of the invention without inventive effort are within the scope of protection of the invention.

[0022] In the following, the terms “comprising,” “having,” and their cognates, which may be used in various embodiments of the invention, are intended only to indicate a particular feature, number, step, operation, element, component, or combination thereof, and should not be construed as excluding, firstly, the presence of one or more other features, numbers, steps, operations, elements, components, or combinations thereof, or adding the possibility of one or more features, numbers, steps, operations, elements, components, or combinations thereof.

[0023] Furthermore, the terms "first," "second," and "third" are used only to distinguish descriptions and should not be interpreted as indicating or implying relative importance.

[0024] Unless otherwise specified, all terms used herein (including technical and scientific terms) shall have the same meaning as commonly understood by one of ordinary skill in the art to which the various embodiments of the invention pertain. Terms (such as those defined in commonly used dictionaries) shall be interpreted as having the same meaning as in their contextual meaning in the relevant technical field and shall not be interpreted as having an idealized or overly formal meaning, unless clearly defined in the various embodiments of the invention.

[0025] Currently, some aerial robots are beginning to use a tightly coupled fusion localization method using three types of sensors: LiDAR, visual cameras, and inertial measurement units (IMUs). Some aerial robots are also starting to use Kalman filters to fuse data from multiple sensors. The laser residual, visual residual, and IMU pre-integrated residual are jointly added to a backend based on a sliding window or factor graph for joint nonlinear optimization. When data from one sensor is missing, the system degrades to running the subsystem with the remaining normal sensors; after recovery, it is reintegrated into the joint optimization.

[0026] However, current aerial robot localization fusion frameworks only perform degradation processing for missing data and cannot identify and isolate continuously outputting erroneous data. When any sensor experiences a motor failure or data blockage, the sensor may still output data that appears to exist but is actually completely wrong. Erroneous observations are directly added to the joint optimization process, skewing the pose estimation across the entire sliding window, causing the entire fusion system to collapse, making it difficult for the aerial robot to navigate accurately when sensors fail.

[0027] Currently, aerial robots employ conditional branch navigation hard switching during primary / backup positioning source switching or faulty sensor recovery. Because there is a cumulative deviation between the primary and backup positioning sources or between the faulty sensor's original pose at the moment of recovery and the current system's drifted pose, this deviation is directly injected into the flight control's pose feedback loop in the form of a position step, triggering violent attitude correction maneuvers by the aerial robot and threatening flight safety. Furthermore, in the event of positioning source failure, only the flight control's runaway protection logic can be triggered due to pose invalidity for a forced landing, failing to meet the core requirement of hovering in the air while awaiting fault recovery or manual takeover during inspection missions.

[0028] In the field of nonlinear state estimation, commonly used filter variants include the Extended Kalman Filter (EKF), Unscented Kalman Filter (UKF), Iterated Extended Kalman Filter (IEKF), and Error-State Kalman Filter (ESKF). Commonly used EKF, Unscented Kalman Filter, and Iterated Extended Kalman Filter directly represent all state variables, such as pose, velocity, and IMU bias, in the same state vector, without structural separation between nominal and error states. When any observation source in the fusion framework needs to perform at least one of switching, skipping, or temporary deactivation, the entire state vector must be directly reset or reinitialized, resulting in a position step jump for the aerial robot at the output, threatening the stability of the aerial robot's flight control.

[0029] Example 1 Please see Figure 1 , Figure 1 This is a flowchart of a highly reliable aerial robot sensor failure navigation switching and recovery method according to an embodiment of the present invention. The aerial robot includes an inertial measurement unit and at least two sensors. Figure 1 The highly reliable aerial robot sensor failure navigation switching and recovery methods include: S110 performs health checks on the output of each sensor to obtain a temporary health flag for each sensor.

[0030] The outputs of the inertial measurement unit and each sensor are acquired synchronously, maintaining hardware or software time synchronization during the acquisition process. The quality and validity of the output of each sensor are checked, enabling independent health checks for each sensor's output and obtaining a temporary health flag for each sensor. This temporary health flag can be either a healthy status flag or an abnormal status flag, which will not be elaborated upon here.

[0031] S120: Construct a sliding window for the temporary health flag of each sensor, compare the number of consecutive frames of the temporary health flag within the sliding window with the length of the sliding window, and obtain the final health flag of each sensor. The sliding window is used to store the history of the temporary health flag of the sensor.

[0032] A sliding window is constructed for the temporary health flags of each sensor, and the sliding window is used to store the history of temporary health flags of the sensor. The sliding window has a fixed length and stores the history of temporary health flags for multiple consecutive frames. By comparing the number of consecutive frames of temporary health flags stored in the sliding window with the length of the sliding window, the sensor is determined to be abnormal based on the ratio between the temporary health flags and the length of the sliding window, and thus the final health flag of each sensor is obtained.

[0033] S130, construct an error state Kalman filter, continuously recursively derive the nominal state through the output of the inertial measurement unit, and predict and propagate the covariance matrix of the error state.

[0034] The error-state Kalman filter includes the nominal state, the error state, and the covariance matrix of the error state. The structural characteristic of the error-state Kalman filter is that it decomposes the system state into two parts: the nominal state and the error state. This structural characteristic of separating the nominal and error states is structurally unattainable by other filters such as the extended Kalman filter, the unscented Kalman filter, and the iterative extended Kalman filter. For filters other than the error-state Kalman filter, resetting the state means resetting the nominal value. To achieve navigation switching without position steps, this embodiment constructs an error-state Kalman filter as a fusion framework.

[0035] The nominal state is continuously recursively derived from the output of the inertial measurement unit (IMU). Driven by the IMU, the nominal state is continuously derived and does not participate in the direct estimation of the filter. The error-state Kalman filter also includes the covariance matrix of the error state. Predictive propagation is performed on the covariance matrix of the error state, allowing the error state to be independently maintained and corrected, serving as the actual estimation object for the Kalman filter.

[0036] S140, for any sensor, if the final health flag corresponding to the sensor is a healthy status flag, perform an observation update on the sensor's error status to obtain the updated error status, wherein the observation update of sensors whose final health flag is an abnormal status flag is skipped.

[0037] When the final health flag corresponding to a sensor is an abnormal state flag, the sensor's observation update is skipped, and the continuous recursion of the nominal state remains unaffected. Sensor data deemed abnormal is directly blocked at the entry point and does not participate in any state estimation. When the final health flag corresponding to a sensor is a healthy state flag, the sensor's error state is updated through observation, resulting in the updated error state. When any sensor recovers from abnormal to healthy, it is only necessary to re-gated the sensor's observation update at the error layer. Subsequent corrections are naturally injected into the nominal state in the form of minute amounts, resulting in a continuous and smooth output pose.

[0038] S150, the updated error state is compensated to the nominal state, and the error state is cleared. External sources are prohibited from resetting the nominal state to obtain the optimal estimated positioning state. The attitude and position of the aerial robot are controlled by the optimal estimated positioning state, where the positioning state includes the position, velocity and attitude of the aerial robot.

[0039] The sensor's error state is observed and updated. After obtaining the updated error state, it is compensated to the nominal state, and the error state is cleared to obtain the optimal estimated positioning state. During the compensation of the nominal state, external sources are prohibited from resetting the nominal state to ensure the continuity of the pose output. The positioning state includes the optimal estimated position, velocity, and attitude of the aerial robot. In this embodiment, the aerial robot also includes a flight control system, which transmits the optimal estimated positioning state to the flight control system to control the aerial robot's attitude and position. When the sensor recovers from an anomaly to a healthy state, it is only necessary to re-gated the sensor observation at the error layer. The correction amount is naturally injected into the nominal state in the form of a small amount, resulting in a continuous and smooth output pose.

[0040] It should be understood that the aerial robot in this embodiment can also be replaced by other flying devices. These other flying devices are similar to the aerial robot, including an inertial measurement unit and at least two sensors. The type of other flying device is determined according to actual needs and can be a drone, etc., and is not limited here.

[0041] Currently, aerial robot observation updates are fixed after filter initialization by connecting to all observation sources. This application provides a highly reliable aerial robot sensor fault navigation switching and recovery method, including: performing health checks on the output of each sensor to obtain a temporary health flag for each sensor; constructing a sliding window for the temporary health flag of each sensor, comparing the number of consecutive frames of the temporary health flag within the sliding window with the length of the sliding window to obtain the final health flag of each sensor; constructing an error state Kalman filter, continuously recursively extrapolating the nominal state through the output of the inertial measurement unit, and performing predictive propagation on the covariance matrix; for any sensor, when the final health flag corresponding to the sensor is a health state flag, performing error state observation updates on the sensor to obtain the updated error state; compensating the updated error state to the nominal state, clearing the error state, prohibiting external sources from resetting the nominal state, obtaining the optimal estimated positioning state, and controlling the attitude and position of the aerial robot through the optimal estimated positioning state.

[0042] Each sensor requires a final health flag-driven gating decision. Only when the final health flag indicates a healthy state will the observation be included in the error state measurement update; otherwise, the error layer skips the observation update. The continuous recursion of the nominal state remains unaffected, thus mapping the online access and exit of sensors to whether the error state has undergone a small correction, resulting in a continuous and smooth pose for the aerial robot's localization. This fundamentally eliminates the positional step caused by hard navigation switching, achieving measurement-level isolation of faulty sensors and stepless navigation switching, ensuring accurate navigation of the aerial robot even with the failure of any sensor.

[0043] Before the data from various sensors enters the fusion backend, independent health status detection and fault isolation nodes are set up. These nodes can identify and isolate hardware-level faulty sensors that continuously output erroneous data in real time, preventing erroneous observations from contaminating the global state estimation. This enables the aerial robot's multi-sensor fusion system to operate under degraded conditions for both fault and safety. When a sensor recovers from an anomaly to a healthy state, it is only necessary to re-gated the sensor observations at the error layer. The correction amount is naturally injected into the nominal state in the form of a small amount, resulting in a continuous and smooth output pose, thereby enabling accurate navigation of the aerial robot.

[0044] In embodiments of this application, the method further includes: If the duration of the final health flag for each sensor being an abnormal status flag exceeds the first duration threshold, an independent hovering state command is triggered, and the altitude reference value corresponding to the moment the independent hovering state command is triggered is latched. A virtual damping term is applied to the output velocity, and the horizontal attitude of the aerial robot is maintained by attitude estimation from the inertial measurement unit. The aerial robot's altitude is maintained in an open loop by integrating the altitude reference value with the vertical direction of the inertial measurement unit accelerometer.

[0045] If the duration of the final health indicator for each sensor corresponding to the LiDAR and visual acquisition equipment being an abnormal state exceeds a first duration threshold, an independent hovering state command is triggered, automatically activating the aerial robot into a hovering state. The altitude reference value corresponding to the moment the independent hovering state command is triggered is latched. A virtual damping term approaching zero is applied to the output speed. Formula (1) Where t is the current time, V out (t) represents the output velocity at time t after applying a virtual damping term that approaches zero. est (t) represents the velocity estimate output by the error state Kalman filter at time t, and t0 is the time when the independent hovering state command is triggered. This is the decay time constant.

[0046] The virtual damping term gradually decays the output velocity to near zero, thereby generating a command that causes the aerial robot to hover, providing a stable hovering window before the inertial measurement unit's integration error becomes unacceptable. Triggering the independent hovering command requires both the lidar and visual acquisition devices to fail; this can be understood as no longer relying on sensor observations. The aerial robot's horizontal attitude is maintained through attitude estimation by the inertial measurement unit, and its altitude is maintained in an open-loop manner by integrating the altitude reference value with the vertical direction of the inertial measurement unit's accelerometer. The values ​​of the first duration threshold and the decay time constant are set according to actual needs. In this embodiment, the first duration threshold can be selected within the range of 0.3 seconds to 1.0 seconds, and the decay time constant can be selected within the range of 0.5 to 2.0 seconds; no specific limitation is imposed here.

[0047] In the event of failure of all external sensors, an independent hovering state command is triggered, providing a time window for fault recovery or manual takeover, avoiding unnecessary forced landings, and improving the completion rate and overall safety of high-value inspection tasks. This differs from existing aerial robots that only switch to other sensors for continued positioning when a single sensor fails. In this embodiment, even with all sensors failing, the independent hovering state command triggers an independent hovering sub-state for the aerial robot, providing it with active hovering capability.

[0048] In embodiments of this application, the method further includes: Health checks are performed on the inertial measurement unit to obtain its health indicators; If the duration of the abnormal status indicator of the inertial measurement unit exceeds the second duration threshold and an independent hovering state command is triggered, a runaway protection logic command is triggered to control the aerial robot to make an emergency landing.

[0049] This embodiment also provides failure handling for the inertial measurement unit (IMU). A health check is performed on the IMU to obtain a health flag. For ease of understanding, the health flag of the IMU in this embodiment is represented as F. imu The health indicator of the inertial measurement unit is the abnormal state indicator, represented by F. imu =0. If the duration of the abnormal state flag in the inertial measurement unit exceeds the second duration threshold, a runaway protection logic command is triggered to control the aerial robot to make an emergency landing. The second duration threshold is set according to actual needs; in this embodiment, it ranges from 0.1 seconds to 0.5 seconds and is not limited thereto. For ease of understanding, the second duration threshold in this embodiment is 0.2 seconds.

[0050] If the duration of the abnormal status flag (health indicator) of the inertial measurement unit (IMU) exceeds a second duration threshold, the IMU is determined to have failed. If the duration of the abnormal status flag exceeds the second duration threshold and an independent hovering state command is triggered, the IMU failure renders the independent hovering sub-state inoperable, as the IMU is the sole input source for the independent hovering sub-state. In this embodiment, a runaway protection logic command is triggered as the final level of safety degradation to control the aerial robot to make an emergency landing.

[0051] In the embodiments of this application, health detection includes data integrity anomaly detection, data quality anomaly detection, and data activity anomaly detection; The nominal state includes the three-dimensional position of the aerial robot, the three-dimensional velocity of the aerial robot, the four-dimensional attitude quaternion of the aerial robot, and the zero bias of the six-dimensional inertial measurement unit; The error states include the three-dimensional position error of the aerial robot, the three-dimensional velocity error of the aerial robot, the four-dimensional attitude quaternion error of the aerial robot, and the zero bias error of the six-dimensional inertial measurement unit.

[0052] Health checks include data integrity anomaly detection, data quality anomaly detection, and data activity anomaly detection. Data integrity anomaly detection includes, but is not limited to, detection of data timeouts, dropped frames, and stuck values, which will not be elaborated upon here. Data quality anomaly detection includes, but is not limited to, detection of sparse laser point clouds, point cloud degradation, overexposed images, underexposed images, and weak image textures, which will not be elaborated upon here. Data activity anomaly detection includes, but is not limited to, detection of zero-variance deadlock in the inertial measurement unit and data spikes, which will not be elaborated upon here.

[0053] Data integrity anomaly detection is used to detect timeouts, frame drops, and data freezes in the inertial measurement unit (IMU) and various sensors. Data quality anomaly detection is used to detect the sparsity and degradation of the LiDAR point cloud, the mean brightness and gradient variance of the image, and peaks in the IMU data. Data activity anomaly detection checks whether the sliding window variance is below a threshold to determine zero-variance deadlock and detects the continuity of timestamps for data frames from various sensors, used for IMU detection.

[0054] The nominal state includes the aerial robot's three-dimensional position, three-dimensional velocity, four-dimensional attitude quaternion, and six-dimensional inertial measurement unit zero bias. In this embodiment, the nominal state is a 16-dimensional vector, represented as x=[p, v, q, b]. a b g ], where x is the 16-dimensional nominal state, p is the 3D position of the aerial robot in the world coordinate system, v is the 3D velocity of the aerial robot in the world coordinate system, q is the 4D attitude quaternion of the aerial robot from the body coordinate system to the world coordinate system, b a For zero bias of the accelerometer, bg The zero bias of the gyroscope, the zero bias of the accelerometer, and the zero bias of the gyroscope together form a six-dimensional inertial measurement unit.

[0055] The error state is the difference between the true state and the nominal state, and it is the object actually estimated by the error state Kalman filter. The error state includes the aerial robot's 3D position error, 3D velocity error, 4D attitude quaternion error, and 6D inertial measurement unit zero-bias error. The error state is a 15-dimensional vector, defined as follows: = ,in, For 15-dimensional error states, For the three-dimensional position error of the aerial robot, For the three-dimensional velocity error of the aerial robot, Let be a small rotation vector representing the four-dimensional attitude quaternion error of the aerial robot, and its relationship with the true attitude and nominal attitude is as follows: , This represents quaternion multiplication, where Exp is the value that maps minute rotations to unit quaternions, and b... a For accelerometer zero bias error, b g The zero bias error of the gyroscope, the zero bias error of the accelerometer, and the zero bias error of the gyroscope together form the zero bias error of the six-dimensional inertial measurement unit, where T is the transpose.

[0056] Regardless of sensor health, the system executes the steps of continuously recursively estimating the nominal state using the output of the inertial measurement unit (IMU) and predictively propagating the covariance matrix. The nominal state is a state quantity obtained by continuously integrating the IMU measurements through the kinematic equations; it is not directly estimated using the error state Kalman filter. For ease of understanding, in the embodiments of this application, the nominal state recursion specifically employs the fourth-order Runge-Kutta method for numerical integration. The nominal state recursion equation is: ; ; ; ; Formula (2) Where dt is the differential symbol, p is the three-dimensional position of the aerial robot in the world coordinate system, v is the three-dimensional velocity of the aerial robot in the world coordinate system, q is the four-dimensional attitude quaternion of the aerial robot from the body coordinate system to the world coordinate system, and b is the three-dimensional position of the aerial robot in the world coordinate system. a For zero bias of the accelerometer, b g Let R(q) be the rotation matrix corresponding to the nominal attitude quaternion, and g be the gravitational acceleration vector in the world coordinate system, where q is the gyroscope's zero bias. ( ) is the pure imaginary quaternion corresponding to the angular velocity. , a mFor the mechanical acceleration measured by the inertial measurement unit, n is the angular velocity of the machine system measured by the inertial measurement unit. a and n g All are zero-mean Gaussian white noise, a ture For the actual acceleration under the machine system, For the actual angular velocity in the machine system, For the true zero bias of the accelerometer, This represents the true zero bias of the gyroscope.

[0057] The prediction propagation of the covariance matrix is ​​performed, and the error state covariance propagation equation is: Formula (3) Where dt is the differential symbol, p is the three-dimensional position of the aerial robot in the world coordinate system, P is the covariance matrix of the error state, which is a 15×15 matrix, F is the error state transition matrix, G is the noise injection matrix, Q is the spectral density matrix of the inertial measurement unit noise, and T is the transpose.

[0058] In the embodiments of this application, the final health flag of each sensor is obtained by comparing the number of consecutive frames of the temporary health flag within the sliding window with the length of the sliding window, including: If the first ratio is greater than or equal to the first ratio threshold, the final health flag is switched to a health status flag, wherein the first ratio is the ratio between the number of frames in which the temporary health flag is continuously a health status flag within the sliding window and the length of the sliding window. If the second ratio is greater than or equal to the second ratio threshold, the final health flag is switched to the abnormal state flag, wherein the first ratio threshold is greater than the second ratio threshold, and the second ratio is the ratio between the number of frames in which the temporary health flag is continuously an abnormal state flag within the sliding window and the length of the sliding window.

[0059] A sliding window is maintained for the temporary health flag of each sensor, and a ratio is determined between the number of consecutive identical temporary health flag frames and the length of the sliding window. The first ratio is the ratio between the number of consecutive healthy frames within the sliding window and the length of the sliding window; the second ratio is the ratio between the number of consecutive abnormal frames within the sliding window and the length of the sliding window. For ease of understanding, in the embodiments of this application, the temporary health flag of the i-th sensor is represented as F. i The temporary health flag of the i-th sensor is represented by the health status flag F. i =1, the temporary health flag of the i-th sensor is the abnormal state flag represented by F. i =0.

[0060] The final health flag determination uses a hysteresis-based method, preventing immediate switching of the final health flag due to temporary health flag changes in one or two frames, thus avoiding invalid isolation caused by brief frame drops. When the first ratio is greater than or equal to a first ratio threshold, the output final health flag switches from an abnormal state flag to a healthy state flag. When the second ratio is greater than or equal to a second ratio threshold, the output final health flag switches from a healthy state flag to an abnormal state flag. For ease of understanding, the temporary health flag in this embodiment is represented as F. sensor The final health indicator is continuously represented by the health status indicator, which is F. sensor =1, the final health indicator is continuously an abnormal state, represented by F. sensor =0.

[0061] The length of the sliding window is set according to actual needs. In this embodiment, it can be selected within the range of 10 to 50, and is not limited here. The first ratio threshold is greater than the second ratio threshold. Both the first ratio threshold and the second ratio threshold are set according to actual needs and are not limited here. For ease of understanding, in the embodiment of this application, the first ratio threshold is 0.8 and the second ratio threshold is 0.7.

[0062] In the embodiments of this application, at least two sensors include at least one lidar and at least one visual acquisition device; For any given sensor, if the final health flag corresponding to the sensor is a health status flag, the sensor's error status is updated through observation, resulting in the updated error status, including: When the final health indicator corresponding to the lidar is the health status indicator, the lidar error status is updated by laser observation based on the distance residual from the point cloud to the line and the distance residual from the point cloud to the surface, and the updated error status is obtained. When the final health indicator corresponding to the visual acquisition device is a health status indicator, the visual observation of the error status of the visual acquisition device is updated based on the feature point reprojection residual, and the updated error status is obtained.

[0063] The two sensors include at least one LiDAR and at least one visual acquisition device. The types of LiDAR and visual acquisition devices are determined according to actual needs and are not limited here. For ease of understanding, in the embodiments of this application, the visual acquisition device is a visual camera. If the final health indicators corresponding to both the LiDAR and the visual acquisition device are abnormal status indicators, the duration of the abnormal status indicator is determined.

[0064] When the final health indicator corresponding to the lidar is a health status indicator, the lidar error state is updated based on the distance residuals from the point cloud to the line and from the point cloud to the surface, resulting in the updated error state. The lidar observation update equation is: Formula (4) Formula (5) Formula (6) Among them, K lidar Let P be the Kalman gain matrix, P be the covariance matrix of the error state, and H be the Kalman gain matrix. lidar To observe the Jacobian matrix of the relative error state of the residuals, lidar represents lidar observation. For 15-dimensional error states, R lidar Let T be the observation noise covariance matrix matched for the laser point cloud, and T be the transpose. , Let be the distance residual of the i-th feature point in the point cloud. Let be the coordinates of the i-th feature point in the point cloud in the world frame. Let R(q) be the coordinates of the i-th feature point in the point cloud in the body coordinate system of the inertial measurement unit, and let R(q) be the rotation matrix corresponding to the nominal attitude quaternion.

[0065] When the final health indicator corresponding to the visual acquisition device is a health status indicator, the visual observation update of the error status of the visual acquisition device is performed based on the feature point reprojection residual, resulting in the updated error status. The observation update equation of the visual acquisition device is as follows: Formula (7) Formula (8) Formula (9) Among them, K vison Let P be the Kalman gain matrix, P be the covariance matrix of the error state, and H be the Kalman gain matrix. vison To observe the Jacobian matrix of the relative error state of the residuals, Vision is used as the visual acquisition device. For 15-dimensional error states, R vison Let r be the observation noise covariance matrix for matching laser point clouds. vison This represents the reprojection residual in the world coordinate system.

[0066] In embodiments of this application, the method further includes: For any sensor, if the final health flag corresponding to the sensor is an abnormal flag, skip the observation update of the sensor's error state and obtain the duration of skipping the observation update. If the duration of all sensors skipping observation updates exceeds the third duration threshold, upper limit saturation processing is performed on the covariance matrix, or prediction propagation of the covariance matrix is ​​stopped. Upper limit saturation processing restricts each diagonal element of the covariance matrix to within a preset upper limit of covariance.

[0067] If the final health flag is a healthy state flag, observation updates are performed; if the final health flag is an abnormal state flag, observation updates are skipped, and only the time update structure is retained. The nominal state is not truncated during switching; it is continuously recursively pushed forward in the time update process. Switching only affects the error state; enabling or disabling any observation only affects whether the error state receives a correction once. Error injection is smooth; the error state is always a small quantity, and its addition to the nominal state naturally smooths the correction to the output.

[0068] Since the covariance matrix of the error-state Kalman filter continues to grow over time in the absence of external observations, and the final health indicator for all sensors is an abnormal state indicator—that is, after all sensors fail, and the duration for which all sensors skip observation updates exceeds a third duration threshold—an upper limit saturation process can be performed on the covariance matrix. Specifically, the upper limit saturation process restricts each diagonal element of the covariance matrix to within a preset upper limit of covariance, i.e., makes... ,in, The diagonal elements are represented by P, which is the covariance matrix of the error state. This is a preset upper limit for the covariance. If the duration of all sensors skipping observation updates exceeds the third duration threshold, prediction propagation of the covariance matrix can also be stopped, i.e., let... =0, where dt is the differential symbol, and p is the three-dimensional position of the aerial robot in the world coordinate system. It should be understood that the third duration threshold and the preset upper limit of covariance are set according to actual needs and are not limited here.

[0069] In the embodiments of this application, for any sensor, when the final health flag corresponding to the sensor is a health status flag, the sensor's error status is observed and updated to obtain the updated error status, including: For any sensor, if the final health flag corresponding to the sensor is a health status flag and the number of health status flag frames does not exceed a preset number, perform a chi-square test on the observation residuals of the sensor to obtain the chi-square test value. If the chi-square test values ​​of multiple consecutive frames do not have a preset chi-square critical value, then the error state of the sensor is updated by observation, and the updated error state is obtained. The method also includes: If the chi-square test value exceeds the preset chi-square threshold, the sensor's observation update will be interrupted.

[0070] For any given sensor, when the final health flag corresponding to the sensor is a health status flag and the number of health status flag frames does not exceed a preset number, a chi-square test is performed on the sensor's observation residuals to obtain the chi-square test value. The preset number is set according to actual needs and can be selected within the range of 3 to 10 frames, and is not limited here. For ease of understanding, in the embodiment of this application, the preset number is 5, that is, within 5 frames when the final health flag switches from 0 to 1, a chi-square test is performed on the sensor's observation residuals to obtain the chi-square test value. ,in, is the chi-square test value, i.e., the test statistic; T is the transpose; r is the observation residual; H is the observation matrix; R is the observation noise covariance matrix; and P is the error state covariance matrix.

[0071] The chi-square test value is compared with the preset chi-square critical value. If the chi-square test value exceeds the preset chi-square critical value, it is determined that the observation has not yet converged, and the sensor's observation update is interrupted. If the chi-square test values ​​of multiple consecutive frames do not exceed the preset chi-square critical value, the sensor's error state is updated to obtain the updated error state. Sensor recovery suppression is used to prevent interference with the error state caused by the residual not yet converging during the sensor's recovery from anomalies.

[0072] Example 2 Please see Figure 2 , Figure 2 This is a schematic diagram of a highly reliable aerial robot sensor failure navigation switching and recovery device according to an embodiment of the present invention. The aerial robot includes an inertial measurement unit and at least two sensors. Figure 2 The high-reliability aerial robot sensor failure navigation switching and recovery device 200 includes: The health detection module 210 is used to perform health detection on the output of each sensor and obtain a temporary health flag for each sensor. The health flag acquisition module 220 is used to construct a sliding window for the temporary health flag of each sensor. By comparing the number of consecutive frames of the temporary health flag within the sliding window with the length of the sliding window, the final health flag of each sensor is obtained. The sliding window is used to store the history of the temporary health flags of the sensor. The nominal state recursion module 230 is used to construct the error state Kalman filter. It continuously recursively recurses the nominal state through the output of the inertial measurement unit and predicts and propagates the covariance matrix. The error state Kalman filter includes the nominal state, the error state, and the covariance matrix of the error state. Error state update module 240 is used to update the error state of any sensor when the final health flag of the sensor is a healthy state flag, and obtain the updated error state. The update of the error state is skipped for the sensor whose final health flag is an abnormal state flag. The positioning state acquisition module 250 is used to compensate the updated error state to the nominal state, clear the error state, prevent external sources from resetting the nominal state, obtain the optimal estimated positioning state, and control the attitude and position of the aerial robot through the optimal estimated positioning state. The positioning state includes the position, velocity and attitude of the aerial robot.

[0073] This application also provides a computing device, including: The memory is configured to store instructions; The processor is configured to retrieve instructions from memory and, when executing instructions, implement the aforementioned highly reliable aerial robot sensor failure navigation switching and recovery method.

[0074] In this embodiment, the health detection module 210, the health mark acquisition module 220, the nominal state recursion module 230, the error state update module 240, and the positioning state acquisition module 250 are all stored as program units in the memory, and the processor executes the above program units stored in the memory to realize the corresponding functions.

[0075] The processor contains a kernel, which retrieves the corresponding program units from memory. One or more kernels can be configured, and the aforementioned high-reliability aerial robot sensor failure navigation switching and recovery method can be implemented by adjusting kernel parameters.

[0076] The memory may include non-permanent memory in computer-readable media, such as random access memory (RAM) and / or non-volatile memory, such as read-only memory (ROM) or flash RAM, and the memory includes at least one memory chip.

[0077] This application also provides an aerial robot, including an inertial measurement unit, at least two sensors, and the aforementioned computing device.

[0078] The aerial robot includes one inertial measurement unit and at least two sensors. It should be understood that the aerial robot also includes other structural devices, which are set according to actual needs and are not limited here. The aerial robot in this embodiment is suitable for scenarios such as power line inspection, indoor pipe gallery inspection, and emergency rescue.

[0079] This application also provides a machine-readable storage medium storing instructions that cause the machine to execute the above-described high-reliability aerial robot sensor failure navigation switching and recovery method.

[0080] Those skilled in the art will understand that embodiments of this application can be provided as methods, systems, or computer program products. Therefore, this application can take the form of a completely hardware embodiment, a completely software embodiment, or an embodiment combining software and hardware aspects. Furthermore, this application 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.

[0081] This application is described with reference to flowchart illustrations and / or block diagrams of methods, apparatus (systems), and computer program products according to embodiments of this application. 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... Figure 1 One or more processes and / or boxes Figure 1 A device that provides the functions specified in one or more boxes.

[0082] 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.

[0083] These computer program instructions may also be loaded onto a computer or other programmable data processing equipment to cause a series of operational steps to be performed on the computer or other programmable equipment to produce a computer-implemented process, thereby providing instructions that execute on the computer or other programmable equipment 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.

[0084] In a typical configuration, a computing device includes one or more processors (CPU), input / output interfaces, network interfaces, and memory.

[0085] Memory may include non-persistent memory in computer-readable media, such as random access memory (RAM) and / or non-volatile memory, such as read-only memory (ROM) or flash RAM. Memory is an example of computer-readable media.

[0086] Computer-readable media includes both permanent and non-permanent, removable and non-removable media that can store information using any method or technology. Information can be computer-readable instructions, data structures, modules of programs, or other data. Examples of computer storage media include, but are not limited to, phase-change memory (PRAM), static random access memory (SRAM), dynamic random access memory (DRAM), other types of random access memory (RAM), read-only memory (ROM), electrically erasable programmable read-only memory (EEPROM), flash memory or other memory technologies, CD-ROM, digital versatile optical disc (DVD) or other optical storage, magnetic tape, magnetic disk storage or other magnetic storage devices, or any other non-transferable medium that can be used to store information accessible by a computing device. As defined herein, computer-readable media does not include transient computer-readable media, such as modulated data signals and carrier waves.

[0087] It should also be noted that 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 process, method, article, or apparatus. Unless otherwise specified, 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 that element.

[0088] The above are merely embodiments of this application and are not intended to limit the scope 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 the claims of this application.

Claims

1. A highly reliable method for navigation switching and recovery in the event of sensor failure in an aerial robot, characterized in that, The aerial robot includes an inertial measurement unit and at least two sensors, and the method includes: Perform health checks on the output of each sensor to obtain a temporary health flag for each sensor; A sliding window is constructed for the temporary health flag of each sensor. The final health flag of each sensor is obtained by comparing the number of consecutive frames of the temporary health flag within the sliding window with the length of the sliding window. The sliding window is used to store the history of the temporary health flag of the sensor. An error state Kalman filter is constructed, which continuously recursively calculates the nominal state through the output of the inertial measurement unit and predicts and propagates the covariance matrix of the error state. For any sensor, if the final health flag corresponding to the sensor is a health status flag, the error status of the sensor is observed and updated to obtain the updated error status. The updated error state is compensated to the nominal state, and the error state is cleared. External sources are prohibited from resetting the nominal state to obtain the optimal estimated positioning state. The attitude and position of the aerial robot are controlled by the optimal estimated positioning state, wherein the positioning state includes the position, speed and attitude of the aerial robot.

2. The highly reliable aerial robot sensor failure navigation switching and recovery method according to claim 1, characterized in that, The method further includes: If the duration of the final health flag corresponding to each sensor being an abnormal state flag exceeds the first duration threshold, an independent hovering state command is triggered, and the altitude reference value corresponding to the moment the independent hovering state command is triggered is latched. A virtual damping term is applied to the output velocity, and the horizontal attitude of the aerial robot is maintained by the attitude estimation of the inertial measurement unit. The aerial robot's altitude is maintained in an open loop by integrating the altitude reference value with the vertical direction of the accelerometer of the inertial measurement unit.

3. The highly reliable aerial robot sensor failure navigation switching and recovery method according to claim 2, characterized in that, The method further includes: A health check is performed on the inertial measurement unit to obtain a health indicator for the inertial measurement unit; If the duration of the abnormal status indicator of the inertial measurement unit exceeds the second duration threshold and an independent hovering state command is triggered, a runaway protection logic command is triggered to control the aerial robot to make an emergency landing.

4. The highly reliable aerial robot sensor failure navigation switching and recovery method according to claim 1, characterized in that, The health detection includes data integrity anomaly detection, data quality anomaly detection, and data activity anomaly detection. The nominal state includes the three-dimensional position of the aerial robot, the three-dimensional velocity of the aerial robot, the four-dimensional attitude quaternion of the aerial robot, and the zero bias of the six-dimensional inertial measurement unit; The error states include the three-dimensional position error of the aerial robot, the three-dimensional velocity error of the aerial robot, the four-dimensional attitude quaternion error of the aerial robot, and the zero bias error of the six-dimensional inertial measurement unit.

5. The highly reliable aerial robot sensor fault navigation switching and recovery method according to claim 1, characterized in that, The final health flag for each sensor is obtained by comparing the number of consecutive frames of the temporary health flag within the sliding window with the length of the sliding window, including: If the first ratio is greater than or equal to the first ratio threshold, the final health flag is switched to a health status flag, wherein the first ratio is the ratio between the number of frames in which the temporary health flag is continuously a health status flag within the sliding window and the length of the sliding window. If the second ratio is greater than or equal to the second ratio threshold, the final health flag is switched to the abnormal state flag, wherein the first ratio threshold is greater than the second ratio threshold, and the second ratio is the ratio between the number of frames in which the temporary health flag is continuously an abnormal state flag within the sliding window and the length of the sliding window.

6. The highly reliable aerial robot sensor fault navigation switching and recovery method according to claim 1, characterized in that, The at least two sensors include at least one lidar and at least one vision acquisition device; For any given sensor, when the final health flag corresponding to the sensor is a health status flag, the error status of the sensor is observed and updated to obtain the updated error status, including: When the final health flag corresponding to the lidar is a health status flag, the lidar is updated with laser observations to determine its error status based on the distance residual from the point cloud to the line and the distance residual from the point cloud to the surface, thus obtaining the updated error status. When the final health indicator corresponding to the visual acquisition device is a health status indicator, the visual observation of the error status of the visual acquisition device is updated based on the feature point reprojection residual, and the updated error status is obtained.

7. The highly reliable aerial robot sensor fault navigation switching and recovery method according to claim 1, characterized in that, The method further includes: For any sensor, if the final health flag corresponding to the sensor is an abnormal flag, skip the observation update of the sensor's error state and obtain the duration of skipping the observation update. If the duration of all sensors skipping observation updates exceeds the third duration threshold, upper limit saturation processing is performed on the covariance matrix, or prediction propagation of the covariance matrix is ​​stopped. The upper limit saturation processing is to restrict each diagonal element of the covariance matrix to within a preset upper limit of covariance.

8. The highly reliable aerial robot sensor fault navigation switching and recovery method according to claim 1, characterized in that, For any given sensor, when the final health flag corresponding to the sensor is a health status flag, the error status of the sensor is observed and updated to obtain the updated error status, including: For any sensor, if the final health flag corresponding to the sensor is a health status flag and the number of health status flag frames does not exceed a preset number, perform a chi-square test on the observation residual of the sensor to obtain the chi-square test value. If the chi-square test values ​​of multiple consecutive frames do not have a preset chi-square critical value, then the error state of the sensor is observed and updated to obtain the updated error state. The method further includes: If the chi-square test value exceeds the preset chi-square threshold, the sensor's observation update will be interrupted.

9. A computing device, characterized in that, include The memory is configured to store instructions; The processor is configured to retrieve the instructions from the memory and, when executing the instructions, to implement the highly reliable aerial robot sensor failure navigation switching and recovery method according to any one of claims 1 to 8.

10. A machine-readable storage medium, characterized in that, The machine-readable storage medium stores instructions for causing the machine to perform the high-reliability aerial robot sensor failure navigation switching and recovery method according to any one of claims 1 to 8.

Images