Pose Estimation and Control of Micro-Air Vehicles

Transcription

1 Pose Estimation and Control of Micro-Air Vehicles IVAN DRYANOVSKI, Ph.D. Candidate, Computer Science ROBERTO G. VALENTI, Ph.D. Candidate, Electrical Engineering Mentor: JIZHONG XIAO, Professor, Electrical Engineering ABSTRACT This paper presents a state estimation and control system for a Micro-Air Vehicles (MAV). The system is designed to provide high frequency 9-state (position, orientation and linear velocity in x-y-z coordinate) estimates in unknown, GPS-denied indoor environments. It requires a minimal set of sensors including a planar laser range-finder and an inertial measurement unit. The algorithms are designed to run entirely onboard, so no wireless link and ground station are explicitly needed. A major focus in our work is modularity, allowing each component to be benchmarked individually, or swapped out for a different implementation, without change to the rest of the system. We demonstrate how the state estimation can be used for simultaneous localization and mapping (SLAM) experiments. Furthermore we use the estimated pose to autonomously control the position of the MAV. KEYWORDS: Micro-air- vehicle, Pose estimation, Mapping, SLAM, Control I. INTRODUCTION Micro aerial vehicles (MAVs), such as quadrotor helicopters, are emerging as popular platforms for unmanned aerial vehicle (UAV) research due to their structural simplicity, small form factor, their vertical take-off and landing (VTOL) capability and high maneuverability. They have many military and civilian applications, such as target localization and tracking, 3D mapping, terrain and utilities inspection, disaster monitoring, environmental surveillance, search and rescue, traffic surveillance, deployment of instrumentation, and cinematography. In recent years, numerous research efforts have been made in this field, ranging from MAV test-bed and flight control design and path planning to 3D SLAM and multi-robot coordination. As a result, today s MAVs have improved in autonomy to the level that they can achieve autonomous exploration in structured indoor environments, waypoint following and openspace navigation. In this paper, we present a system for 9-degree-offreedom pose estimation and control, in GPS-denied, indoor environments (Fig. 1). The system makes the assumptions that the floors are piecewise linear and that the walls and obstacles are mostly rectilinear. These assumptions hold in typical indoor spaces. Our work was tested with the CityFlyer (Fig. 2), a research project at The City College of New York to develop a MAV that is capable of autonomous flight in a variety of threedimensional environments. A good estimation for the pose of the robot as it is moving is essential for many navigation tasks, including localization, mapping, and control. In the case of Figure 1. MAV quadrotor in an indoor mapping experiment. Figure 2. The CityFlyer research MAV based on Pelican AscTec quadrotor. 30 JOURNAL OF STUDENT RESEARCH

