Abstract — Applications of mobile robots in real life are increasing. The effective use of robot systems is highly dependent on the location of the robots at any given time. GPS, a low cost widely used navigational system in standalone mode is not able to fulfill the localization requirement in challenging GPS environments such as urban canyon as a direct line of sight is a prerequisite between the receiver’s antenna and four GPS satellites. Navigation algorithms integrating measurements from multi-sensor systems overcome the problems that arise from using GPS navigation system in standalone mode. Algorithms that integrate the data from a 2D low-cost, reduced inertial sensor system, consisting of a gyroscope and an odometer along with a GPS via a Kalman filter has proven to be worthy in providing a consistent and more reliable navigation solution compared to the standalone GPS.. The main objective of this research is to narrow the idea-to-implementation gap that follows the algorithm development by realizing a low-cost real-time embedded navigation system that is capable of computing the data-fused positioning solution instantly. The role of the developed system is to synchronize the measurements from three sensors, GPS, gyroscope and odometer, relative to the pulse per second signal generated from the GPS after which the navigation algorithm is applied to the synchronized measurements to compute the navigation solution in real-time. Xilinx’s MicroBlaze soft-core processor on a Virtex-4 FPGA is utilized and customized for developing the real-time navigation system. An embedded system design model is chosen to act as a framework for the work flow to be carried through the system life cycle starting from the system specification phase and ending with the system release. The developed navigation system is tested on a mobile robot and compared to a high-end navigation system. Index Terms— Mobile Robot Localization, GPS, Inertial Sensors, Kalman Filter, Embedded Systems, FPGA, soft-core

1 INTRODUCTION

The utilization of mobile robots is increasing in many aspects of life that include environmental sensing, damage assessment in case of natural disaster, remote sensing, surveying, reconnaissance and military operations (Pacis et al., 2005). The successful localization, guidance, and navigation of robots are key issues in determining the effectiveness of the overall robots’ operations. Global positioning system (GPS) is a satellite-based navigation system that is widely used in land applications including robots (Blanche 1991). Autonomous robots face challenging problems inside urban canyons or tunnels as GPS requires a line-of-sight (LOS) between the receiver’s antenna and the satellites (Borenstein et al., 1997, Georgiev and Allen, 2004, Skog, 2009 and Miller, 2008). The limitation of GPS can be addressed by integrating the measurements from one or more sensor-based systems (Skog, 2009 and Aggarwal et al., 2010) such as Inertial Measurement Units (IMU), odometers, cameras, magnetometers and others with

Realization of a Real-Time Embedded Navigation System for Robots in Challenging Environments

Walid Farid Abdelfatah, Jacques Georgy Trusted Positioning Inc., Canada

Alastair Ross Technology Centre, Suite 311, 3553 31 Street NW, Calgary, AB T2L 2K7 wabdelfatah@trustedpositioning.com, jgeorgy@trustedpositioning.com

Umar Iqbal, Abolemagd Noureldin

Royal Military College of Canada / Queen’s University, Electrical and Computer Engineering Department PO Box 17000, Station Forces, Kingston, Ontario CANADA, K7K 7B4

Umar.Iqbal@rmc.ca, Aboelmagd.Noureldin@rmc.ca

GPS measurements using data fusion techniques such as Kalman Filter (KF), Particle Filter (PF) and artificial intelligence-based techniques .

Inertial Navigation System (INS) is a self-contained, dead reckoning device that utilizes a full IMU which is composed of three orthogonal accelerometers and three orthogonal gyroscopes to continuously measure the linear accelerations and angular rates of a moving body (Titterton and Weston, 2004). The advantage of an INS is that it requires no external references in order to determine the position, velocity and attitude of the platform on which it is strapped, relative to a known starting position, velocity and attitude[9]. Odometry using wheel encoders is another type of dead reckoning that provides limited localization information. This research focuses on combining the advantages of Micro-Electro-Mechanical System (MEMS)-based inertial sensors and odometry, while mitigating their disadvantages, to provide enhanced low-cost mobile robot localization capabilities during GPS outages. Instead of integrating the GPS with a full IMU, a reduced inertial sensor system (RISS) which utilizes only one gyroscope and wheel encoders is integrated with GPS to provide a 2D positioning solution (Iqbal et al., 2008, Iqbal and Noureldin, 2009). KF is employed for integrating RISS and GPS measurements to estimate the position, velocity and heading errors relying on RISS error-state system model and GPS position and velocity updates.

