Joint state and parameter estimation in particle filtering and stochastic optimization

In this paper, an adaptive estimation algorithm is proposed for non-linear dynamic systems with unknown static parameters based on combination of particle filtering and Simultaneous Perturbation Stochastic Approxi- mation （SPSA） technique. The estimations of parameters are obtained by maximum-likelihood estimation and sampling within particle filtering framework, and the SPSA is used for stochastic optimization and to approximate the gradient of the cost function. The proposed algorithm achieves combined estimation of dynamic state and static parameters of nonlinear systems. Simulation result demonstrates the feasibilitv and efficiency of the proposed algorithm     

Multi-target state-estimation technique for the particle probability hypothesis density filter

A simple yet effective state-estimation algorithm is presented and demonstrated to have advantages over previous standard clustering techniques used for the particle probability hypothesis density filter.The idea behind the proposed algorithm is that it uses the latest available information(i.e.,the measurements) to direct particle clustering.The particle likelihood and target number estimation,computed during probability hypothesis density recursion,are both used to partition particles into clusters,and the center of each cluster gives the state estimation of an individual target.Simulation results indicate that the proposed algorithm outperforms the standard clustering approach using the k-means algorithm,achieving higher accuracy and shorter computational time.     

A new approach of formation navigation derived from multi-robots cooperative online FastSLAM

A new formation navigation approach derived from multi-robots cooperative online FastSLAM is proposed. In this approach,the leader and follower robots are defined.The posteriori estimation of the leader robot state is treated as a relative reference for all follower robots to correct their state priori estimations.The control volume of individual follower will be achieved from the results of the corrected estimation.All robots are observed as landmarks with known associations by the others and are considered in their landmarks updating.By the method,the errors of the robot posterior estimations are reduced and the formation is well kept.The simulation and physical experiment results show that the multi-robots relative localization accuracy is improved and the formation navigation control is more stable and efficient than normal leader-following strategy.The algorithm is easy in implementation.     

A novel suboptimal algorithm for state estimation of Markov jump linear systems

This paper is concerned with state estimation problem for Markov jump linear systems where the disturbances involved in the systems equations and measurement equations are assumed to be Gaussian noise sequences.Based on two properties of conditional expectation,orthogonal projective theorem is applied to the state estimation problem of the considered systems so that a novel suboptimal algorithm is obtained.The novelty of the algorithm lies in using orthogonal projective theorem instead of Kalman filters to estimate the state.A numerical comparison of the algorithm with the interacting multiple model algorithm is given to illustrate the effectiveness of the proposed algorithm.     

A learning particle swarm optimization algorithm for odor source localization

This paper is concerned with the problem of odor source localization using multi-robot system. A learning particle swarm optimization algorithm, which can coordinate a multi-robot system to locate the odor source, is proposed. First, in order to develop the proposed algorithm, a source probability map for a robot is built and updated by using concentration magnitude information, wind information, and swarm information. Based on the source probability map, the new position of the robot can be generated. Second, a distributed coordination architecture, by which the proposed algorithm can run on the multi-robot system, is designed. Specifically, the proposed algorithm is used on the group level to generate a new position for the robot. A consensus algorithm is then adopted on the robot level in order to control the robot to move from the current position to the new position. Finally, the effectiveness of the proposed algorithm is illustrated for the odor source localization problem.     

Marginalized particle filter for spacecraft attitude estimation from vector measurements

An algorithm based on the marginalized particle filters （MPF） is given in details in this paper to solve the spacecraft attitude estimation problem： attitude and gyro bias estimation using the biased gyro and vector observations. In this algorithm, by marginalizing out the state appearing linearly in the spacecraft model, the Kalman filter is associated with each particle in order to reduce the size of the state space and computational burden. The distribution of attitude vector is approximated by a set of particles and estimated using particle filter, while the estimation of gyro bias is obtained for each one of the attitude particles by applying the Kalman filter. The efficiency of this modified MPF estimator is verified through numerical simulation of a fully actuated rigid body. For comparison, unscented Kalman filter （UKF） is also used to gauge the performance of MPE The results presented in this paper clearly derfionstrate that the MPF is superior to UKF in coping with the nonlinear model.     

