Scale-aware navigation of a low-cost quadrocopter with a monocular camera

We present a complete solution for the visual navigation of a small-scale, low-cost quadrocopter in unknown environments. Our approach relies solely on a monocular camera as the main sensor, and therefore does not need external tracking aids such as GPS or visual markers. Costly computations are carried out on an external laptop that communicates over wireless LAN with the quadrocopter. Our approach consists of three components: a monocular SLAM system, an extended Kalman filter for data fusion, and a PID controller. In this paper, we (1) propose a simple, yet effective method to compensate for large delays in the control loop using an accurate model of the quadrocopter’s flight dynamics, and (2) present a novel, closed-form method to estimate the scale of a monocular SLAM system from additional metric sensors. We extensively evaluated our system in terms of pose estimation accuracy, flight accuracy, and flight agility using an external motion capture system. Furthermore, we compared the convergence and accuracy of our scale estimation method for an ultrasound altimeter and an air pressure sensor with filtering-based approaches. The complete system is available as open-source in ROS. This software can be used directly with a low-cost, off-the-shelf Parrot AR.Drone quadrocopter, and hence serves as an ideal basis for follow-up research projects.     

Bearing-only visual SLAM for small unmanned aerial vehicles in GPS-denied environments

This paper presents a hierarchical simultaneous localization and mapping(SLAM) system for a small unmanned aerial vehicle(UAV) using the output of an inertial measurement unit(IMU) and the bearing-only observations from an onboard monocular camera.A homography based approach is used to calculate the motion of the vehicle in 6 degrees of freedom by image feature match.This visual measurement is fused with the inertial outputs by an indirect extended Kalman filter(EKF) for attitude and velocity estimation.Then,another EKF is employed to estimate the position of the vehicle and the locations of the features in the map.Both simulations and experiments are carried out to test the performance of the proposed system.The result of the comparison with the referential global positioning system/inertial navigation system(GPS/INS) navigation indicates that the proposed SLAM can provide reliable and stable state estimation for small UAVs in GPS-denied environments.     

基于惯性/磁力传感器与单目视觉融合的SLAM方法

针对单目视觉SLAM（同时定位与地图构建）算法没有尺度信息以及在相机移动过快时无法使用的问题,提出了一种IMU（惯性测量单元）!!/磁力传感器与单目视觉融合的SLAM方法．首先,提出了一种模糊自适应的九轴姿态融合算法,对IMU的航向角进行高精度估计．然后,采用单目ORB-SLAM2（oriented FAST and rotated BRIEF SLAM2）算法,通过IMU估计其尺度因子,并对其输出的位姿信息进行尺度转换．最后,采用松耦合方式,对IMU估计的位姿和ORB-SLAM2算法经过尺度转换后的位姿,进行卡尔曼滤波融合．在公开数据集EuRoC上进行了测试,测试结果表明本文方法总的位置均方根误差为5.73 cm．为了进一步在实际环境中验证,设计了全向移动平台,以平台上激光雷达所测的位姿数据为基准,测试结果表明本文方法的旋转角度误差小于5°,总的位置均方根误差为9.76 cm．     

Joint ego-motion and road geometry estimation

We provide a sensor fusion framework for solving the problem of joint ego-motion and road geometry estimation. More specifically we employ a sensor fusion framework to make systematic use of the measurements from a forward looking radar and camera, steering wheel angle sensor, wheel speed sensors and inertial sensors to compute good estimates of the road geometry and the motion of the ego vehicle on this road. In order to solve this problem we derive dynamical models for the ego vehicle, the road and the leading vehicles. The main difference to existing approaches is that we make use of a new dynamic model for the road. An extended Kalman filter is used to fuse data and to filter measurements from the camera in order to improve the road geometry estimate. The proposed solution has been tested and compared to existing algorithms for this problem, using measurements from authentic traffic environments on public roads in Sweden. The results clearly indicate that the proposed method provides better estimates.     

一种机器人未知环境下动态目标跟踪交互多模滤波算法

为了解决机器人同时定位、地图构建和目标跟踪问题,提出了一种基于交互多模滤波(interacting multiple model filter, IMM)的方法.该方法将机器人状态、目标状态和环境特征状态作为整体来构成系统状态向量并利用全关联扩展式卡尔曼滤波算法对系统状态进行估计,由此随着迭代估计的进行,系统各对象状态之间将产生足够的相关性,这种相关性能够正确反映各对象状态估计间的依赖关系,因此提高了目标跟踪的准确性.该方法进一步和传统的IMM滤波算法相结合,从而解决了目标运动模式未知性问题,IMM方法的采用使系统在完成目标追踪的同时还能对其运动模态进行估计,进而提高了该算法对于机动目标的跟踪能力.仿真实验验证了该方法对机器人和目标的运动轨迹以及目标运动模态进行估计的准确性和有效性.     