The main objective of this paper is to present the realization process of a low-cost real-time embedded navigation system for robots, capable of computing the data-fused RISS/GPS navigation solution by using Xilinx’s MicroBlaze soft-core processor on Field Programmable Gate Arrays (FPGA). The processor is customized with the needed peripherals to communicate with the sensors and a single-precision floating-point unit to carry out the computation intensive operations required by the navigation algorithm. The implemented system first acquires and synchronizes the stream of measurements from the RISS and GPS information, relative to the Pulse Per Second (PPS) signal generated from the GPS receiver. KF integration algorithm is then applied to compute the navigation solution in real-time and stream it as output.

The real-time solution from the implemented system is compared to the solution provided by an off-the -shelf high-end high-cost navigation system to show the usefulness and performance of the proposed system. Utilizing a soft-core processor in the core of the navigation system provided the flexibility to customize a processor according to the requirements of the Reduced Inertial Sensor System (RISS)/GPS system. The proposed embedded navigation system provides continuous determination of the position, velocity, and heading with higher performance along with considerable reduction in the cost, size, and power consumption when compared to traditional systems. These features make the proposed system a viable product, not only for autonomous robots, but also for a wide scale of applications, that require the availability of a low-cost, accurate, and always available positioning solution.

The structure of the rest of this paper is as follows. Section II presents the methodology used. Section III provides a review of the system design and implementation. Section IV is a description of the mobile robot, and the setup used in the experiments. Section V presents the results and discussion of this work. Finally, Section VI is the conclusion of the work and discussion of open problems.

2 METHODOLOGY

2.1 Reduced Inertial Sensor System

RISS was proposed in (Iqbal et al., 2008, Iqbal and Noureldin, 2009) in order to reduce the cost of the overall positioning system for land vehicles and robots depending on the fact that land robots mostly stay in the horizontal plane. RISS is based on a single-axis gyroscope and a speed sensor to provide full 2D positioning solution, especially in denied GPS environments. With the reasonable assumption that the robot mostly stays in the horizontal plane, the robot’s speed from the wheel encoders is used together with the heading information obtained from the gyroscope to determine the velocities along the east and north directions. Consequently, the robot’s longitude and latitude are determined.

2.2 Data Fusion

The fusion of the position and velocity measurements from the GPS with the RISS computed position and velocity is done using the conventional KF method in a loosely coupled fashion. KF is a state estimation technique that can integrate measurement data to obtain state estimate by recognizing that the measurements are noisy, and can have only a small effect on the state estimate, or in some cases to be neglected completely. It smoothes out the effects of noise in the state variable being estimated by incorporating more information from reliable data than unreliable data. The designer can tune the KF with how much noise there is in the system (including the measurements, and the control inputs together with the system model) and it calculates an estimate of the state variables taking the noise into account. In addition to an estimate of the state variable vector, the algorithm provides an estimate of the state vector uncertainty (i.e. how confident the estimate is given the value for the amount of error in it).

Figure 1: Block Diagram of loosely-coupled 2D-RISS/GPS integration

Figure 1 shows where the KF fits in the navigation solution. When GPS is available, it provides measurement update for KF. During GPS outages, the KF uses velocity updates computed from forward speed (from wheel encoders) and azimuth angles from RISS mechanization. The stochastic drift errors of the inertial sensors are modeled in this KF solution as first order Gauss Markov processes. One of the main advantages of the KF, computation-wise, is that it is very convenient for real-time processing (Noureldin, 2009, Grewal and Andrews 2001). The navigation solution computed by the RISS/GPS algorithm is bounded by the rate of system sensors. The solution computed by the developed real-time system is at a rate of 1Hz due to the speed-reading which is acquired from the robot’s wheel encoders at a 1 Hz rate with respect to the arrival of the PPS signal from the GPS receiver. This rate is not a limitation of the RISS/GPS algorithm or the real-time system implementation.

3 SYSTEM DESIGN AND IMPLEMENTATION