2 STUDENT AUTHOR: IVAN DRYANOVSKI developed an interest in science, and in particular engineering and computing, at an early age. I obtained a Bachelor s Degree in Physics at Franklin and Marshall College, Pennsylvania, with a minor in Computer Science, in The undergraduate program offered me a great deal of flexibility in academic interests to explore. After joining a team working on a mobile robot project for a competition, I discovered the field of Robotics, which provided a great intersection point for my focus in software and electronics. I completed a Master s degree in Computing Science in Imperial College, London, UK, in My Master s Thesis work focused on map-building and localization for mobile ground robots. I decided to pursue this field further and enrolled in the Computer Science Ph.D. program at the Graduate Center of the City University of New York. I have had the pleasure to do research in the Robotics Lab of The City College of New York, under the mentorship of Prof. Jizhong Xiao. My work includes 3D mapping and localization for Micro-Aerial-Vehicle using range-finder sensors such as laser scanners and RGB-D cameras. ROBERTO G. VALENTI Passion for robotics and science in general is something I always had and cultivated. This is why I followed a scientific schooling from the time of high school. I chose to take my Bachelor s and Master s Degrees in Electronics Engineering at the University of Catania, Italy, with emphasis in the Control theory which I applied in several robotics projects and in my Master Thesis. These experiences made me love Robotics and research therefore, after my graduation, I followed my inclinations and I joined The Robotics and Intelligent Systems Lab at The City College of New York where I am currently a Ph.D. student under the mentoring of Prof. Jizhong Xiao. Here I found an exciting environment and an outstanding team and I started working on a very interesting, innovative and challenging project: autonomous navigation of Micro-Aerial Vehicles (MAV) and its applications. I had the pleasure to team up with my colleague as well as friend Ivan, and in this paper we briefly explain some recent results of our research. My current research includes state estimation, modeling, sensor fusion and Control, but I am going to improve my knowledge and skills in mapping and Computer Vision in order to have a wider knowledge and more tools to conduct a more competitive research in this area. DR. JIZHONG XIAO is an associate professor of Electrical Engineering at The City College of New York, CUNY. He received his Ph.D. degree from Michigan State University in 2002; Master of engineering degree from Nanyang Technological University, Singapore, and MS, BS degrees from East China Institute of Technology in 1999, 1993, and 1990 respectively. He started the robotics research program at CCNY in 2002 as the founding director of CCNY Robotics Lab. He is a recipient of the U.S. National Science Foundation (NSF) CAREER award in 2007 and the CCNY Mentoring award in His research interests include robotics and control, mobile sensor networks, robotic navigation, and multi-agent systems. His innovative design of wall-climbing robot was reported on the JSR Vol.1, 2008, Page 40. From Left to right: Roberto G. Valenti, Ivan Dryanovski, Dr. Jizhong Xiao. VOLUME 5, AUGUST

3 ground robots, a good estimate for the robot s pose can be obtained by odometric sensors such as wheel encoders. However, no such direct approach is available for micro-air vehicles. The system works in the following way: an on-board IMU provides reasonably accurate information about the roll, pitch and yaw angles. The altitude is obtained by a laser altimeter. We present a method to obtain the displacement in the horizontal x and y directions through registration of scans from a horizontallymounted laser scanner. As a preliminary step, we project the laser scans onto the xy-plane, to make them independent of the roll and pitch motion of the vehicle. Then the position from the laser is fused together with the acceleration values from the IMU to get a fast and better estimate of position and linear velocity which are used to control the robot position. This paper is an extension of the authors own previous work [1]. The notable additions include the augmented velocity estimation and full autonomous position control. II. PLATFORM DESCRIPTION The Micro-aerial vehicle we use for our project is a quadrotor, a particular kind of helicopter which has 4 identical rotors symmetrically positioned with respect to the center of mass, rotating in 2 pairs in opposite direction to balance the total torque. For our experiments we use a Pelican quadrotor from Ascending Technologies (ASCTEC) [2] equipped with rotors which allow a total payload of about 500g. The high payload of the platform permits us to mount devices like computer board and/or sensors like a camera and a laser. In our helicopter we have an embedded computer (already included in the Asctec product) and an external laser scanner Hokuyo UTM-30LX. The embedded computer is based on a 1.6 GHz Intel Atom processor used for the computationally more expensive software. Important component of the Pelican quadrocopter is the Asctec Autopilot board composed of two ARM-7 microcontrollers and embedded sensors such as IMU (Inertial Measurement Unit) which gives reading of the roll, pitch and yaw angles, angular velocities and linear accelerations. The Microcontrollers are called Low level Processor (LLP) and High Level Processor (HLP), the LLP is responsible for the hardware management and IMU sensor data fusion and it is delivered as a black box with defined interfaces to additional components. To operate the quadrocopter, only the LLP is necessary. Therefore, the HLP is dedicated for custom code [3]. Our software runs on the Atom board (Pose estimation and Mapping) and on the HLP Figure 3. System overview. (Sensor fusion and Control). On the Atom computer is installed Linux Ubuntu in order to avoid potential driver issues and to provide maximum portability of the code, further we use ROS (Robot Operating system) framework [4] which provides many tools commonly used in Robotics and an easy way to communicate among several running nodes (programs). III. POSE ESTIMATION The entire system is presented in detail in Fig. 3. During the scan projection step, laser data is orthogonally projected onto the xy plane by using the roll and pitch IMU readings. The projected data is then passed to a scan matcher. The scan matcher uses the IMU s yaw angle reading as an initial guess for the orientation of the vehicle. The output of the scan matcher includes x, y, and yaw angle pose components. A laser altimeter uses the laser scan readings of the floor (by a mirror which deflects few laser beams to the floor), as well as the roll and pitch orientation of the MAV, to estimate the altitude (z) of the vehicle. A threshold filter is applied to detect discontinuities in the floor, assuming the floor is piecewise-linear. The estimates from the various components are 32 JOURNAL OF STUDENT RESEARCH

