Inertial Navigation for Wheeled Robots in Outdoor Terrain

Transcription

1 Inertial Navigation for Wheeled Robots in Outdoor Terrain Jan Koch, Carsten Hillenbrand and Karsten Berns Department of Computer Science, Robotics Group University of Kaiserslautern Gottlieb-Daimler-Straße, Kaiserslautern, Germany {koch, cahillen, Abstract The implementation of an inertial measurement system used within the behavior-based control of an autonomous outdoor vehicle is presented in this paper. Autonomous dead-reckoning navigation can be substantially improved by an inertial sensor system. The determined orientation information is also mandatory for higher level behavior-based control to allow path-finding adaptive to the environment. Index Terms mobile robot, inertial measurement system, navigation, behavior I. INTRODUCTION Autonomous navigation has been a major task since the beginning of robotics. Though already self-evident in all fictional applications of robots and strongly required for present projects, the actual abilities of mobile robots still lack local and global self-localization, path-finding and forward-thinking navigation. Ambitious enterprises like the Grand Challenge 24 1 revealed that even with the best technology available, it remains very hard to succeed in the mentioned goals. A behavior-based system constitutes a more reactive approach than conventional control systems. This paper presents the theory and implementation of an inertial sensor system and combining it with a behavior-based offroad navigation system. The data from this sensor system is merged with the information of the odometry in order to improve the disposable knowledge. Calculations, implementation details, and assembly of the inertial system will be illustrated in addition to the utilized behavior network and the data fusion of vehicle and inertial sensor information. At the end of this work the performance tests and results are shown. A. Scenario II. OFFROAD NAVIGATION The application scenario includes unstructured natural or wooded terrain where GPS-reception cannot be guaranteed. Especially in unstructured natural terrain the robot requires extensive information additional to navigational data. The measurement of slope, the detection of obstacles and consequently the avoidance of accidents have important influence on path-finding. For all these purposes a convenient assembly of sensor systems and navigation software has to be developed on a robust outdoor platform. The errors of 1 the sensors have to be taken into account and data fusion has to be implemented. B. Navigation Information Localization possibilities are based on two sources. Global data can be obtained by landmarks, map matching or GPS information. Dead-reckoning data is acquired by measuring and calculating the movement of the robot itself. For navigational efforts and behavior-based control one can identify five state vector variables and their according profits: acceleration: slip detection at translation velocity: navigation control angular velocity: slip detection in curves position: course correction attitude: safety All of them can be determined by dead-reckoning techniques. But as shown later, the longterm-stability of this state information cannot be guaranteed. Furthermore, dependent on the system implementation, some of these state vectors are necessary for others. III. INERTIAL MEASUREMENT SYSTEM One possibility for dead-reckoning navigation is based on inertial forces. In theory, acceleration forces and angular velocity information provide a perfect way to determine the trajectory of a system with a known initial state of motion. Due to our specific requirements, a lot of reasons made a self-development necessary, see Fig. 1. These are low power consumption, light-weight parts, small dimensions, accessibility to raw data, high data rates, adjustable parameters, low costs, and adequate precision. A. Recent Research Inertial navigation in the field of robotics is continuously investigated. Several groups work on approaches to make inertial measurement systems usable on mobile robots [1] [2]. In most cases only 2D scenarios have been considered. Promising results were achieved in this area, because as shown later, errors of the orientation do not have such a severe influence in 2D as in 3D. The calibration and statistical improvement and sensor data fusion have already been studied [3] [4]. Especially the limits of low-cost systems are evaluated, while statistical methods, first of all Kalman based procedures, could increase the effectiveness. Error estimation and evaluation of sensors, assembly and application of inertial systems are presented by [5] and [6].

2 Fig. 1. B. Calculation Methods Inertial measurement unit The given hardware provides discrete information of angular velocity and acceleration forces. The angular velocity information is mandatory to calculate the attitude of the robot or vehicle on which the inertial system is mounted. In addition the attitude is required to interpret the measured acceleration which has to be known for the global view in world coordinates. The following notations are used: ω: measured angular velocities σ: angles from integration of angular velocity φ: roll-angle around x-axis θ: pitch-angle around y-axis ψ: yaw-angle around z-axis a: accelerations g: gravity force v: velocities s: position u: rotation axis for attitude correction heuristic α: rotation angle for attitude correction heuristic t: time interval W : matrix with unit vectors of world frame B: matrix with unit vectors of body frame T : rotation tensor, translates from body to world frame ɛ: tolerance interval lower index b: vector/matrix referred to body frame lower index w: vector/matrix referred to world frame double lower index α, β: tensor rotates around axis α by angle β upper index k: vector/matrix/tensor in kth time step For the investigated application it is not necessary to consider earth rotation, earth shape, coriolis-forces and stellar influences. The world model shall be an unaccelerated plane surface only interfered by the gravity force. The calculation is performed on a standard PC, connected to the sensor system via a CAN-Bus. The components of software are visualized in Fig. 2. B w should always contain the requested attitude information of the system. At this part the common strap-down approach [7] is modified, where the attitude information is directly stored in Euler angles. Using the unit matrix of the body frame from world view has the advantage that the calculation of the Euler angles has only to be done, when needed [8]. The matrix however is updated in every calculation step. W w = 1 1 (1) 1 B w = b 11 b 12 b 13 b 21 b 22 b 23 (2) b 31 b 32 b 33 The angle change per time step is deduced by trapezoid integration of the current and last angular velocity measurement. Now this angle needs transforming to the world frame. σ k b = t k ( ω k b + ω k 1) /2 (3) σ k w = B k w σ k b (4) The attitude information is updated by means of multiplication with a rotation tensor. The rotation axis represented in this tensor has the the actual angular rate as components. The rotation angle is derived as the length of the actual angular velocity vector. In fact in every processing loop, only two of three normal vectors (rows in the orientation matrix) are rotated. The third one is always calculated as the cross product of the other ones and normed afterwards. On the one hand this saves calculating time and does on the other hand guarantee the rectangularity and normalization of the unit vectors in the transformation matrix B w. B k+1 w = T σ k, σ k B k w (5) The matrix which transforms from the body coordinate frame to the world frame is conveniently the attitude representation itself, as the following arguing shows. D shall be the unknown transformation matrix here. B w = DW w (6) W w = I (7) B w = D (8) For some applications the attitude needs to be represented in Euler angles. The combined Euler matrices represent three sequential rotations by the angles φ, θ and ψ around the world axis x, y, and z, or the body axis in opposite order (c: cos, s: sin): cψcθ sψcφ + cψsθsφ sψsφ + cψsθcφ D = sψcθ cψcφ + sψsθsφ cψsφ + sψsθcφ sθ cθsφ cθcφ (9) These angles can be determined by coefficient comparison from B w. ( ) b32 φ = arctan (1) b 33 θ = arcsin ( b 31 (11) ( ) b21 ψ = arctan (12) b 11

3 Naturally in dead reckoning systems the long term stability cannot be maintained. To get the best possible results within the capabilities of the sensors, the system has to be calibrated as correct as possible. The measured value x can be described as polynomial function of the true x and error coefficients e i. Fig. 2. Steps of the inertial calculations With knowledge of the orientation the measured acceleration forces can be handled. By multiplication with the matrix B w, they are translated to the world frame and can be used to continuously integrate the acceleration of the system to get velocity and position. The force of gravity has to be subtracted in every time step. Here we have the largest source of positioning errors. A wrong attitude would cause a wrong difference and an incorrect acceleration vector would be processed. C. Calibration g w = (13) 9, 8665 m/s 2 a w = B w a b g w (14) v w k = v w k 1 + t k ( a k w + a k 1 ) w /2 (15) s k w = s k 1 w + t k ( v k w + v k 1 w ) /2 (16) To calculate the position and attitude, the sensor data has to be integrated. Unfortunately all errors will be totalized due to integration. Assembly errors: sensor alignment on board rectangularity of board axis Electrical errors: voltage fluctuations system noise digital jitter Sensor errors: characteristic curve errors package alignment on chip temperature dependence cross axis influence acceleration influence (angular rate sensors) x = e n x n + e n 1 x n e 1 x + e n = e n x n (17) n= The coefficients of a suitable linear error model can be determined by doing representative measurements. The model consist of offsets, linear scale factors and a cross correlation matrix. These are 12 unknown variables for the set of angular velocity sensors and another 12 for the set of acceleration sensors if we move the scale factors to the diagonal elements of the correlation matrix. The unknown coefficients could be determined by taking a couple of linear independent measurements and solving an equation system but indeed a gradient descent procedure shows better results. The taken test measurements are compared to their goal results. An error sum represents a simple fitness function. The gradiend descent is done by starting with plausible initial values for the unknown variables and making random changes trying to minimize the differences between measured test vectors and their corresponding goals. If the error decreases in one loop, the achieved set of values becomes the basis for the next step. This strategy leads to an optimal solution as the model is linear. x 1 x 2 x 3 = k 11 k 12 k 13 k 21 k 22 k 23 k 31 k 32 k 33 a 1 x 1 a 2 + x 2 a 3 x 3 (18) The output of the angular velocity sensors depends on their temperature. Therefore at least the offset has to be gathered at different temperatures to make linear interpolation possible. D. Attitude Heuristic It will be shown in III-C that there are many problems with long term stability, called drift of position and attitude. In order to minimize this drift, one possibility is to adjust the attitude with respect to the gravity force. From this attitude, an expected gravity vector can be created. In each calculation step, the current attitude is determined from the totalized angular velocities. If the amount of the acceleration determined equals the standard gravity force, the first criteria is fulfilled. a w g ɛ (19) If it differs in direction from the actual acceleration vector, a correction of the attitude representation is done by

4 rotating it in order to make the two vectors point in the same direction. The rotation is done respectively to the axis built from the cross product of the two vectors while the rotation angle arises from their different directions. ( ) aw g α = arccos (2) a w g As a result of the lack of one dimension, only the roll and pitch values can be corrected by this method. u = a w g (21) B w = T u,α B w (22) This attitude correction is only heuristic and therefor is only applied if the deviation reaches a certain amount. Additionally the correction is suppressed while the acceleration data is fluctuating. But the acceleration vector might have the same length as the gravity vector in other cases than no motion. Nevertheless, in all experiments the correction helped greatly to improve the overall performance. To reduce the drift of the yaw rate, another source of information like a magnetic compass would be necessary. E. Sensor Data Fusion As a matter of fact, data provided by the inertial measurement system lacks longterm stability [7]. Therefore additional information can improve the knowledge of the system state. 1) Fusion Principle: RAVON is equipped with odometry sensors providing the wheel speed and absolute encoder values of the steering angles. Using the attitude information from the inertial measurement system, a velocity vector can be generated from these scalar odometry information. Melting this velocity vector with the one provided by the inertial measurement system improves the overall velocity information of the robot. The sensor data fusion is done by calculating the weighted mean value out of each available sensor value x i of one type of data x. x = i w ix i (23) i w i This gives a more direct control option than using a statistical method with fixed system model. The weights w i are adjustable for the particular circumstances, nevertheless, the use of Kalman or Particle-Filters will be part of future work. For example, the yaw angle is determined by the inertial measurement system and additionally by the steering angle. If the vehicle speed increases, the weight of the steering angle for yaw calculation is decreased, as slipping rises with increasing velocity. 2) Vehicle Description: The test platform used to examine the motion control is our wheel driven outdoor vehicle RAVON (Robust Autonomous Vehicle for Offroad Navigation, see Fig. 6 and Fig. 7). It has a size of about 1.4 m in width and 2.4 m in length and wheels of 73 cm in diameter. Each wheel has an individual D/C motor, the front and rear steering is independent. The robot is capable of driving up to 3 m/s and of climbing slopes with 4 % inclination. Fig. 3. F. Architecture Architecture of the inertial system 1) Sensors: For application on mobile robots, as said in III, the sensors have to be small, cheap and energy-saving. Despite the fact that optical fiber gyros are state of the art concerning precision, they cannot be used because of their weight and size. Though micromechanical sensors are less precise, they comply with all the other requirements. Some important characteristics of the chosen sensors are given. Angular rate sensor ADXRS15: Measurement area: ±15 /s or ±2.62 rad/s Non-linearity:.1% Acceleration influence:.23 /s/g Temperature influence: 15% or 21.7 /s (-4 C to +85 C),.17 /s/ C Acceleration sensor ADXL23: Measurement area: 1.7 g or m/s Non-linearity:.5% Temperature influence over full scale:.3% Cross axis influence: 2% Alignment error on chip: ±1. 2) System Design: The system consists of three sensor boards each equipped with high rate analog digital converters. The digitalized data is fetched by an Atmel CPLD triggering an interrupt in a DSP where basic filtering is done. Subsequently the position and orientation data is sent via the CAN-Bus to a PC, see Fig. 3. While sensor data is sent to the higher levels of application, control information can be given downwards. The DSP-program on the other hand adjusts the sampling rate by setting special registers of the CPLD. System state