Rotation estimation for mobile robot based on single-axis gyroscope and monocular camera

The rotation matrix estimation problem is a keypoint for mobile robot localization, navigation, and control. Based on the quaternion theory and the epipolar geometry, an extended Kalman filter (EKF) algorithm is proposed to estimate the rotation matrix by using a single-axis gyroscope and the image points correspondence from a monocular camera. The experimental results show that the precision of mobile robot s yaw angle estimated by the proposed EKF algorithm is much better than the results given by the image-only and gyroscope-only method, which demonstrates that our method is a preferable way to estimate the rotation for the autonomous mobile robot applications.     

Real-time visual odometry estimation based on principal direction detection on ceiling vision

In this paper,we present a novel algorithm for odometry estimation based on ceiling vision.The main contribution of this algorithm is the introduction of principal direction detection that can greatly reduce error accumulation problem in most visual odometry estimation approaches.The principal direction is defned based on the fact that our ceiling is flled with artifcial vertical and horizontal lines which can be used as reference for the current robot s heading direction.The proposed approach can be operated in real-time and it performs well even with camera s disturbance.A moving low-cost RGB-D camera(Kinect),mounted on a robot,is used to continuously acquire point clouds.Iterative closest point(ICP) is the common way to estimate the current camera position by registering the currently captured point cloud to the previous one.However,its performance sufers from data association problem or it requires pre-alignment information.The performance of the proposed principal direction detection approach does not rely on data association knowledge.Using this method,two point clouds are properly pre-aligned.Hence,we can use ICP to fne-tune the transformation parameters and minimize registration error.Experimental results demonstrate the performance and stability of the proposed system under disturbance in real-time.Several indoor tests are carried out to show that the proposed visual odometry estimation method can help to signifcantly improve the accuracy of simultaneous localization and mapping(SLAM).     

Analysis of quantization noise and state estimation with quantized measurements

The approximate correction of the additive white noise model in quantized Kalman filter is investigated under certain conditions. The probability density function of the error of quantized measurements is analyzed theoretically and experimentally. The analysis is based on the probability theory and nonparametric density estimation technique, respectively. The approximator of probability density function of quantized measurement noise is given. The numerical results of nonparametric density estimation algorithm demonstrate that the theoretical conclusion is reasonable. Based on the analysis of quantization noise, a novel algorithm for state estimation with quantized measurements also is proposed. The algorithm is based on the least-squares estimator and unscented transform. By least-squares estimator, the effective information is extracted from the quantized measurements. Also, using the information to update the estimated state can give a better estimation under the influence of quantization. The root mean square error (RMSE) of the proposed algorithm is compared with the RMSE of the existing methods for a typical tracking scenario in wireless sensor networks systems. Simulations provide a strong evidence that this tracking algorithm could indeed give us a more precise estimated result.     

一种用于移动机器人状态和参数估计的自适应UKF算法

For improving the estimation accuracy and the convergence speed of the unscented Kalman filter(UKF),a novel adaptive filter method is proposed.The error between the covariance matrices of innovation measurements and their corresponding estimations/predictions is utilized as the cost function.On the basis of the MIT rule,an adaptive algorithm is designed to update the covariance of the process uncertainties online by minimizing the cost function.The updated covariance is fed back into the normal UKF.Such an adaptive mechanism is intended to compensate the lack of a priori knowledge of the process uncertainty distribution and to improve the performance of UKF for the active state and parameter estimations.The asymptotic properties of this adaptive UKF are discussed.Simulations are conducted using an omni-directional mobile robot,and the results are compared with those obtained by normal UKF to demonstrate its effectiveness and advantage over the previous methods.     

