Cooperative Localization of AUVs Using Moving Horizon Estimation

This paper studies the localization problem of autonomous underwater vehicles (AUVs) constrained by limited size, power and payload. Such AUVs cannot be equipped with heavy sensors which makes their underwater localization problem difficult. The proposed cooperative localization algorithm is performed by using a single surface mobile beacon which provides range measurement to bound the localization error. The main contribution of this paper is twofold: 1) The observability of single beacon based localization is first analyzed in the context of nonlinear discrete time system, deriving a sufficient condition on observability. It is further compared with observability of linearized system to verify that a nonlinear state estimation is necessary. 2) Moving horizon estimation is integrated with extended Kalman filter (EKF) for three dimensional localization using single beacon, which can alleviate the computational complexity, impose various constraints and make use of several previous range measurements for each estimation. The observability and improved localization accuracy of the localization algorithm are verified by extensive numerical simulation compared with EKF.      

Correlation‐based visual odometry for ground vehicles

Reliable motion estimation is a key component for autonomous vehicles. We present a visual odometry method for ground vehicles using template matching. The method uses a downward‐facing camera perpendicular to the ground and estimates the motion of the vehicle by analyzing the image shift from frame to frame. Specifically, an image region (template) is selected, and using correlation we find the corresponding image region in the next frame. We introduce the use of multitemplate correlation matching and suggest template quality measures for estimating the suitability of a template for the purpose of correlation. Several aspects of the template choice are also presented. Through an extensive analysis, we derive the expected theoretical error rate of our system and show its dependence on the template window size and image noise. We also show how a linear forward prediction filter can be used to limit the search area to significantly increase the computation performance. Using a single camera and assuming an Ackerman‐steering model, the method has been implemented successfully on a large industrial forklift and a 4×4 vehicle. Over 6 km of field trials from our industrial test site, an off‐road area and an urban environment are presented illustrating the applicability of the method as an independent sensor for large vehicle motion estimation at practical velocities. © 2011 Wiley Periodicals, Inc.     

Inspection of an underwater structure using point‐cloud SLAM with an AUV and a laser scanner

This paper presents experimental results using a newly developed 3D underwater laser scanner mounted on an autonomous underwater vehicle (AUV) for real‐time simultaneous localization and mapping (SLAM). The algorithm consists of registering point clouds using a dual step procedure. First, a feature‐based coarse alignment is performed, which is then refined using iterative closest point. The robot position is estimated using an extended Kalman filter (EKF) that fuses the data coming from navigation sensors of the AUV. Moreover, the pose from where each point cloud was collected is also stored in the pose‐based EKF‐SLAM state vector. The results of the registration algorithm are used as constraint observations among the different poses within the state vector, solving the full‐SLAM problem. The method is demonstrated using the Girona 500 AUV, equipped with a laser scanner and inspecting a 3D sub‐sea infrastructure inside a water tank. Our results prove that it is possible to limit the navigation drift and deliver a consistent high‐accuracy 3D map of the inspected object.     

Localizing Ground Penetrating RADAR: A Step Toward Robust Autonomous Ground Vehicle Localization

Autonomous ground vehicles navigating on road networks require robust and accurate localization over long‐term operation and in a wide range of adverse weather and environmental conditions. GPS/INS (inertial navigation system) solutions, which are insufficient alone to maintain a vehicle within a lane, can fail because of significant radio frequency noise or jamming, tall buildings, trees, and other blockage or multipath scenarios. LIDAR and camera map‐based vehicle localization can fail when optical features become obscured, such as with snow or dust, or with changes to gravel or dirt road surfaces. Localizing ground penetrating radar (LGPR) is a new mode of a priori map‐based vehicle localization designed to complement existing approaches with a low sensitivity to failure modes of LIDAR, camera, and GPS/INS sensors due to its low‐frequency RF energy, which couples deep into the ground. Most subsurface features detected are inherently stable over time. Significant research, discussed herein, remains to prove general utility. We have developed a novel low‐profile ultra‐low power LGPR system and demonstrated real‐time operation underneath a passenger vehicle. A correlation maximizing optimization technique was developed to allow real‐time localization at 126 Hz. Here we present the detailed design and results from highway testing, which uses a simple heuristic for fusing LGPR estimates with a GPS/INS system. Cross‐track localization accuracies of 4.3 cm RMS relative to a “truth” RTK GPS/INS unit at speeds up to 100 km/h (60 mph) are demonstrated. These results, if generalizable, introduce a widely scalable real‐time localization method with cross‐track accuracy as good as or better than current localization methods.     

