Performance analysis of an INS/SLAM integrated system with respect to the geometrical arrangement of multiple vision sensors

A single vision system is the simplest vision-based method of simultaneous localization and mapping (SLAM) but it cannot reduce navigation errors that occur in the direction of the optical axis of vision sensors. Moreover, the limited observation area of vision sensors sometimes leads to failure in tracking a feature point. A vision sensor-based inertial navigation system INS/SLAM integrated system performs better with multiple vision sensors than with a single vision sensor because the observation area is wider and the impact of the sensitivity of the vision sensors is considerably smaller. However, the geometrical arrangement of multiple vision sensors induces navigation errors. This paper analyzes how the performance of the vision sensor-based INS/SLAM integrated system varies in relation to the geometrical arrangement of multiple vision sensors and the observation area of the vision sensors. The analysis shows that vertical navigation errors decline when the vision sensors are aimed horizontally, and that horizontal navigation errors decline in a similar manner when the vision sensor is aimed vertically; moreover, this behavior is especially evident when the level of sensitivity peaks in the horizontal direction.     

基于结构分解的GPS/INS组合导航粒子滤波技术

针对粒子滤波应用于GPS/INS组合导航系统时难以保证滤波实时性的问题,提出一种基于线性/非线性结构分解的改进粒子滤波算法.改进算法对状态方程进行线性/非线性结构分解,分别采用重点采样和线性卡尔曼方式进行一步预测递推,充分发挥粒子滤波和卡尔曼滤波的特点,有效降低了粒子滤波的计算量,在保证GPS/INS组合导航系统滤波精度的条件下提高了组合滤波的实时性.     

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.     

Sigma-Point直接式卡尔曼滤波惯性组合导航算法

基于西格玛点采样加权的方法,以姿态、速度和位置等9个导航参数为状态向量,以卫星导航系统的速度和位置组成6维观测向量,构建直接式卡尔曼滤波器,融惯性导航系统求解和状态估计的过程为一体,直接描述系统导航参数动态过程.仿真结果验证了惯性组合导航Sigma-Point直接式滤波方法的有效性,表明该非线性直接式滤波方法可提高惯性组合导航系统的导航精度和对飞机、导弹等载体非线性机动过程的适应性.     

多传感器分布式信息融合粒子滤波器

针对非线性非Gaussian系统的状态估计问题,提出一种基于信息融合的多传感器分布式粒子滤波算法。该算法首先利用粒子滤波方法分别计算局部传感器的状态估值,再应用分布式标量加权融合准则对状态估值进行信息融合。仿真结果表明和单传感器情形相比可提高滤波的精度。     

基于中心差分粒子滤波的SLAM算法

针对移动机器人同时定位与地图创建(Simultaneous localization and mapping, SLAM)中的FastSLAM算法, 存在非线性系统线性化处理和计算雅可比矩阵的缺点, 本文提出了基于Sterling多项式插值处理非线性系统的SLAM方法. 该方法基于Rao-Blackwellized粒子滤波框架, 利用中心差分滤波方法产生改进的建议分布函数, 提高了机器人位姿估计的精度; 利用中心差分滤波初始化特征和更新地图中的特征, 提高了地图创建的精度; 针对实际应用中存在虚假特征的情况 提出了一种有效的地图管理方法. 在同等粒子数的情况下, 该方法改进了SLAM结果的精度. 基于仿真和实际数据的实验结果验证了该方法的有效性.     

基于分布式粒子滤波的二进制无线传感器网络目标跟踪

为了提高二进制无线传感器网络跟踪算法的精度和实时性,降低传感器节点能耗,将分布式粒子滤波运用到二进制无线传感器网络中进行目标跟踪。选择信号强度最大的节点作为簇头节点,在簇头单跳通信范围内的所有节点和簇头组成对目标跟踪的动态分簇,在簇头节点进行粒子采样和状态估计,在簇头之间传递粒子及其权值,从而得到了二进制无线传感器网络的分布式粒子滤波跟踪算法。研究了粒子数和网络节点数量对跟踪精度的影响。仿真结果表明,传感器的节点数量会影 响跟踪精度,但是粒子数对跟踪精度的影响更大。同时分布式粒子滤波比集中式粒子滤波具有更好的实时性和更低的能耗。     

二阶中心差分粒子滤波FastSLAM算法

为改善SLAM算法中非线性系统状态估计精度不高,计算繁杂的问题,本文创新性地提出了基于二阶中心差分滤波并融合最新观测数据来产生建议分布函数的新算法。新算法基于二阶sterling插值公式处理SLAM中的非线性系统问题,无须计算雅可比矩阵,容易实现。此外,该算法使用Cholesky分解技术,在SLAM概率估计中直接依据协方差平方根因子进行传播,保证协方差矩阵正定性的同时减小了局部线性化的截断误差。仿真试验表明,在粒子数相同的情况下,二阶中心差分FastSLAM(SOFastSLAM)在不同噪声条件下的估计精度均优于FastSLAM2.0、UFastSLAM算法,且用时最少,证实了SOFastSLAM算法的优越性。     