The real-time realization of the developed navigation algorithm on an embedded system is the phase that follows the navigation algorithm design and its verification, for either a proof-of-concept or a product development. The goal of realization is to choose a convenient hardware platform and a software methodology to develop an embedded system that will acquire measurements from the various sensors, synchronize the acquired data, and perform the navigation algorithm, the 2D RISS/GPS integration in this case before new measurements are available. Moreover, the computations done by the navigation algorithm has to be optimized to execute efficiently on the system, to save the processor’s computation time and memory. Other requirements are related to user interfacing such as visualizing the navigation solution, and accepting commands from the user.

For designing an embedded system, there exists several hardware platforms that can be used such as microcontrollers (µC), digital signal processors (DSP), and field programmable gate arrays (FPGA) (Stringham, 2009, Lapsley, 2008, Dubey, 2008). Choosing one platform over the other depends not only

GPS Receiver

+

-

Mechanization

Integration

+

-

Integrated NavigationalSolution

Reduced Inertial Sensor System Data

on the system to-be developed such as the performance, power consumption and cost-per-chip but also on the ease of the development tools accompanied by a specific platform to assist the developer in implementing the system within the constrained system cost and project time.

A processor-based system based on off-the-shelf (OTS) microprocessor/microcontroller solutions is the traditional approach that designers have used in creating embedded systems. These types of traditional processors are available with a wide range of features and peripherals from a number of vendors. The process of selecting an OTS processor that can meet specific cost and functional requirements is time-consuming. It needs all the requirements to be gathered and solidified before the project development begins, which is seldom the case. In addition, some of the low-end OTS solutions lack a dedicated floating-point unit (FPU) which can boost the system performance for math centric algorithms. Due to the mentioned reasons, another approach is sought, which can allow the designer to tailor a processor and a specific set of features and peripherals for the system to-be implemented (Arbinger and Erdmann, 2010).

The real-time embedded navigation system specifications are defined as follows: I. The embedded system should have at least four universal asynchronous receiver transceiver

(UART) serial communication interfaces to communicate with the three 2D RISS/GPS system’s sensors and a host machine. The three sensors are (1) NovAtel OEM4 GPS receiver (2) Crossbow IMU which is a MEMS grade low-end device (i.e. The 2D RISS/GPS algorithm, however, uses one gyroscope only from the Crossbow IMU) (3) and a mobile robot’s encoders which use the universal asynchronous receiver transmitter (UART) serial communication interface. For each sensor, a UART channel is required. The system is also required to communicate with a PC via the UART interface to upload the measurements and the computed navigation solution to retrieve, visualize, and analyze the data.

II. Synchronize the data from the three sensors using the PPS signal from the GPS, for a more reliable fusion solution.

III. The computational power of the system should be capable of executing the navigation algorithm in the least time possible to produce a consistent navigation solution before the new measurements are available from the GPS receiver; the angular rotation rate from the vertical gyroscope within the IMU which is aligned with the robot’s z-axis; and the robot’s speed from the wheel encoders.

To test the system two approaches are adopted: a. Using the measurements that are uploaded to a host machine, the solution of the navigation

system in real-time is compared to the off-line solution computed using the main algorithm that is coded in C which makes use of the uploaded measurements. If the results from both solutions are comparable, then the developed system has succeeded in delivering a real-time navigation solution.

b. The reference solution is generated from the NovAtel SPAN unit that contains a GPS receiver and a high-end, high-cost Honeywell IMU, will be used as a general reference, for assessing the performance of the low-cost developed navigation solution and the off-line C solution.

The evaluation kit (Virtex-4 ML402) powered by the high-end Xilinx Virtex-4 SX35 FPGA was utilized for building the embedded system with the MicroBlaze processor in its core running at 100 MHz. The MicroBlaze was customized with four UARTs to communicate with the three sensors and the monitoring PC. In addition, general-purpose input-output (GPIO) channels were required for PPS and user interfacing, a timer for code profiling and an interrupt controller for the event-based system. An IEEE-745 compliant single-precision FPU provided via Xilinx cores was also added to the processor to execute the navigation algorithm once the measurements are available in the least time possible. Although the FPU used is single-precision, the algorithm was developed making use of double-precision arithmetic which gives more accurate results. Software libraries were used by the compiler to emulate the double-precision operations, using the single-precision FPU, result in a slightly denser code but a far better navigation solution.