非结构道路环境下的智能汽车质心侧偏角估计

车辆质心侧偏角是描述车辆侧向运动状态的重要参量之一,其估计的精度直接影响车辆的安全控制,传统的质心侧偏角估计方法不能满足非结构道路环境下的智能汽车质心侧偏角估计的要求。通过建立3自由度智能汽车动力学模型,采用CarSim和MATLAB构建智能汽车整车参数化模型;基于扩展kalman滤波(EKF)算法,设计非结构道路环境下的状态观测器对智能汽车质心侧偏角进行估计。在高、低附着系数路面双移线工况和蛇形工况下,对状态观测器的估计效果进行联合仿真验证。仿真结果表明:该方法能较精确地估计出非结构道路环境下智能汽车的质心侧偏角。     

Vision‐based Localization and Robot‐centric Mapping in Riverine Environments

This paper presents a vision‐based localization and mapping algorithm developed for an unmanned aerial vehicle (UAV) that can operate in a riverine environment. Our algorithm estimates the three‐dimensional positions of point features along a river and the pose of the UAV. By detecting features surrounding a river and the corresponding reflections on the water's surface, we can exploit multiple‐view geometry to enhance the observability of the estimation system. We use a robot‐centric mapping framework to further improve the observability of the estimation system while reducing the computational burden. We analyze the performance of the proposed algorithm with numerical simulations and demonstrate its effectiveness through experiments with data from Crystal Lake Park in Urbana, Illinois. We also draw a comparison to existing approaches. Our experimental platform is equipped with a lightweight monocular camera, an inertial measurement unit, a magnetometer, an altimeter, and an onboard computer. To our knowledge, this is the first result that exploits the reflections of features in a riverine environment for localization and mapping.     

Model‐based Prediction of Skid‐steer Robot Kinematics Using Online Estimation of Track Instantaneous Centers of Rotation

This paper presents a kinematic extended Kalman filter (EKF) designed to estimate the location of track instantaneous centers of rotation (ICRs) and aid in model‐based motion prediction of skid‐steer robots. Utilizing an ICR‐based kinematic model has resulted in impressive odometry estimates for skid‐steer movement in previous works, but estimation of ICR locations was performed offline on recorded data. The EKF presented here utilizes a kinematic model of skid‐steer motion based on ICR locations. The ICR locations are learned by the filter through the inclusion of position and heading measurements. A background on ICR kinematics is presented, followed by the development of the ICR EKF. Simulation results are presented to aid in the analysis of noise and bias susceptibility. The experimental platforms and sensors are described, followed by the results of filter implementation. Extensive field testing was conducted on two skid‐steer robots, one with tracks and another with wheels. ICR odometry using learned ICR locations predicts robot position with a mean error of ?0.42 m over 40.5 m of travel during one tracked vehicle test. A test consisting of driving both vehicles approximately 1,000 m shows clustering of ICR estimates for the duration of the run, suggesting that ICR locations do not vary significantly when a vehicle is operated with low dynamics.     

SVD‐aided EKF attitude estimation with UD factorized measurement noise covariance

This paper describes singular value decomposition (SVD) aided extended Kalman filter (EKF) for nanosatellite's attitude estimation. The development of the filter kinematic/dynamic model, the measurement models of the sun sensors, and the magnetometers used to generate vector measurements are presented. Vector measurements are used in SVD for satellite attitude determination purposes. In the proposed method, EKF inputs come from SVD as the linear measurements of attitude angles and their error covariance. In this step, UD factorizes the attitude angles error covariance, forming the measurements in order to obtain the appropriate inputs for the filtering stage. Results are presented and analyzed in addition to discussion of the sub‐step, which is the UD factorization on the measurement covariance. The accuracy of the estimation results of the SVD‐aided EKF with and without UD factorization is compared for estimation performance.     

A Computational Geometry Approach for Localization and Tracking in GPS‐denied Environments*

