No abstract

Abstract-This paper considers the problem of estimating the covariance of roto-translations computed by the Iterative Closest Point (ICP) algorithm. The problem is relevant for localization of mobile robots and vehicles equipped with depthsensing cameras (e.g., Kinect) or Lidar (e.g., Velodyne). The closed-form formulas for covariance proposed in previous literature generally build upon the fact that the solution to ICP is obtained by minimizing a linear least-squares problem. In this paper, we show this approach needs caution because the rematching step of the algorithm is not explicitly accounted for, and applying it to the point-to-point version of ICP leads to completely erroneous covariances. We then provide a formal mathematical proof why the approach is valid in the pointto-plane version of ICP, which validates the intuition and experimental results of practitioners.

Localization in indoor environments is a technique which estimates the robot's pose by fusing data from onboard motion sensors with readings of the environment, in our case obtained by scan matching point clouds captured by a low-cost Kinect depth camera. We develop both an Invariant Extended Kalman Filter (IEKF)-based and a Multiplicative Extended Kalman Filter (MEKF)-based solution to this problem. The two designs are successfully validated in experiments and demonstrate the advantage of the IEKF design.

No abstract

We introduce a magnetometer-plus-GPS aided inertial navigation system for a helicopter UAV. A nonlinear observer is required to estimate the navigation states, typically an Extended Kalman Filter (EKF). A novel approach is the invariant observer, a constructive design method applicable to systems possessing symmetries. We review the theory and design an invariant observer for our example. Using an invariant observer guarantees a simplified form of the nonlinear estimation error dynamics. These are stabilized using an adaptation of the Invariant EKF, a systematic approach to compute the gains of an invariant observer. The resulting design is successfully implemented and validated in experiment and shows an improvement in performance over a conventional EKF.

We derive a control-oriented model for a helicopter UAV whose complexity is sufficiently low to be usable for model-based control design yet which accurately reflects the dominant physics. The model consists of rigid-body dynamics coupled with simplified models of its subsystems: main rotor, Bell-Hiller system and tail rotor. The Bell-Hiller system is shown to provide a derivative feedback action in flight. The physical parameters of the resulting model are directly identified through a series of measurements explained throughout.

The Iterative Closest Point (ICP) algorithm is a classical approach to obtaining relative pose estimates of a robot by scan matching successive point clouds captured by an onboard depth camera such as the Kinect V1, which has enjoyed tremendous popularity for indoor robotics due to its low cost and good performance. Because the sensed 3D point clouds are noticeably corrupted by noise, it is useful to associate a covariance matrix to the relative pose estimates, either for diagnostics or for fusing them with other onboard sensors by means of a probabilistic sensor fusion method such as the Extended Kalman Filter (EKF). In this paper, we review the sensing characteristics of the Kinect camera, then present a novel approach to estimating the covariance of pose estimates obtained from ICP-based scan matching of point clouds from this sensor. Our key observation is that the prevailing source of error for ICP registration of Kinect-measured point clouds is quantization noise rather than white noise. We then derive a closed-form formula which can be computed in real time onboard the robot's hardware, for the case where only 1D translations are considered. Experimental testing against a ground truth provided by an optical motion capture system validates the effectiveness of our proposed method.

This paper presents a trajectory generation method for a nonlinear system under closed-loop control (here a quadrotor drone) motivated by the Nonlinear Model Predictive Control (NMPC) method. Unlike NMPC, the proposed method employs a closed-loop system dynamics model within the optimization problem to efficiently generate reference trajectories in real time. We call this approach the Nonlinear Model Predictive Horizon (NMPH). The closed-loop model used within NMPH employs a feedback linearization control law design to decrease the nonconvexity of the optimization problem and thus achieve faster convergence. For robust trajectory planning in a dynamically changing environment, static and dynamic obstacle constraints are supported within the NMPH algorithm. Our algorithm is applied to a quadrotor system to generate optimal reference trajectories in 3D, and several simulation scenarios are provided to validate the features and evaluate the performance of the proposed methodology.