Regarding the memory option that is used to store the code and data; the local memory (Fletcher 2005), which utilizes Xilinx’s Block RAM (BRAM) memory blocks was selected over the choice to use the external memory combined with the cache option. Two memory cores of 128 KB each are used for storing the code and data. MicroBlaze accesses to BRAM take a single bus cycle. Since the processor and bus run at the same frequency in MicroBlaze, instructions stored in BRAM are executed at the full MicroBlaze processor frequency, thus the design achieves optimal memory performance as it was optimized to fit within the customized local memory.

4 EXPERIMENTAL SETUP

A mobile robot was used to acquire the sensor measurements for the real-time system debugging and testing. The Xilinx FPGA evaluation board ML402 featuring Virtex-4 SX35 was used. The real-time navigation system was implemented on this board and connected to the various sensors for acquiring the measurements. Figure 2 presents the experimental setup of the sensors and the FPGA evaluation kit on the robot. Figure 3 shows the wiring connections of the sensors mounted on the robot with the ML402 evaluation board. The power connections to the sensors and the evaluation board are also shown. It has to be mentioned that the host machine is not externally powered and depends on batteries, which made the experiments on the mobile robot limited to a maximum of 1-1.5 hours.

Figure 2: The mobile robot used in the experiments.

Figure 3 Mobile robot equipment-wiring connections

The inertial sensors used in this work are from the MEMS-grade IMU made by Crossbow, model

IMU300CC-100 the detailed specifications can be found in (Crossbow). The forward speed used to get velocity updates is derived from wheel encoders. The results of the presented navigation solution are