Localization and tracking of vehicles is still an important issue in GPS‐denied environments (both indoors and outdoors), where accurate motion is required. In this work, a localization system based on the random disposition of LiDAR sensors (which share a partially common field of view) and on the use of the Hausdorff distance is addressed. The proposed system uses the Hausdorff distance to estimate both the position of the LiDAR sensors and the pose of the vehicle as it drives within the environment. Our approach is not restricted to the number of LiDAR sensors (the estimation procedure is asynchronous), the number of vehicles (it is a multidimensional approach), or the nature of the environment. However, it is implemented in open spaces, limited by the range of the LiDAR sensors and the geometry of the vehicle. An empirical analysis of the presented approach is also included here, showing that the error in the localization estimation remains bounded in approximately 50 cm. Real‐time experimentation as validation of the proposed localization and tracking techniques as well as the pros and cons of our proposal are also shown in this work.     

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.     

改进强跟踪滤波算法及其在汽车状态估计中的应用

准确实时地获取汽车行驶过程中的状态变量,对汽车底盘控制有着重要的意义,而这些关键状态往往难以直接测量或 者成本较高.结合纵向、侧向和横摆三自由度非线性汽车模型,将改进强跟踪滤波(Improved strong track filter, ISTF)算法应用到汽车的状态估计中,并改进了算 法的稳定性.与扩展卡尔曼滤波(Extended Kalman filter, EKF)算法进行比较分析.通过Carsim和Matlab/Simulink联合仿真和实车双移线实验验证算法,结果 表明,该算法在估计精度、跟踪速度、抑制噪声等方面均优于扩展卡尔曼滤波算法,满足汽车状态估计器的软件性能要求.     

Real-time lane tracking using Rao-Blackwellized particle filter

A novel approach to real-time lane modeling using a single camera is proposed. The proposed method is based on an efficient design and implementation of a particle filter which applies the concepts of the Rao-Blackwellized particle filter (RBPF) by separating the state into linear and non-linear parts. As a result the dimensionality of the problem is reduced, which allows the system to perform in real-time in embedded systems. The method is used to determine the position of the vehicle inside its own lane and the curvature of the road ahead to enhance the performance of advanced driver assistance systems. The effectiveness of the method has been demonstrated implementing a prototype and testing its performance empirically on road sequences with different illumination conditions (day and nightime), pavement types, traffic density, etc. Results show that our proposal is capable of accurately determining if the vehicle is approaching the lane markings (Lane Departure Warning), and the curvature of the road ahead, achieving processing times below 2 ms per frame for laptop CPUs, and 12 ms for embedded CPUs.     

Accurate lateral positioning from map data and road marking detection

We are witnessing the clash of two industries and the remaking of in-car market order, as the world of digital knowledge recently made a significant move toward the automotive industry. Mobile operating system providers are battling between each other to take over the in-vehicle entertainment and information systems, while car makers either line up behind their technology or try to keep control over the in-car experience. What is at stake is the map content and location-based services, two key enabling technologies of self-driving cars and future automotive safety systems. These content-based augmented geographic information systems (GIS) as well as Advanced Driver Assistance Systems (ADAS) require an accurate, robust, and reliable estimation of road scene attributes. Accurate localization of the vehicle is a challenging and critical task that natural GPS or classical filter (EKF) cannot reach. This paper proposes a new approach allowing us to give a first answer to the issue of accurate lateral positioning. The proposed approach is based on the fusion of 4 types of data: a GPS, a set of INS/odometer sensors, a road marking detection, and an accurate road marking map. The lateral road markings detection is done with the processing of two lateral cameras and provides an assessment of the lateral distance between the vehicle and the road borders. These information coupled with an accurate digital map of the road markings provide an efficient and reliable way to dramatically improve the localization obtained from only classical way (GPS/INS/Odometer). Moreover, the use of the road marking detection can be done only when the confidence is sufficiently high (punctual use). In fact, the vision processing and the map data can be used punctually only in order to update the classical localization algorithm. The temporary lack of vision data does not affect the quality of lateral positioning. In order to evaluate and validate this approach, a real test scenario was performed on Satory’s test track with real embedded sensors. It shows that the lateral estimation of the ego-vehicle positioning is performed with a sub-decimeter accuracy, high enough to be used in autonomous lane keeping, and land-based mobile mapping.     