4 Figure 4. An overview of the laser projection step. sent by serial communication to the HLP of the autopilot where they are fused together with the IMU reading by a Kalman Filter (KF) to produce the final state, which contains the 6DoF pose in the fixed (inertial) frame, as well as the linear velocities of the MAV in reference to the body frame (vx, vy, vz). In indoor environments, obstacles usually look the same regardless of the height, due to the rectilinear property. This means that changes in the altitude of the vehicle do not affect the accuracy of the scan matcher. However, changes in the pitch and roll angle affect the laser readings, and need to be accounted for. If we know the 3D orientation of the laser scanner, we can project the scans onto the xy-plane, and perform scan matching on sequences of projected scans instead of raw scans. An overview of the projection is presented in Figure 4. The orthogonally projected scans can be used to estimate the motion of the MAV in the xy- plane. This is accomplished through the use of a laser scan matcher, based on the work of Censi [5]. A scan matcher operates in the following manner. We are given two laser scan readings, at time t and t-1. Assuming that the environment is static, the scans should look the same, except rotated and translated by a certain amount. The rotation and translation corresponds to the motion of the vehicle between time t and t-1. If we compute the transformation which best aligns the two scans, we can recover the change of pose of the MAV. The computation of the transformation usually is done using the ICP (Iterative Closest Point) algorithm, or any of its variations. By applying the scan registration step between consecutive laser scans, we can keep track of the global pose of the quadrotor in the fixed frame and then compute the map of the environment. Differentiating the pose with respect to time can also provide us with x-, y-, and yaw-velocity estimates. IV. SENSOR FUSION Referring to the pose estimation described in Figure 3, a Kalman Filter (KF) was developed in order to fuse the data from the scan matcher with the IMU data and reduce the noise. The Kalman Filter is a recursive algorithm which estimates the state of a process that minimizes the mean-square error (the optimal estimates) by knowing the statistical system model, the available measurements which represent the input of the system, and their noise that is supposed to be white Gaussian. The discrete time Kalman Filter dynamics results from two consecutive step: prediction and correction. The prediction step uses the system dynamic model to produce a prediction of the state evolution between consecutive measurements. The correction step uses the measurements to combine along with the prediction in order to refine the state estimate. The filter characterizes the stochastic disturbance input through its spectral density matrix and through the measurement error by Figure 5. Plots of the estimated translational velocities of the MAV during 3 separate test runs. The derivative of the position is shown in red. The output of the KF is shown in blue. The z- velocity graph includes a simulated laser failure during which the filter maintains the estimation using data from the IMU. VOLUME 5, AUGUST