5 angle [rad] position [m] Fig. 4. x y z roll pitch yaw Drift of position and attitude at zero movement updates from the sensor data fusion modules also have to be transferred from the PC to the DSP. While running the program, several data output modes can be selected for the DSP. The PC can request the bit, raw, filtered or fully processed data from the DSP. In this context, processed means fully calculated attitude and position information. The other modes are for debugging only. 3) Data Rates: The analog sensor data is low-pass filtered at about 1Hz by a simple capacitor. The CPLD reads the A/D converters with 6,4kHz. This oversampling is used in the DSP to do some better filtering. The algorithms mentioned in III-B run on the DSP with a loop time of about 2 ms. The final state information is sent via CAN to the PC where a sense and control loop works at about 1 ms. 4) Characteristics: The inertial measurement unit (see Fig. 1) is constructed as a aluminum cube with 4 cm edge length. Its mass is about 2 g and it consumes 2 W. IV. EXPERIMENTS Several tests have been performed to figure out the quality of the information provided by the inertial system and its capability to work together with the behavior-based control. A. Evaluating the Inertial Measurement System As expected, orientation and position accumulated from angular velocity and acceleration show immense drift even after thorough calibration (see Fig. 4). As a matter of fact, the drifts of attitude and position are significantly reduced (as shown in Fig. 5) by using the attitude correction heuristic, described in III-D. As mentioned before, a precise position integration does depend significantly on a proper attitude. With the given system the best possible location estimation has therefor an error of 1 m after one minute. Another influencing effect is the temperature dependence of the angular velocity sensors. Careful capturing of the offset in the whole expected temperature range is therefore necessary. angle [rad] position [m] x y z roll pitch yaw Fig. 5. Drift of position and attitude at zero movement with correction heuristic B. Outdoor Test of the Combined System The inertial measurement system has been attached centered to the top of the robot. At the beginning of each test, this is defined as the center of the navigation coordinate frame, which is only a displacement of angle and position in respect to the world frame. The motion control of RAVON is carried out using a behavior-based approach. Details about the architecture and about single behaviors can be found in [9]. For tests concerning the approach of goal positions the following behaviors were activated: Velocity behavior: Intended vehicle velocities generated by higher behaviors are transformed and supervised here. The output sets the vehicle velocity. Steering behavior: Front and rear steering are actuated by instances of this low-level behavior. It transforms intended translatory and rotatory movements into a steering angle. Translatory movement to a goal position behavior: The target of this behavior is to reach a given position by a translational movement. Depending on the current pose of the robot derived from odometry and inertial measurement system the translatory motion to the goal is generated. Additionally, depending on the distance to the target, the velocity is set. Orientate to a goal direction behavior: While the previous behavior handled the translatory movement to a target point this behavior has the goal to rotate the vehicle so that it points to the given position. The rotational movement and the velocity are set depending on the deviation between the vehicle orientation and the target direction. With these behaviors four destinations located in a square were to be approached in a testing scenario. After the test the vehicle was supposed to be located at the start position again.