适用于户外增强现实系统的混合跟踪定位算法

单一传感器无法解决户外增强现实系统中的跟踪定位问题.为了提高视觉跟踪定位算法的精度和鲁棒性,提出一种基于惯性跟踪器与视觉测量相结合的混合跟踪定位算法.该算法在扩展卡尔曼滤波框架下,通过融合来自视觉与惯性传感器的信息进行摄像机运动轨迹估计,并利用视觉测量信息对惯性传感器的零点偏差进行实时校正;同时采用SCAAT方法解决惯性传感器与视觉测量间的时间采样不同步问题.实验结果表明,该算法能够有效地提高运动估计的精度和稳定性.     

基于2点RANSAC的无人直升机单目视觉SLAM

1点随机抽样一致性(RANSAC)算法是一种准确度高、计算量小的数据关联算法,但是其在摄像机多个轴上的角速度都快速变化时会失效,用在以无人直升机为载体的单目视觉同步定位与地图构建(SLAM)上存在滤波发散的风险.针对该问题,提出2点RANSAC算法,结合EKF运动模型的先验信息,用只抽样2个匹配点的RANSAC去除野点.在微小型无人直升机平台上进行了基于2点RANSAC算法的单目视觉SLAM实验,实验结果表明2点RANSAC算法工作可靠,SLAM的位姿估计精度可以达到自主飞行需要.     

Adaptive extended Kalman filtering for visual motion estimation of 3D objects

An algorithm for the real-time estimation of the position and orientation of a moving object of known geometry is presented in this paper. An estimation algorithm is adopted where a discrete-time extended Kalman filter computes the object pose on the basis of visual measurements of the object features. The scheme takes advantage of the prediction capability of the extended Kalman filter for the pre-selection of the features to be extracted from the image at each sample time. To enhance the robustness of the algorithm with respect to measurement noise and modelling error, an adaptive version of the extended Kalman filter, customized for visual applications, is proposed. Experimental results on a fixed single-camera visual system are presented to test the performance and the feasibility of the proposed approach.     

Experimental Study of Iterated Kalman Filters for Simultaneous Localization and Mapping of Autonomous Mobile Robots

In this paper, we investigate the role of iteration in Kalman filters family for improvement of the estimation accuracy of states in simultaneous localization and mapping (SLAM). The linearized error propagation existing in Kalman filters family can result in large errors and inconsistency in the SLAM problem. One approach to alleviate this situation is the use of iteration in extended Kalman filter (EKF) and sigma point Kalman filter (SPKF) based SLAM. The main contribution is to present that the iterated versions of Kalman filters can increase consistency and robustness of these filters against linear error propagation. Experimental results are presented to validate this improvement of state estimate convergence through repetitive linearization of the nonlinear observation model in EKF-SLAM and SPKF-SLAM algorithms.     

视觉与惯性传感器融合的隐式卡尔曼滤波位置估计算法

机器人对自身位置的实时感知在机器人技术中非常重要.本文主要研究机器人技术中一类基于视觉与惯性传感器的位置估计问题.与传统的状态估计问题不同的是,所研究位置估计问题为带有隐式观测方程的线性状态估计问题.为此提出一种能够解决此类估计问题的隐式卡尔曼滤波器,并给出了详细的滤波器设计过程.另外采用扩展变量法将加速度信息中的偏移量作为滤波器状态来估计,以补偿其对位置估计结果的影响.仿真结果显示,所给出的隐式卡尔曼滤波器收敛,加速度偏移带来的影响被有效的补偿.     

Inertial-Aided Metric States and Surface Normal Estimation using a Monocular Camera

In this paper, a visual inertial fusion framework is proposed for estimating the metric states of a Micro Aerial Vehicle (MAV) using optic flow (OF) and a homography model. Aided by the attitude estimation from the on-board Inertial Measurement Unit (IMU), the computed homography matrix is reshaped into a vector and directly fed into an Extend Kalman Filter (EKF). The sensor fusion method is able to recover metric distance, speed, acceleration bias and surface normal of the observed plane. We further consider reducing the size of the filter by using only part of the homography matrix as the system observation. Simulation results show that these smaller filters have reduced observability compared with the filter using the complete homography matrix, however it is still possible to estimate the metric states as long as one of the axes is linearly excited. Experiments using real sensory data show that our method is superior to the homography decomposition method for state and slope estimation. The proposed method is also validated in closed-loop flight tests of a quadrotor.     