A model-driven approach for real-time road recognition

This article describes a method designed to detect and track road edges starting from images provided by an on-board monocular monochromic camera. Its implementation on specific hardware is also presented in the framework of the VELAC project. The method is based on four modules: (1) detection of the road edges in the image by a model-driven algorithm, which uses a statistical model of the lane sides which manages the occlusions or imperfections of the road marking – this model is initialized by an off-line training step; (2) localization of the vehicle in the lane in which it is travelling; (3) tracking to define a new search space of road edges for the next image; and (4) management of the lane numbers to determine the lane in which the vehicle is travelling. The algorithm is implemented in order to validate the method in a real-time context. Results obtained on marked and unmarked road images show the robustness and precision of the method. Received: 18 November 2000 / Accepted: 7 May 2001     

Observability of Depth Estimation for a Hand‐Eye Robot System

This paper deals with the depth observability problem of a hand‐eye robot system. In contrast to earlier works, this paper presents a complete study of this observability problem. The velocity of the active camera in the hand‐eye robot system is considered as the input. The observability of depth estimation is then related to the velocity of the camera. A necessary and sufficient condition for the types of camera velocities necessary to ensure observability is found. This compensates for the results of earlier works, in which the velocity of camera was estimated. The theory is also verified by both simulations and experiments in this paper. Furthermore, a modified LQ visual servo control law is proposed to vary the weighting matrices so that depth estimation is improved while the level of control performance is still retained.     

Self‐alignment and calibration of a MEMS IMU: Improving observability and estimability by rotary motions of IMU

The stationary self‐alignment and calibration (SSAC) for a low‐cost MEMS IMU is quite challenging due to the poor observability of an inertial system under static condition and the significant sensor errors of MEMS inertial sensors. This research proposes to employ IMU rotations to improve the system observability and estimability regarding the SSAC of a low‐cost MEMS IMU. IMU rotations about the X, Y, and Z axes are employed in this paper. The analytic estimation algorithm for each error state is derived and the observability of the system with IMU rotation is analyzed. As the observability analysis will not provide clues about how well an error state can be estimated, the estimability analysis is also conducted based on the eigenvalues and eigenvectors from the covariance matrix in the Kalman filter. Tests are conducted with a tri‐axial turntable to verify the improvements on system observability and estimability brought by IMU rotations. Of both theoretical analysis and results indicated with proper IMU rotations, only azimuth error still remains unobservable, and the IMU rotation also significantly improves the estimability of all error states, including the unobservable azimuth.     

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.     

Kalman Filter Approach for Lane Extraction and Following

In this paper, we focus on the use of Kalman filter approach to lane extraction and following. We assume a structured environment where a mobile vehicle equipped with a camera sensor, situated at a fixed distance from the vehicle frame, is navigating. A quadratic model of the road is considered. This enables the state vector of the filter to be coincided with the three parameters pertaining to a second-order polynom. The determination of the state model is carried out considering either a pure translation of the (i+1)th frame with respect to the ith frame attached respectively to the images at time t _i and t _i+1, or a combination of both translation and rotation. While the measurement model comes down to the camera images obtained using the inverse perspective transformation, which due to the road model, permits a straightforward link between the x–y co-ordinates of images and the state vector parameters of the road. The performance of these two state models are compared with a purely measurement approach where least squares methodology is performed regardless the state model.The estimates of the filter are used by the vehicle in order to update its own knowledge about the environment and to accomplish the task of road following.Furthermore, this permits the vision system to encompass into a more general structure of Integrated Supervisory Control Systems (ISCS), where multiple functionalities are allowed.     

基于云台摄像的实时车速检测算法

针对采用固定摄像的路况监视系统无法观看自如的缺点,提出了基于云台摄像的实时车速检测算法．建立了简化的摄像机参数模型,提取了线性拟合后的车道图像特征参数,并利用Kluge曲线模型和随机霍夫变换实现了像平面车道分割线的二维重建和云台摄像机的标定;应用自适应背景减除、扩展Kalman滤波器等方法,提取了帧运动域及域中目标轮廓,从而实现了车辆的精确定位、跟踪,以至实时速度检测．该算法已试用于工程实践,具有较好的鲁棒性．