6 roll φ [rad] pitch θ [rad] Fig. 6. RAVON in the testing area Fig. 8. isolines Roll and pitch angle in an experiment with vehicle following could be improved by the combination of inertial and odometry sensors. Especially the attitude determination is an indispensable feature not only for capturing the trajectory in a 3D-world. Also the detection of possible risks by the gradient of the environment can be avoided. Further work is done in our group in areas of sensor data fusion, GPS integration, obstacle detection and collision avoidance with stereo-vision and laser scanners. Fig. 7. Robosoft outdoor vehicle Without the assistance of the inertial measurement system and its orientation determination the path-finding is based only on the odometry. Because of slipping, the steering angle is quite imprecise. After 8 m a difference of 7 m was determined, mainly induced by slipping in curves on the snowy surface. With additional data from the angular rate sensors the obtained error was reduced to about 1 m. The odometry information was used to correct the velocity information of the system state. At a speed of 1 m/s this error was within the one from the static drift measurements. In further experiments behaviors for minimzing the pitch angle and for following isolines were activated. As a result the vehicle remained oriented along a slope and moved in the direction of minimal change in height. Fig. 8 shows the roll and pitch angle for such an experiment. The robot responded quickly to disturbances caused by the terrain in a way that it remained oriented along the isolines. The roll angle, which is not considered in the pose correction, changed distinctly, while the pitch angle remained at a safe absolute value. REFERENCES [1] A. Rudolph, A. Siegel, and J. Adamy, Ein integriertes navigationssystem für einen mobilen roboter, Thema Forschung, vol. 1, 22. [2] E. Nebot and H. Durrant-Whyte, Initial calibration and alignment of low cost inertial navigation units for land vehicle applications, Department of Mechanical an Mechatronic Engineering, University of Sidney, Tech. Rep., [3] H. Janocha and J. Fox, Eds., Statische Kalibrierung von Inertialsensoren mit Hilfe eines Industrieroboters, 24. [4] E. Kiriy and M. Buehler, Three-state extended kalman filter for mobile robot localization, McGill Center For Intelligent Machines, Montreal Canada, Tech. Rep., 22. [5] M. Hashimoto, H. Kawashima, T. Nakagami, and F. Oba, Sensor fault detection and identification in dead-reckoning system of mobile robot: Interacting multiple model approach, Department of Mechanical System Engineering, Hiroshima University, Tech. Rep., 21. [6] J. Borenstein and L. Feng, A method for measuring, comparing and correcting dead-reckoning errors in mobile robots, University of Michigan, Tech. Rep., [7] D. H. Titterton and J. L. Weston, Strapdown inertial navigation technology. Peter Peregrinus Ltd. London, [8] J. Koch, Lage- und positionsbestimmung mobiler roboter auf basis eines intertialen messsystems und ergänzender sensorik, Master s thesis, University of Kaiserslautern, unpublished, October 24. [9] M. Proetzsch, T. Luksch, and K. Berns, Fault-tolerant behavior-based motion control for offroad navigation, in 2th IEEE International Conference on Robotics and Automation (ICRA), Barcelona, Spain, April V. CONCLUSION AND OUTLOOK The inertial sensor system outstands the odometry in precision of rotation estimation because of wheel slipping in curves. As the experiments showed, navigational efforts