纯方位角目标跟踪及移动平台可观性控制方法

为了解决未知环境下的单目视觉移动机器人目标跟踪问题,提出了一种将目标状态估计与机器人可观性控制相结合的机器人同时定位、地图构建与目标跟踪方法。在状态估计方面,以机器人单目视觉同时定位与地图构建为基础,设计了扩展式卡尔曼滤波框架下的目标跟踪算法;在机器人可观性控制方面,设计了基于目标协方差阵更新最大化的优化控制方法。该方法能够实现机器人在单目视觉条件下对自身状态、环境状态、目标状态的同步估计以及目标跟随。仿真和原型样机实验验证了目标状态估计和机器人控制之间的耦合关系,证明了方法的准确性和有效性,结果表明:机器人将产生螺旋状机动运动轨迹,同时,目标跟踪和机器人定位精度与机器人机动能力成正比例关系。     

A Robust Solution to the Stereo-Vision-Based Simultaneous Localization and Mapping Problem with Steady and Moving Landmarks

The problem of visual simultaneous localization and mapping (SLAM) is examined in this paper using recently developed ideas and algorithms from modern robust control and estimation theory. A nonlinear model for a stereo-vision-based sensor is derived that leads to nonlinear measurements of the landmark coordinates along with optical flow-based measurements of the relative robot–landmark velocity. Using a novel analytical measurement transformation, the nonlinear SLAM problem is converted into the linear domain and solved using a robust linear filter. Actually, the linear filter is guaranteed stable and the SLAM state estimation error is bounded within an ellipsoidal set. A mathematically rigorous stability proof is given that holds true even when the landmarks move in accordance with an unknown control input. No similar results are available for the commonly employed extended Kalman filter, which is known to exhibit divergence and inconsistency characteristics in practice. A number of illustrative examples are given using both simulated and real vision data that further validate the proposed method.     

基于部分惯性传感器信息的单目视觉同步定位与地图创建方法

传统的单目视觉同步定位与地图创建(MonoSLAM)方法很难处理累积误差问题,如何有效地利用惯性传感器输出的运动信息辅助SLAM系统抑制累积误差是MonoSLAM研究中的一项重要内容.由于惯性传感器输出的三轴方向角中横滚角和俯仰角的精度较高,而偏航角的精度相对较低,如果在SLAM系统中直接使用惯性传感器输出的偏航角信息不但无法有效地抑制该系统中的累积误差,反而会进一步增大系统误差、降低SLAM系统的稳定性.针对这种情况,提出一种基于惯性传感器横滚角和俯仰角的MonoSLAM方法.首先利用惯性传感器输出的横滚角和俯仰角进行系统标定;然后将惯性传感器自身的偏航角作为系统状态向量的一个分量,利用扩展卡尔曼滤波器实时地估计状态向量,进而实现实时鲁棒的同步定位和地图创建.实验结果表明,该方法可以有效地抑制SLAM系统运行过程中产生的累积误差,并降低惯性传感器测量误差对SLAM系统稳定性的影响.     

Multi-Sensory Motion Estimation and Control of an Autonomous Quadrotor

In this paper, a fully autonomous quadrotor in a heterogeneous air–ground multi-robot system is established only using minimal on-board sensors: a monocular camera and inertial measurement units (IMUs). Efficient pose and motion estimation is proposed and optimized. A continuous-discrete extended Kalman filter is applied, in which the high-frequency IMU data drive the prediction, while the estimates are corrected by the accurate and steady vision data. A high-frequency fusion at 100 Hz is achieved. Moreover, time delay analysis and data synchronizations are conducted to further improve the pose/motion estimation of the quadrotor. The complete on-board implementation of sensor data processing and control algorithms reduces the influence of data transfer time delay, enables autonomous task accomplishment and extends the work space. Higher pose estimation accuracy and smaller control errors compared to the standard works are achieved in real-time hovering and tracking experiments.     

Multi-sensor based real-time 6-DoF pose tracking for wearable augmented reality