带有色观测噪声多传感器多重时滞系统分布式融合滤波器

基于新息分析方法, 对带有色观测噪声的多重时滞系统, 提出了一种带白噪声估值器的非增广的最优滤波器. 它等价于一个带相关白噪声多重时滞系统的一步预报器. 当系统带有多个传感器时, 推导了多重时滞系统的任意两个传感器子系统之间的估计误差互协方差阵. 基于线性最小方差最优加权融合估计算法, 给出了分布式加权融合最优滤波器. 分布式融合估计比基于每个传感器的局部估计具有更高的精度. 比增广的集中式最优滤波器具有更好的可靠性, 且避免了高维计算和大存储空间. 仿真例子验证了其有效性.     

Approximate nonlinear filtering and its application in navigation

In this paper, we introduce for the first time particle filtering for an exponential family of densities. We prove that under certain conditions the approximated conditional density of the state converges to the true conditional density. In the realistic setting where the conditional density does not lie in an exponential family but stays close to it, we show that under certain assumptions the error of the estimate given by an approximate nonlinear filter (which we call the projection particle filter), is bounded. We use projection particle filtering in state estimation for a combination of inertial navigation system (INS) and global positioning system (GPS), referred to as integrated INS/GPS. We illustrate via numerical experiments that projection particle filtering outperforms regular particle filtering in navigation performance, and extended Kalman filter as well when satellite loss-of-lock occurs.     

基于粒子滤波的无线传感器网络目标跟踪算法

传感器节点的组织和路由对无线传感器网络(WSN)目标跟踪算法的性能有重大影响.为此,针对具有簇一树型网络拓扑结构的WSN,首先给出集中式粒子滤波跟踪算法(CPFTA)实现的具体步骤,然后提出一种分布式粒子滤波跟踪算法(DPFTA),构建性能评价体系,通过仿真实验给出两种跟踪算法的定量比较,结果表明DPFTA的跟踪精度稍低于CPFTA,但能大幅度减少通信开销,而且具有更小的跟踪反应时间;最后仿真分析了传感器覆盖密度和检测周值对跟踪算法性能的影响.     

无线传感器网络中基于量化观测的粒子滤波状态估计

无线传感器网络环境下处理分布式状态估计问题,由于网络中的带宽限制,减少通信成本是非常重要的一个环节,需要将观测值量化后再传送.针对非线性系统的状态滤波问题,本文提出了一种基于量化观测的粒子滤波状态估计算法,并阐述了基于量化观测的状态估计过程.文中分别采用基于均匀量化(UQDPF)和非均匀量化(NUQDPF)观测的分布式粒子滤波算法进行状态估计,通过被动跟踪仿真实例,利用均方根误差(RMSE)比较了误差性能,并且比较了在不同量化级数下的非均匀量化算法的跟踪误差,仿真结果表明,基于非均匀量化观测的粒子滤波器具有更高的跟踪精度,是一种有效的非线性滤波算法.     

Effective cubature FastSLAM: SLAM with Rao-Blackwellized particle filter and cubature rule for Gaussian weighted integral

Simultaneous localization and mapping (SLAM) is a key technology for mobile robot autonomous navigation in unknown environments. While FastSLAM algorithm is a popular solution to the large-scale SLAM problem, it suffers from two major drawbacks: one is particle set degeneracy due to lack of measurements in proposal distribution of particle filter; the other is errors accumulation caused by inaccurate linearization of the nonlinear robot motion model and the environment measurement model. To overcome the problems, a new Jacobian-free cubature FastSLAM (CFastSLAM) algorithm is proposed in this paper. The main contribution of the algorithm lies in the utilization of third-degree cubature rule, which calculates the nonlinear transition density of Gaussian prior more accurately, to design an optimal proposal distribution of the particle filter and to estimate the Gaussian densities of the feature landmarks. On the basis of Rao-Blackwellized particle filter, the proposed algorithm is comprised by two main parts: in the first part, a cubature particle filter (CPF) is derived to localize the robot; in the second part, a set of cubature Kalman filters is used to estimate environment landmarks. The performance of the proposed algorithm is investigated and compared with that of FastSLAM2.0 and UFastSLAM in simulations and experiments. Results verify that the CFastSLAM improves the SLAM performance.     

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方法。该方法以EKF-SLAM方法为基础,引入了一种特征点稀疏性控制机制,该机制对地图库中的特征点同时在位置空间和特征空间进行分布密度限制,克服了因特征点快速上升而导致的庞大的计算量问题,提高了数据关联的准确性。此外,降低了SIFT矢量的维数以降低计算量,且在左右试图的特征匹配中,引入了多个约束条件,以提高匹配的准确性。最终实验结果表明,方法能够实现特征点分布的均匀性,且在定位精度满足要求的前提下缩短定位时间。     