evaluated with respect to a reference solution made by NovAtel, where Honeywell HG1700 high-end tactical grade IMU is integrated with the NovAtel OEM4 GPS receiver. Together the NovAtel and Honeywell systems are integrated with an off-the-shelf unit developed by NovAtel, the G2 Pro-Pack SPAN unit. The details of this system are described in (NovAtel's SPAN). The specification of HG1700 IMU can be found in (Honeywell). The high-cost NovAtel SPAN system provided the reference solution to validate the proposed method, which uses the low-cost MEMS-based sensors, and to examine the overall performance during some GPS outages.

5 RESULTS AND DISCUSSIONS

The mobile robot is used to iteratively test the system and reveal any bugs, due to the flexibility and ease of system debugging and testing offered by the robot. A number of trajectories was acquired using the robot around the Royal Military College of Canada. The presented trajectory was acquired in an open sky and a portion of the trajectory was acquired inside of a building as shown in Figure 4.

Figure 4 Mobile Robot Trajectory-inside a building with a natural GPS Outage of 100 seconds duration.

Figure 5: The top plot shows East Position Solution and bottom plot shows the Error in East Position between the Real-time and Reference Solutions during Trajectory.

-­‐    Reference  (SPAN)  -­‐  GPS  -­‐  MicroBlaze  (Real-­‐time)  

Figure 6: The top plot shows North Position Solution and bottom plot show the Error in North Position between the

Real-time and Reference Solutions during Trajectory.

A natural GPS outage of 100 seconds occurred, which is equivalent to the time taken by the mobile robot from the entrance to the exit of the building. The natural outage was detected by the system, which acted accordingly in providing a navigation solution in prediction only mode based on RISS readings. The solution still shows a very competitive performance with respect to the high-end, high-cost NovAtel SPAN reference system. The RMS error in the horizontal position during the outage is 2.39 meters, while the maximum error is 10.25 meters. Figure 5 and Figure 6 show the error in East and North positions between the high-end reference solution and the developed low-cost real-time solution.

The solution is real-time, and the difference is due to utilizing the low-cost gyroscope from the MEMS-based Crossbow IMU300CC-100 IMU in the realized system while the SPAN unit uses the high-end tactical grade HG1700 IMU.

6 CONCLUSION

The main contribution of the work presented in this paper is the development a low-cost real-time embedded navigation system utilizing the MicroBlaze soft-core embedded processor to synchronize the measurements from three sensors and then apply the KF for 2D RISS/GPS navigation algorithm, which used double-precision arithmetic that was emulated on a single-precision FPU. The result was an embedded system, which was able to provide a consistent positioning solution that is far more accurate than using the GPS in a standalone mode. A vital conclusion that can be drawn from the process and experimental work conducted on the robot in this research is that FPGA soft-core processors can be used solely in implementing math-centric algorithms, such as the KF for 2D RISS/GPS integration algorithm, where it provides the flexibility to add as many peripherals as wanted by the sensors used in the application. It has to be noted that the resources used by the processor and the peripherals of the system has utilized a small portion of the total available on the FPGA used in this research, which offers the opportunity to move to lower-density FPGAs or augment the system on the same FPGA for more computation extensive algorithms.

7 ACKNOWLEDGMENTS

This research was supported in part by research grants from NSERC. The equipment was acquired by research funds from Canada Foundation for Innovation, Ontario Innovation Trust and the Royal Military College of Canada.

REFERENCES

Aggarwal P., Syed Z., Noureldin A. and El-Sheimy N., MEMS-Based Integrated Navigation, Artech House, 2010. Arbinger D. and Erdmann J., "Designing with an embedded soft-core processor," July 2010; http://www.eetimes.com/design/signal-processing-dsp/4006632/Designing-with-an-embedded-soft-core-processor. Blanche I. J. C., "An experiment in guidance and navigation of an autonomous robot vehicle," IEEE Transactions on Robotics and Automation, vol. 7, p. 12, 1991. Borenstein J., Everett L. F. H., and Wehe D., "Mobile robot positioning: Sensors and techniques," Journal of Robotic Systems, vol. 14, p. 19, 1997. Crossbow, "IMU300 6DOF Inertial Measurement Unit Data sheet http://bullseye.xbow.com:81/Products/Product_pdf_files/Inertial_pdf/IMU300CC_Datasheet.pdf," 4145 N. First St, San Jose, CA, 95134: Crossbow Technology, 2012. Dubey R., Introduction to Embedded System Design Using Field Programmable Gate Arrays Springer, 2008. Fletcher B. H., "FPGA Embedded Processors - Revealing True System Performance," 2005. Georgiev A. and Allen P. K., "Localization Methods for a Mobile Robot in Urban Environments," IEEE Transactions on Robotics, vol. 20, p. 14, 2004. Grewal M. S. and Andrews A. P., Kalman Filtering: Theory and Practice Using MATLAB, 2nd ed. New York: John Wiley & Sons Inc., 2001. "HG1700 Inertial Measurement Unit Datasheet, Honeywell Inc. http://www51.honeywell.com/aero/common/documents/myaerospacecatalog-documents/Missiles-Munitions/HG1700_Inertial_Measurement_Unit.pdf [Accessed: July 20,2012]." Honeywell International, Minneapolis MN, USA. Iqbal U., Okou F., and Noureldin A., "An Integrated Reduced Inertial Sensor System-RISS/GPS for Land Vehicles," in Proceeding of IEEE/ION Position, Location and Navigation Symposium, Monterey, CA, 2008, pp. 1014-1021. Iqbal U. and Noureldin A., Integrated Reduced Inertial Sensor System/GPS for Vehicle Navigation, VDM Verlag, 2009. Lapsley P., DSP processor fundamentals : architectures and features, New York: IEEE Press, 1997. Miller M. M., Raquet J. F., Haag M. U. de, “Navigating in Difficult Environments: Alternatives to GPS – 2,” Proceedings of the NATO RTO Lecture Series on “Low Cost Navigation Sensors and Integration Technology,” SET-116, 2008 Noureldin A., "Mobile Multi-Sensor System Integration Course Notes," 2009. "NovAtel's SPAN (Synchronous Position, Attitude and Navigation) Technology user manual http://www.novatel.com/products/span.htm [Accessed: July 20,2012]." Pacis E. B., Everett H. R., Farrington N., Kogut G., Sights B., Kramer T., Thompson M., Bruemmer D., and Few D., "Transitioning unmanned ground vehicle research technologies," in Unmanned Ground Vehicle Technology VII, Orlando, FL, USA, 2005. Skog I. and Handel P., "In-Car Positioning and Navigation Technologies-A Survey," Intelligent Transportation Systems, IEEE Transactions on, vol. 10, no.1, pp. 4-21, 2009. Stringham G., Hardware/Firmware Interface Design: Best Practices for Improving Embedded Systems Development: Newnes 2009. Titterton D. and Weston J., Strapdown Inertial Navigation Technology, 2nd ed., American Institute of Aeronautics and Astronautics, 2004.