5 its covariance. A more detailed explanation can be found in [6]. The Kalman Filter is an important element because it provides a fast pose estimation which is needed for position control. Further, since no velocity sensor is available for low velocity indoor flight, its estimation becomes necessary whereas a simple position derivative would produce an inaccurate and noisy valuation (Figure. 5). V. POSITION CONTROL The quadrotor helicopter translates on the x-,y- axis by varying its pitch and roll angle respectively. Thus the idea to control the position is using a cascade structure where the outer position loop generates an angle as reference of the inner attitude loop controller. The attitude controller is provided by the LLP of the autopilot and the outer position controller is implemented in the HLP based on a PID control. The PID controller is a linear controller which does not require the knowledge of the system model. It uses the feedback loop to calculate the error as difference between the measured controlled variable and the desired value (setpoint) and attempts to minimize the error by adjusting the control input. The calculation of the control input involves three constant parameters (Kp, Ki, Kd) used to accomplish the three control terms called: Proportional (P), Integrative (I) and Derivative (D), where P depends on the error, or in term of time, on the present error, I depends on the integral of the error, or in term of time, on the accumulation of the past errors, and D depends on the derivative of the error or in term of time is a prediction of future error. The weighted sum of these three terms will be the control input as showed below. Figure 6. 3D plot of a hovering experiment of 3 minutes duration. Figure 7. Top view of the 3D plot where the position of the quadrotor is always inside a circle of 20 cm radius. For more explanation about the PID refer to [7]. The dynamic of the quadrotor is non linear but for our purpose we can consider it with discrete approximation linear since for indoor mapping experiment we fly at low velocity with small values of roll and pitch, allowing us to decouple the states and then apply the linear PID controller to control the position. In Fig. 6, 7 and 8 we can see the plot position of the quadrotor in a hover experiment where the helicopter maintains a position around the desired point (x=0,y=0) with oscillations smaller than 20 cm for x- and y- axis and around 5 cm Figure 8. Plot of the 3 coordinate values of the position in the same experiment: in red x, in blue y and in green z. 34 JOURNAL OF STUDENT RESEARCH

6 Figure 9. Left: the 2D map of the first floor of Steinman Hall at CCNY created by autonomously flying the MAV and running the pose estimation system in conjunction with gmapping on board. Right: the same map (in red line) overlaid on the real floor plan. for z after more than 3 minutes. VI. EXPERIMENTAL APPLICATIONS A. 3D Mapping We performed experiments in which the MAV was flown autonomously in indoor environments: The MAV was flown at varying altitudes, in order to obtain data from different crosssection. B. 2D SLAM with 3D Mapping Our pose estimation system can easily be used in conjunction with existing mapping tools. The pose estimation is used as an Figure 10. 3D map of the first floor of Steinman Hall at The City College of New York. The map was created by autonomously flying the MAV and running the pose estimation system in conjunction with gmapping. odometric estimate to a 2D SLAM (simultaneous Localization and Mapping) algorithm (gmapping) running onboard the vehicle in real time. The map is constructed by passing all the projected laser scan to gmapping. The 2D step performs additional alignment of the data and guarantees loop closure and global consistency. The 2D point cloud is built on top of the 2D-refined data. REFERENCES [1] Ivan Dryanovski*, William Morris and Jizhong Xiao, An Open- Source Pose Estimation System for Micro-Air Vehicles In. IEEE International Conference on Robotics and Automation Shanghai International Conference Center May 9-13, 2011, Shanghai, China. [2] Ascending Technologies GmbH, website, [3] M. Achtelik, M. Achtelik, S. Weiss, R. Siegwart, Onboard IMU and Monocular Vision Based Control for MAVs in Unknown In- and Outdoor Environments, Proc. of the IEEE International Conference on Robotics and Automation (ICRA), [4] M. Quigley, K. Conley, B. P. Gerkey, J. Faust, T. Foote, J. Leibs, R. Wheeler, and A. Y. Ng, ROS: an open-source Robot Operating System, in ICRA Workshop on Open Source Software, [5] Andrea Censi, An ICP variant using a point-to-line metric In: Robotics and Automation, ICRA IEEE International Conference on. IEEE, New York, NY. [6] A New Approach to Linear Filtering and Prediction Problems, by R. E. Kalman, [7] Bennett, Stuart. A history of control engineering, IET. p. p. 48. ISBN VOLUME 5, AUGUST