Cross-spectral visual simultaneous localization and mapping (SLAM) with sensor handover

In this work, we examine the classic problem of robot navigation via visual simultaneous localization and mapping (SLAM), but introducing the concept of dual optical and thermal (cross-spectral) sensing with the addition of sensor handover from one to the other. In our approach we use a novel combination of two primary sensors: co-registered optical and thermal cameras. Mobile robot navigation is driven by two simultaneous camera images from the environment over which feature points are extracted and matched between successive frames. A bearing-only visual SLAM approach is then implemented using successive feature point observations to identify and track environment landmarks using an extended Kalman filter (EKF). Six-degree-of-freedom mobile robot and environment landmark positions are managed by the EKF approach illustrated using optical, thermal and combined optical/thermal features in addition to handover from one sensor to another. Sensor handover is primarily targeted at a continuous SLAM operation during varying illumination conditions (e.g., changing from night to day). The final methodology is tested in outdoor environments with variation in the light conditions and robot trajectories producing results that illustrate that the additional use of a thermal sensor improves the accuracy of landmark detection and that the sensor handover is viable for solving the SLAM problem using this sensor combination.     

量测随机延迟下带相关乘性噪声的非线性系统分布式估计

本文提出了乘性噪声和加性噪声相关下的量测随机延迟非线性系统分布式状态估计.在所考虑系统中,相关状态被多传感器簇构成的传感器网所观测.所得理想量测被传送到远程分布式处理网,并伴随服从一阶马尔可夫过程的随机延迟.在此基础上,本文提出了分布式高斯信息滤波(distributed Gaussian-information filter,DGIF),来实现估计精度与计算时间的折中.在单处理节点/单元中,以估计误差协方差最小化为准则,设计了相应的高斯递推滤波,并实现了延迟概率的在线递推估计.进一步地,在分布式处理网中,基于非线性量测方程的统计线性回归,结合一致性算法,给出了一种分布式信息滤波形式,有效实现了分布式融合.分别在单处理单元和分布式处理网中仿真验证了所提算法的有效性.     

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

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

Recent advances in simultaneous localization and map-building using computer vision

Simultaneous localization and map-building (SLAM) continues to draw considerable attention in the robotics community due to the advantages it can offer in building autonomous robots. It examines the ability of an autonomous robot starting in an unknown environment to incrementally build an environment map and simultaneously localize itself within this map. Recent advances in computer vision have contributed a whole class of solutions for the challenge of SLAM. This paper surveys contemporary progress in SLAM algorithms, especially those using computer vision as main sensing means, i.e., visual SLAM. We categorize and introduce these visual SLAM techniques with four main frameworks: Kalman filter (KF)-based, particle filter (PF)-based, expectation-maximization (EM)-based and set membership-based schemes. Important topics of SLAM involving different frameworks are also presented. This article complements other surveys in this field by being current as well as reviewing a large body of research in the area of vision-based SLAM, which has not been covered. It clearly identifies the inherent relationship between the state estimation via the KF versus PF and EM techniques, all of which are derivations of Bayes rule. In addition to the probabilistic methods in other surveys, non-probabilistic approaches are also covered.     

基于变尺度变换减少Sigma点的粒子滤波算法研究

为了减少传统无味粒子滤波(Unscented particle filter, UPF) 算法的计算负担, 提出了最小斜度单形无味转换(Minimal skew simplex UT, MSSUT) 方法, 这种方法是用最小斜度无味卡尔曼滤波来产生粒子的重要性函数. 它不仅能够扩大重要性分布与系统状态的后验概率密度的重叠性, 而且能够通过减少Sigma 点来减少计算负担. 但是, 随着状态空间维数的增加, Sigma 点集的覆盖半径增大, 导致了Sigma 点集的聚集性变差. 辅助随机变量变尺度无味变换(Auxiliary random variable formulation of the scaled unscented transformation, ASUT) 能够克服Sigma 点集分布扩展的缺点. 所以, 提出了一种高维空间中改进的变尺度最小斜度无味粒子滤波(Scaled minimal skew simplex unscented particle filter, SMSSUPF) 算法. 仿真结果表明: 在高维状态空间中, 与传统的无味粒子滤波(UPF) 相比, 计算复杂度和计算负担显著减少. 与最小斜度无味粒子滤波(Minimal skew simplex unscented particle filter, MSSUPF) 相比, SMSSUPF 减少了系统噪声方差和测量噪声方差所带来的估计误差.