New Entropy-Based Adaptive Particle Filter for Mobile Robot Localization

Over the last decade, particle filters have been applied with great success to a variety of state estimation problem. The standard particle filter suffers poor efficiency during the estimation process, especially in the global localization and kidnapped problem. In this paper, we proposed a novel information entropy-based adaptive approach to improve the efficiency of particle filters by adapting the number of particles. The information entropy-based adaptive particle filter approaches use the information entropy to present the uncertainty of a mobile robot to the environment. By continuously obtaining the sensor information, the robot gradually reduces the uncertainty to the environment and, therefore, reduces the particle number for the estimation process. We derived the mathematic equation relating the information entropy with particle number. Extensive localization experiments using a mobile robot showed that our approach yielded drastic improvements and efficiency performance over a standard particle filter with fixed particles and over other adaptive approaches.     

基于粒子滤波器的移动机器人自定位方法研究

首先,对粒子滤波器的原理进行了简要阐述。然后详细描述了基于粒子滤波器的移动机器人自定位算法——蒙特卡洛定位算法。在ROS(Robot Operating System)平台上对该算法进行了仿真实验并分析了其性能。最后,对蒙特卡洛粒子滤波定位方法用于移动机器人定位进行了总结。结果表明,MCL(蒙特卡洛)算法是一种精确鲁棒的移动机器人概率定位方法,可对解决移动机器人的定位问题提供有意义的参考。提出的机器人自定位方法为机器人在Robocup竞赛中自主执行各种作业提供定位支持,已在2013年中国机器人大赛获奖。     

Particle Filter for Collaborative Multi-Robot Localization Tolerant to Recognition Error

Statistical algorithms using particle filters have been proposed previously for collaborative multi-robot localization. In these algorithms, by synchronizing each robot's belief or exchanging the particles of the robots, fast and accurate localization is attained. However, there algorithms assume correct recognition of other robots and the effects of recognition error are not considered. If the recognition of other robots is incorrect, a large amount of error in localization can occur. This paper describes this problem. Furthermore, in order to cope with the problem, an algorithm for collaborative multi-robot localization is proposed. In the proposed algorithm, the particles of a robot are exchanged with those of other robots according to measurement results obtained by the sending robot. At the same time, some particles remain in the sending robot. Received particles from other robots are evaluated using measurement results obtained by the receiving robot. The proposed method copes with recognition error by using the remaining particles, and increases the accuracy of estimation by twice evaluating the exchanged particles of the sending and receiving robots. These properties of the proposed method are argued mathematically. Simulation results show that incorrect recognition of other robots does not cause serious problems in the proposed method.     

A collaborative localization tolerant to recognition error by double-check particle exchange

Statistical algorithms using particle filters for collaborative multi-robot localization have been proposed. In these algorithms, by synchronizing every robot’s belief or exchanging particles of the robots with each other, fast and accurate localization is attained. These algorithms assume correct recognition of other robots, and the effects of recognition errors are not discussed. However, if the recognition of other robots is incorrect, a large amount of error in localization can occur. This article describes this problem. Furthermore, an algorithm for collaborative multi-robot localization is proposed in order to cope with this problem. In the proposed algorithm, the particles of a robot are sent to other robots according to measurement results obtained by the sending robot. At the same time, some particles remain in the sending robot. Particles received from other robots are evaluated using measurement results obtained by the receiving robot. The proposed method is tolerant to recognition error by the remaining particles and evaluating the exchanged particles in the sending and receiving robots twice, and if there is no recognition error, the proposed method increases the accuracy of the estimation by these two evaluations. These properties of the proposed method are argued mathematically. Simulation results show that incorrect recognition of other robots does not cause serious problems in the proposed method.     

FastSLAM算法的仿生优化改进研究