Wearable augmented reality (WAR) combines a live view of a real scene with computer-generated graphic on resource-limited platforms. One of the crucial technologies for WAR is a real-time 6-DoF pose tracking, facilitating registration of virtual components within in a real scene. Generally, artificial markers are typically applied to provide pose tracking for WAR applications. However, these marker-based methods suffer from marker occlusions or large viewpoint changes. Thus, a multi-sensor based tracking approach is applied in this paper, and it can perform real-time 6-DoF pose tracking with real-time scale estimation for WAR on a consumer smartphone. By combining a wide-angle monocular camera and an inertial sensor, a more robust 6-DoF motion tracking is demonstrated with the mutual compensations of the heterogeneous sensors. Moreover, with the help of the depth sensor, the scale initialization of the monocular tracking is addressed, where the initial scale is propagated within the subsequent sensor-fusion process, alleviating the scale drift in traditional monocular tracking approaches. In addition, a sliding-window based Kalman filter framework is used to provide a low jitter pose tracking for WAR. Finally, experiments are carried out to demonstrate the feasibility and robustness of the proposed tracking method for WAR applications.     

Complementing visual tracking of moving targets by fusion of tactile sensing

Robot control in uncertain and dynamic environments can be greatly improved using sensor-based control. Vision is a versatile low-cost sensory modality, but low sample rate, high sensor delay and uncertain measurements limit its usability, especially in strongly dynamic environments. Vision can be used to estimate a 6-DOF pose of an object by model-based pose-estimation methods, but the estimate is typically not accurate along all degrees of freedom. Force is a complementary sensory modality allowing accurate measurements of local object shape when a tooltip is in contact with the object. In multimodal sensor fusion, several sensors measuring different modalities are combined together to give a more accurate estimate of the environment. As force and vision are fundamentally different sensory modalities not sharing a common representation, combining the information from these sensors is not straightforward. We show that the fusion of tactile and visual measurements enables to estimate the pose of a moving target at high rate and accuracy. Making assumptions of the object shape and carefully modeling the uncertainties of the sensors, the measurements can be fused together in an extended Kalman filter. Experimental results show greatly improved pose estimates with the proposed sensor fusion.     

Hybrid three-dimensional camera pose estimation using particle filter sensor fusion

To properly align objects in the real and virtual worlds in an augmented reality (AR) space it is essential to keep tracking the camera's exact three-dimensional position and orientation (camera pose). State-of-the-art analysis shows that traditional vision-based or inertial sensor-based solutions are not adequate when used individually. Sensor fusion for hybrid tracking has become an active research direction during the past few years, although how to do it in a robust and principled way is still an open problem. In this paper, we develop a hybrid camera pose-tracking system that combines vision and inertial sensor technologies. We propose to use the particle filter framework for the sensor fusion system. Particle filters are sequential Monte-Carlo methods based upon a point mass (or 'particle') representation of probability densities, which can be applied to any state space model and which generalize the traditional Kalman filtering methods. We have tested our algorithm to evaluate its performance and have compared the results obtained by the particle filter with those given by a classical extended Kalman filter. Experimental results are presented     

基于松组合的视觉惯性SLAM方法

研究了一种基于松组合的视觉惯性即时定位与同步构图(SLAM)方法。针对视觉特征点匹配率低问题,研究基于ORB(Oriented FAST and Rotated BRIEF)特征点的提取方法;基于ORB-SLAM的输出,结合SINS提出了一种具有回环检测功能的SLAM/SINS组合方法。利用最小二乘法估计视觉SLAM算法的尺度因子;构建SLAM/SINS的非线性卡尔曼滤波器,将视觉SLAM系统输出的位置信息经过尺度变换后作为观测量进行卡尔曼滤波,修正惯导的误差。最后利用标准数据集证明与开源的SLAM算法进行对比,结果表明,所提出的算法有比较高的定位精度,并且在移动设备上开发了增强现实软件,以增强现实为实验手段验证在较大的空间范围和环境干扰下,这种组合方法具备较好的漂移消除能力。     

紧耦合多传感器混合跟踪算法

在增强现实应用中实现对运动目标的准确跟踪是一个具有挑战性的任务。基于混合跟踪通过对多传感器信息的融合通常比单一传感器跟踪算法更为优越的特性,提出了一种新的紧耦合混合跟踪算法实现视觉与惯性传感器信息的实时融合。该算法基于多频率的测量数据同步,通过强跟踪滤波器引入时变衰减因子自适应调整滤波预测误差协方差,实现对运动目标位置数据的准确估计。通过标示物被遮挡状态下的跟踪实验结果表明,该方法能有效改善基于扩展卡尔曼滤波器的混合跟踪算法对运动目标位置信息预测估计的准确性,提高跟踪快速移动目标的稳定性,适用于大范围移动条件下的增强现实系统。