State estimation for aerial vehicles using multi-sensor fusion

• US 10,295,365 B2
• Filed: 07/31/2017
• Issued: 05/21/2019
• Est. Priority Date: 07/29/2016
• Status: Active Grant

• Current Assignee: Carnegie Mellon University
• Original Assignee: Carnegie Mellon University
• Inventors: Scherer, Sebastian, Yu, Song, Nuske, Stephen
• Primary Examiner(s): Cheung, Calvin
• Application Number:
  US15/664,073
• Publication Number:
  US 20180031387A1
• Time in Patent Office: 659 Days
• CPC Class Codes: B64C 39/024 of the remote controlled vehicle type, i.e. RPV
• CPC Class Codes: G01C 21/1656 with passive imaging devices, e.g. cameras
• CPC Class Codes: G01C 23/00 Combined instruments indicating more than one navigational value, e.g. for aircraft; Combined measuring devices for measuring two or more variables of movement, e.g. distance, speed or acceleration
• CPC Class Codes: G01C 5/06 by using barometric means
• CPC Class Codes: G01S 19/40 Correcting position, velocity or attitude
• CPC Class Codes: G01S 19/48 by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system
• CPC Class Codes: G01S 19/53 Determining attitude
• CPC Class Codes: G06T 2207/10021 Stereoscopic video; Stereoscopic image sequence
• CPC Class Codes: G06T 2207/10032 Satellite or aerial image; Remote sensing
• CPC Class Codes: G06T 2207/30244 Camera pose
• CPC Class Codes: G06T 7/248 involving reference images or patches
• CPC Class Codes: G06T 7/277 involving stochastic approaches, e.g. using Kalman filters
• CPC Class Codes: G06T 7/285 using a sequence of stereo image pairs
• CPC Class Codes: G06T 7/292 Multi-camera tracking
• CPC Class Codes: G06T 7/579 from motion
• CPC Class Codes: G06T 7/593 from stereo images
• CPC Class Codes: G06T 7/74 involving reference images or patches
• CPC Class Codes: H04N 13/239 using two 2D image sensors having a relative position equal to or related to the interocular distance H04N13/243 takes precedence
• CPC Class Codes: H04N 13/282 for generating image signals corresponding to three or more geometrical viewpoints, e.g. multi-view systems

Abstract

A state estimation system that utilizes long-range stereo visual odometry that can degrade to a monocular system at high-altitude, and integrates GPS, Barometer and IMU measurements. The system has two main parts: An EKF that is loosely fused and a long-range visual odometry part. For visual odometry, the system takes the EKF information for robust camera pose tracking, and the visual odometry outputs will be the measurement for EKF state update.

Claims

1. A system for estimating the state of an aerial vehicle comprising:

one or more relative sensors, including at least an inertial measurement unit and a visual odometry unit;

one or more absolute sensors; and

a processor, running software for performing the functions of;

keeping a current state and current state covariance, the current state including at least a current position, a current orientation, a current velocity, a delayed position and a delayed orientation, the delayed position and delayed orientation being based on visual odometry from a previous current state;

predicting an update of the current state and an update of the current state covariance based on an integration of a reading from the inertial measurement unit;

receiving visual odometry, updating the delayed position and orientation with the current position and orientation, updating the current position and orientation with the visual odometry;

receiving state information from an absolute sensor, updating the current state with the state information and covariance from the absolute sensor;

recalculating the covariance of the current state to give readings from the relative and absolute sensors a weight in the estimated state of the vehicle; and

repeating the functions performed by the software.

2. The system of claim 1 wherein updating the current state and covariance is performed by an extended Kalman filter.

3. The system of claim 2 wherein the current state includes covariances of each element included in the current state.

4. The system of claim 3 wherein the one or more absolute sensors are selected from a group consisting of a global positioning system and a barometer.

5. The system of claim 3 wherein the software performs the further functions of:

calculating an uncertainty factor between the covariance of the current position and orientation and the covariance of the delayed position and orientation; and

adjusting the visual odometry covariance based on the uncertainty factor.

6. The system of claim 3 wherein the function of receiving visual odometry includes:

updating the covariance of the visual odometry;

verifying that the covariance of the visual odometry is lower than the covariance of the predicted updated state; and

verifying that the covariance of the visual odometry is greater than a previous error covariance.

7. The system of claim 3 wherein the function of receiving state information from an absolute sensor includes:

verifying that the updated covariance of the position, orientation, delayed position and the delayed orientation have decreased with respect to their respective covariances prior to the receiving of state information from the absolute sensor; and

verifying that the covariances of the current position and current orientation are higher than the covariances for the delayed position and delayed orientation.

8. The system of claim 1 further comprising:

for the visual odometry unit, determining, based on the current stereo baseline-depth ratio, that short range stereo mode is no longer viable;

switching the visual odometry unit to monocular mode;

maintaining a local map consisting of 3D sparse map points generated by sparsely selected key-frames, the selected key-frames providing sufficient relative motion between the frames for long-range triangulation;

identifying new features visible in multiple selected key-frames and performing triangulation using a dynamic pseudo baseline formed by the relative pose of the features between neighboring key-frames.

9. The system of claim 8 further comprising:

for new features that cannot be triangulated, inserting those features into a candidate queue; and

iteratively refining the feature depth in the subsequent key-frames by tracking stereo information with a multi-view inverse depth filter wherein the feature will be added to the map and used for camera pose tracking if the inverse depth variance is smaller than a given threshold.

10. The system of claim 9 wherein, for each subsequent key-frame, the inverse depth observation distribution for the feature is calculated from the tracking frame static stereo matching or obtained by the dynamic pseudo baseline formed by the motion between the current tracking frame and a reference key-frame.

11. The system of claim 8 wherein the integrated readings from the inertial measurement unit are used by the visual odometry unit for pose tracking between the selected key-frames.

12. A method for estimating the state of an aerial vehicle comprising:

keeping a current state and current state covariance, the current state including at least a current position, a current orientation, a current velocity, a delayed position and a delayed orientation, the delayed position and delayed orientation being based on visual odometry from a previous current state;

predicting an update of the current state and an update of the current state covariance based on an integration of a reading from an inertial measurement unit;

receiving visual odometry from a visual odometry unit, updating the delayed position and orientation with the current position and orientation, updating the current position and orientation with the visual odometry and recalculating the covariance of the current state;

receiving state information from an absolute sensor, updating the current state and with the state information and covariance from the absolute sensor and recalculating the covariance of the current state; and

repeating the functions performed by the software.

13. The system of claim 12 wherein updating the current state and covariance is performed by an extended Kalman filter.

14. The system of claim 13 wherein the current state includes covariances of each element included in the current state.

15. The system of claim 13 wherein the one or more absolute sensors are selected from a group consisting of a global positioning system and a barometer.

16. The system of claim 13 wherein the software performs the further functions of:

calculating an uncertainty factor between the covariance of the current position and orientation and the covariance of the delayed position and orientation; and

adjusting the visual odometry covariance based on the uncertainty factor.

17. The system of claim 13 wherein the function of receiving visual odometry includes:

updating the covariance of the visual odometry;

verifying that the covariance of the visual odometry is lower than the covariance of the predicted updated state; and

verifying that the covariance of the visual odometry is greater than a previous error covariance.

18. The system of claim 13 wherein the function of receiving state information from an absolute sensor includes:

verifying that the updated covariance of the position, orientation, delayed position and the delayed orientation have decreased with respect to their respective covariances prior to the receiving of state information from the absolute sensor; and

verifying that the covariances of the current position and current orientation are higher than the covariances for the delayed position and delayed orientation.