针对机器人导航标准的快速同步定位与地图构建算法(FastSLAM)在重采样过程中存在采样粒子集的贫化以及粒子多样性的缺失导致机器人的定位与建图的精度下降的问题,提出一种基于改进的蝴蝶算法来优化FastSLAM中的粒子滤波部分。改进的算法将机器人的最新时刻的观测和状态信息融入到蝴蝶算法的香味公式中,并在蝴蝶位置更新的过程加入自适应香味半径和自适应蝴蝶飞行调整步长因子,来减少算法的运算时间以及提高预测精度,同时引入偏差修正指数加权算法对粒子的权值进行优化组合,对组合后部分不稳定的粒子进行分布重采样,保证粒子的多样性。通过仿真验证了该算法在估计精度与稳定性方面优于FastSLAM,因此在移动机器人运动模型的定位与建图中具有较高的定位精度与稳定性。     

基于多假设跟踪的移动机器人自适应蒙特卡罗定位研究

针对移动机器人蒙特卡罗定位(Monte Carlo localization, MCL)算法在含有对称和自相似结构的环境中容易失败的问题, 提出了一种基于多假设跟踪的自适应蒙特卡罗定位改进算法. 该算法根据粒子间空间相似性采用核密度树聚类算法对粒子群进行聚类, 每簇粒子代表一个位姿假设并用一个独立的MCL算法进行跟踪, 总体上形成了一组非等权的粒子滤波器, 很好地克服了普通粒子滤波器由于粒子贫乏而引起的过度收敛问题. 同时运用该核密度树实现了自适应采样, 提高了算法的性能. 针对机器人``绑架'问题对该算法作了进一步的改进. 实验结果证明了该算法的有效性.     

Simultaneous localization of multiple unknown CSMA-based wireless sensor network nodes using a mobile robot with a directional antenna

We use a single mobile robot equipped with a directional antenna to simultaneously localize unknown carrier sensing multiple access (CSMA)-based wireless sensor network nodes. We assume the robot can only sense radio transmissions at the physical layer. The robot does not know network configuration such as size and protocol. We formulate this new localization problem and propose a particle filter-based localization approach. We combine a CSMA model and a directional antenna model using multiple particle filters. The CSMA model provides network configuration data while the directional antenna model provides inputs for particle filters to update. Based on the particle distribution, we propose a robot motion planning algorithm that assists the robot to efficiently traverse the field to search radio source. The final localization scheme consists of two algorithms: a sensing algorithms that runs in O(n) time for n particles and a motion planning algorithm that runs in O(nl) time for l radio sources. We have implemented the algorithm, and the results show that the algorithms are capable of localizing unknown networked radio sources effectively and robustly.     

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

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

一种改进的粒子滤波SLAM算法

传统的粒子滤波SLAM算法中,由于历史信息未被利用而导致估计精度较低。文中结合精确稀疏滞后状态信息滤波具有自然稀疏的信息矩阵因而估计精度高以及精确稀疏扩展信息滤波计算效率高的优点,将二者混合应用于粒子滤波SLAM算法中。不但充分应用信息矩阵记录的机器人位姿与特征间关系的历史信息从而提高估计的精度,而且克服机器人转动状态及环境特征疏密带来的应用缺陷。仿真与真实机器人实验的实验结果均表明文中算法的有效性与可行性。     

Odor source localization using a mobile robot in outdoor airflow environments with a particle filter algorithm

This paper discusses odor source localization (OSL) using a mobile robot in an outdoor time-variant airflow environment. A novel OSL algorithm based on particle filters (PF) is proposed. When the odor plume clue is found, the robot performs an exploratory behavior, such as a plume-tracing strategy, to collect more information about the previously unknown odor source. In parallel, the information collected by the robot is exploited by the PF-based OSL algorithm to estimate the location of the odor source in real time. The process of the OSL is terminated if the estimated source locations converge within a given small area. The Bayesian-inference-based method is also performed for comparison. Experimental results indicate that the proposed PF-based OSL algorithm performs better than the Bayesian-inference-based OSL method.