Proprioceptive Navigation, Slip Estimation and Slip Control for Autonomous Wheeled Mobile Robots

Transcription

1 Proprioceptive Navigation, Slip Estimation and Slip Control for Autonomous Wheeled Mobile Robots Martin Seyr Institute of Mechanics and Mechatronics Vienna University of Technology Stefan Jakubek Institute of Mechanics and Mechatronics Vienna University of Technology Abstract For a two-wheeled differentially driven mobile robot a navigation and slip control algorithm is developed. The presented concept for purely proprioceptive navigation combines state estimation via extended Kalman filter from inertial sensor data i.e. gyro and acceleration sensors) and odometric measurements i.e. wheel angular encoders). The advantages of both types of sensors are exploited by selective mixing. Tangential slip detection and side-slip angle measurement enable slip control by transiently overriding a pre-planned trajectory. Experimental results demonstrating the performance of the proposed system are presented. Keywords mobile robot navigation, sensor fusion, slip detection, side-slip angle estimation I. INTRODUCTION Autonomous mobile robot control generally raises three problems: control, navigation and motion planning. The present work concentrates on planar navigation, i.e. estimation of the current position and orientation, and slip control, without any absolute position measurement such as GPS, radio beacons or landmark detection. Only data from acceleration sensors, a gyro-sensor inertial sensors) and wheel encoders odometric sensors) are fused. This is known as proprioceptive navigation or dead reckoning as opposed to exteroceptive navigation. Naturally, the position error is subject to unbounded accumulation. Nevertheless, improved proprioceptive navigation systems help increase the allowable travel distance between absolute position updates, []. The standard method for sensor fusion is extended Kalman filtering EKF), e.g. [2], [3]. An EKF yields an optimal estimate in the sense that the statistical variance of the estimated states is minimised. However, biased measurement produces erroneous estimates. Typical sources of measurement bias are inevitable sensor drift for inertial sensors and inaccuracies of geometric parameters and wheel slip for odometric measurement. To reduce the effect of sensor bias, error models predicting the bias have been developed, [4]; or else it can be estimated and re-calibrated, [5]. To calibrate geometric parameters such as wheel diameter and wheelbase, which are vital to odometric navigation, a statistical method has been developed in [6], [7]. To overcome the effects of wheel slip or bumps causing discontinuous ground contact), which introduces a nonsystematic error into odometry, inertial data is used only transiently during periods where odometric measurement is likely to be unreliable. Aside from these periods, odometric measurement is used exclusively, because it is not affected by drift and has little bias when properly calibrated, [], [8]. In the presented approach both gyro and acceleration sensors are transiently substituted for wheel encoder data when necessary, thus enabling reliable side-slip angle estimation and tangential slip detection and ultimately allowing for slip control. The proposed navigation system is used in conjunction with the predictive trajectory tracking algorithm from [9]. Experimental results show that the combined navigation and control system enables appropriate tracking of highly dynamic trajectories. II. HARDWARE The autonomous robot Tinyphoon [], Fig., has two wheels with rubber tires and two felt shoes, one at the front and one at the rear to stabilise it around the pitch axis. It fits into a cuboid with a 75mm square footprint. The two wheels are supported by ball bearings and powered by two individual DC-motors. Exhibiting a power-mass-ratio of 22.5W/kg, the robot is capable of accelerating far beyond the slip boundary. Its maximum velocity is approximately 4m/s. Fig.. Gyro Enc.R Acc. t n Acc.2 Enc.L Autonomous mini-robot Tinyphoon, The robot is equipped with two single-chip two-axis acceleration sensors measuring tangential and lateral acceleration, /6/$2. c 26 IEEE 9 RAM 26

2 a single-chip gyro sensor and wheel encoders for each side. Therefore, seven measurement channels sampled at the same sampling rate of 2ms are available. An Analog Devices Blackfin DSP running at 6MHz provides sufficient calculation performance to execute both the predictive control algorithm and the navigation system. III. KINEMATICS The kinematics of the unicycle-type mobile robot under consideration of side-slip, Fig. 2, are given by ẋ = v cosϕ + α) =v t cos ϕ v n sin ϕ ẏ = v sinϕ + α) =v t sin ϕ + v n cos ϕ ϕ = ω, ) where x and y denote the inertial coordinates of the robot s center of gravity, ϕ denotes its the inertial attitude angle, α is the side-slip angle, v the effective velocity with components v t and v n and ω denotes the yaw rate. x v L ω y Fig. 2. Kinematics of a two-wheeled mobile robot under consideration of side-slip To calculate the robot s position and orientation x, y and ϕ, estimation of v t, v n and ω and subsequent integration using ) is necessary. IV. COMBINED ODOMETRIC AND INERTIAL NAVIGATION As mentioned in the introduction, the general idea is to use data from inertial sensors rather than data from odometric sensors only during periods where odometry is likely to be unreliable, i.e. when tangential or side-slip occur. The task is therefore to detect these situations. As functions of appropriate criteria, mixing ratios are calculated, by which a combination ranging from purely inertial estimation to purely odometric estimation is defined. In Fig. 3 a complete block diagram of the navigation algorithm is depicted. The system operates at two different sampling times, T N = 2ms primarily for navigation integer sampling instants in the following indicated by k) and T C = 2ms for control sampling instants indicated by K), T C >T N, T C /T N IN +. Some parts of the navigation algorithm are also executed at T C,Fig.3. α v t ϕ v n v R v A. Inertial Navigation by Extended Kalman Filter EKF) The two-stage prediction-update) EKF processes the following sensor data at a sampling time of T N : Accelerometers: Two channels in tangential direction, two channels in lateral direction. The sensors are subject to unknown drift which prohibits purely inertial navigation. Another problem is strong mechanical excitation caused by commotions. Gyro sensor: One channel measuring the yaw rate. Compared to the accelerometers, the drift is marginal, thus providing a very reliable measurement. The state prediction using a nonlinear discrete model is given by ˆx k k = â t,k +ˆv n,k ˆω k â n,k ˆv t,k ˆω k T N + ˆv t,k ˆv n,k ˆω k â t,k â n,k } {{ } ˆx k, 2) where v t, v n, ω and the absolute accelerations a t and a n are chosen as the states. The index k k can be spelled out as prediction of states at instant k, based on data of instant k. The measurement vector is written as y k = ω k a t,k + a t2,k )/2 a n,k + a n2,k )/2, 3) where the arithmetic mean of the two respective acceleration measurements is used to reduce noise variance, thus effectively yielding three measurements instead of five. The prediction of the measurements is then given as ŷ k k = ˆω k k â t,k k â n,k k. 4) The Jacobian of the nonlinear discrete state space representation 2) is calculated as G k k = 5) ˆω k T N ˆv n,k T N T N ˆω k T N ˆv t,k T N T N =, whereas the measurement prediction 4) is linear in the states and therefore a constant measurement matrix C IR 3 5 is obtained. The covariance of the prediction ˆx k k is given by P k k = G k k P k G T k k + Q, 6) where Q IR 5 5 denotes the constant) covariance matrix of the states, whose entries are tuned to be as small as possible

3 Inertial sensors Odometric sensors a t,k, a t2,k EKF O v t,k C, O v t ) K ˆx k Prediction 2), 4), 6) Update 7), 8), 9) Odometry ) C ), 2) Q O v t,k, O ω k P k P k I ω k O ω k â n,k k C, O v t ) K I v n,k I v n,k Mixing 5), 7), 6) C 3 C 3 4) C 2 C 2 3) C,K x, y, ϕ) k ˆx k Integration 2) ˆv K 8) ˆv t, ˆv n, ˆω) K Side-slip estimation 9) α K Slip control 22)-25) Q δ K x, y, ϕ) K ˆv, ˆω) K α K C x ref,k+i δ K Predictive control algorithm Fig. 3. Block diagram, light grey: T N,darkgrey:T C but large enough to ensure convergence. The Kalman gain matrix L K IR 5 3 is obtained by minimization of the trace of the state covariance matrix P k IR 5 5. This calculation is performed at T C to save calculation time, without recognisable deterioration of accuracy. L K = P k k C T CP k k C T + R), 7) where R IR 3 3 is the constant) covariance of the measurements, obtained from sensor manufacturer s data sheets or from analysis of the sensor signals in steady state. Using the Kalman gain matrix L K and the difference between predicted and actual measurement, the states are updated to obtain the final state estimate, x k = x k k + L K y k ŷ k k ). 8) As a last step, the covariance of the updated state estimate P k =I L K C)P k k I L K C) T + L K RL T K 9) is calculated to be used in the next sampling instant. B. Odometry From the encoders, which are free of bias due to their principle of operation, the wheel velocities v R, v L are obtained at relatively low noise. The only source of noise is quantisation due to the finite number of encoder impulses per wheel revolution. Aside from non-systematic errors due to wheel slip, odometric data is generally of very high quality. To obtain tangential velocity v t and yaw rate ω, a purely deterministic geometric transformation is applied, v t,k =v R,k + v L,k )/2 ) ω k =v R,k v L,k )/b, where the geometric parameters wheelbase b and the wheel radius used to transform the wheel angular velocity into the wheel velocities have been calibrated offline using results from [6], [7]. Naturally, from odometric measurement no information about the lateral side-slip-) velocity v n can be obtained. C. Tangential slip detection A batch of T C /T N = samples of sensor data is processed. The average tangential velocity difference of the current batch after an initial reset is calculated by discrete integration of the arithmetic mean of the tangential accelerations upper left index O stands for odometric estimate) v K = T T C/T N N O v t,k O v t,k a t,k + a t2,k ) T ) N, T C 2 k= ) which effectively means that a trend is detected, but not a stationary slip. For the latter, the reliability of the acceleration sensors does not suffice.

4 D. Mixing The final estimate is generated by linear mixing between odometric upper left index O) and inertial upper left index I) estimation. As the criterium for mixing of the tangential velocity v t the tangential slip is used. The mixing parameter is calculated every T C using C,K = pc,k + p), 2) +expa v K b )) }{{} C,K a double sided logsig-function as depicted in Fig. 4. Additionally, C is dynamically relaxed following discrete PT- behavior. Odo. C,K, [-] INS. b d d v K = a d d v K = a v K,[m/s] Fig. 4. Double-sided logsig-function as switch between odometric Odo.) and inertial INS.) navigation The tuning parameters are obtained as follows: a : steepness, chosen as steep as possible, but as flat as necessary to avoid limit cycle behavior. b : threshold, chosen as small as possible, but large enough to exclude faulty slip detection due to acceleration sensor bias. p: discrete PT-pole, chosen as close to zero fast) as possible, but large enough to avoid limit cycle behavior. For the lateral velocity v n, the absolute side-acceleration calculated by the EKF) is used as a criterium, its mixing parameter computed every T N by C 2,k = + expa 2 â n,k k b 2 )), 3) where the parameters a 2 and b 2 are obtained much in the same way as for the tangential velocity. The criterium for yaw-rate mixing is simply the absolute difference between odometric and inertial measurement, [7], [], when the difference becomes too large, odometric measurement is assumed to be unreliable, therefore the mixing parameter is obtained every T N by C 3,k = + expa 3 O ω k I ω k b 3 )) with parameters a 3 and b 3 as above. The linear mixing laws for tangential velocity v t, b 4) O ˆv t,k = C,K v t,k + C,K ) I v t,k 5) and yaw rate ω, O ˆω k = C3,k ω k + C 3,k ) I ω k, 6) are straightforward, whereas the lateral velocity v n, calculated by ˆv n,k = C 2,kˆv n,k q 2 + C 2,k ) I v n,k, 7) is taken fully from EKF when the absolute lateral acceleration is beyond the threshold b 2 but geometrically relaxed towards zero when the lateral acceleration is within the threshold. This is necessary, because from odometry no information about lateral velocity can be obtained, i.e. O v n,k does not exist. Without relaxation, however, frequent crossing of the threshold due to inevitably noisy lateral acceleration would result in frequent reset of the lateral velocity to zero. The factor q 2, <q 2 < of geometric progression is chosen as small as possible but large enough to ensure consistent velocity measurement. The final estimate is fed back to the EKF, which means that the EKF does not work consistently in a strict sense, since its output is overwritten after every sampling instant, Fig. 3. E. Calculation of the side-slip angle The resulting track speed, also needed in the control algorithm [9], is calculated by ˆv K = ˆv t,k 2 +ˆv2 n,k signˆv t,k), 8) implicitly assuming that the side-slip angle does not exceed [ π/2; π/2] a reasonable assumption) by using the signfunction. The side-slip angle is calculated from the velocity components, ) ˆvn,K α K = atan fˆv K, ˆω K ), 9) ˆv t,k and multiplied by a linear validity function fˆv K, ˆω K )= : ˆv K ˆω K < c α ˆv K ˆω K c α d α c α : c α < ˆv K ˆω K < d α : d α < ˆv K ˆω K 2) enforcing the side-slip angle to vanish for low track speed or low curvature, where otherwise no meaningful results can be obtained due to numerical reasons, and the side-slip can reasonably be assumed to be very close to zero. F. Deterministic integration of positions Finally, the positions and orientations are calculated by deterministic integration using a simple-forward-eulerdiscretised representation of ), x k+ = x k +v t,k cos ϕ k v n,k sin ϕ k )T N y k+ = y k +v t,k sin ϕ k + v n,k cos ϕ k )T N ϕ k+ = ϕ k + ω k T N. 2) 2

5 V. SLIP CONTROL The reference input to the predictive control algorithm executed at T C ) [9] consists of future reference positions and orientation up to a prediction horizon. These data are generated by a trajectory planning algorithm upper left index T). A trajectory implicitly contains velocity information as opposed to a path, which is purely geometric. In the case of excessive slip, the slip control can now transiently override the trajectory given by the planning algorithm by interpolating the original trajectory points with a reduced step size nominal ), thus remaining on the same geometric path, but with a certain delay. When no tangential or lateral slip occurs, the step size is increased beyond to re-match the original trajectory. The step size δ K, also based on the previous value δ K to ensure continuity, is computed every T C by δ K = min max δ K δ min + min C,K, ) ) q δ δ min ) ),δ min ),δ max,k, + expa 4 α K b 4 )) 22) where the maximum step size δ max,k is given by δ max > ) : K Π K > c δ δ max,k = : c δ > K Π K > d δ δ rem < ) : d δ > K Π K 23) The position on the preplanned trajectory is then given as Π K+ =Π K + δ K, 24) whereas the nominal integer position on the preplanned trajectory naturally equals K. A number of parameters determine the behavior: δ min < : Minimum step size, determines the reduction of velocity in case of slip, δ max > : Maximum step size, determines how quickly the original trajectory is recovered, q δ : factor of geometric growth of the step size after reduction, if chosen too large, limit cycle behavior results, a 4, b 4 : steepness and threshold for side-slip angle, δ rem < : Step size to match reference trajectory after overshoot, c δ, d δ : Upper/lower threshold for re-matching. The reference positions and orientation finally fed to the predictive control algorithm are obtained by interpolation using C x ref,k+i = T x ref floorπk + iδ K ) ) + 25) ΠK + iδ K floorπ K + iδ K ) ) Txref ceilπk + iδ K ) ) T x ref floorπk + iδ K ) )). Thus the progression of velocities associated with the reference trajectory is expanded or compressed according to δ with equal area, which means that both absolute velocity and velocity gradients acceleration or deceleration) are reduced. Fig. 5 gives an example of how the slip control algorithm reduces the step size in a curve, i.e. follows the geometric path with a certain delay, and subsequently catches up with the original trajectory as soon as the curvature decreases, i.e. the side-slip decays. In the picture, associated trajectory points are connected with an oval. T x ref C x ref Fig. 5. Example for the variation of the step size during detected side-slip or tangential slip and corresponding interpolation of the pre-planned trajectory VI. EXPERIMENTAL RESULTS To demonstrate the performance of the navigation and slip control algorithm, two different trajectories to be tracked have been generated: A corner with linear acceleration and deceleration and a cosine-shaped curvature profile, thus producing large side-accelerations. A square with sinusoidal velocity and yaw rate profiles, demanding high translative accelerations and decelerations. The evaluation of the navigation algorithm was performed using an independent offline video system, enabling accurate position and orientation measurement at a framerate of about 25fps and comparing its data offline i.e. after the experiment) with logged navigation data obtained via an RF-module from the autonomous robot. In Fig. 6 the effect of the slip control algorithm is obvious. Due to high side-acceleration, a side-slip angle builds up, which is estimated with high accuracy. The slip control algorithm reduces the step size. Later, when circumstances are safe in regard to slip, the step size is increased to catch up with the original trajectory. In Fig. 7 the action of the slip control algorithm for tangential wheel slip is demonstrated. In situations, where tangential slip is detected indicated by the black circles), the step size is temporarily reduced. In these situations, also a temporary divergence of the velocity estimate can be observed indicated by the light gray areas), which is a consequence of the finite reaction time of switching between odometric and inertial estimation. Finally, in Fig. 8 the position estimation for corner and square is visualised. VII. CONCLUSION In this paper a navigation, slip estimation and slip control algorithm for an autonomous mobile robot, entirely based on 3

6 α [ ] vt,n [ms ] v t, v t, v n, Slip control Rematching v n, y [m] s 3.5s.5s 2s 4.75s s.5s s.5s Fig. 8. right) x [m] x [m] Posture estimates during tracking of a corner left) and a square δ [-] t [s] Fig. 6. Tangential and lateral velocity and side slip angle estimate and slip control during tracking of a corner, indicates the velocities associated with the reference trajectory proprioceptive sensor data has been presented. The navigation algorithm s governing parameters depend on the employed sensors characteristics but are independent of the robot s physical characteristics mass, dimensions). Experimental results show that navigation with a degree of accuracy of approximately 2.5% position error with reference to absolute trajectory length is possible. Furthermore, slip control is made possible by reliable slip estimation, both lateral and tangential. In turn, slip-control supports accurate navigation, because it limits the periods with excessive slip to very short intervals. vt [ms ] ω [rads ] δ [-] t [s] Fig. 7. Tangential velocity and yaw rate estimation and slip control during tracking of a square, indicates the velocities associated with the reference trajectory ACKNOWLEDGMENT This work was financially supported by the Austrian Science Fund FWF, REFERENCES [] J. Borenstein, L. Feng, Gyrodometry: A new method for combining data from gyros and odometry in mobile robots, in IEEE International Conference on Robotics and Automation, Minneapolis, Minnesota, 25. [2] S.I. Roumeliotis, G.A. Bekey, An Extended Kalman Filter for frequent local and infrequent global sensor data fusion, in SPIE International Symposium on Intelligent Systems and Advanced Manufacturing, 997. [3] Y. Fuke, E. Krotkov, Dead reckoning for a lunar rover on uneven terrain, in IEEE International Conference on Robotics and Automation, Minneapolis, Minnesota, 996. [4] B. Barshan, H. F. Durrant-Whyte, Inertial Navigation Systems for Mobile Robots, IEEE Transactions on Robotics and Automation, no., pp , 995. [5] E. Solda, R. Worst, J. Hertzberg, Poor man s gyro-based localization, in IFAC/EURON Symposium on Intelligent Autonomous Vehicles, Lisboa, Portugal, 24. [6] J. Borenstein, L. Feng, Measurement and Correction of Systematic Odometry Errors in Mobile Robots, IEEE Transactions on Robotics and Automation, no. 2, pp , 996. [7] J. Borenstein, H. R. Everett, L. Feng, Where am I? Sensors and Methods for Mobile Robot Positioning. Technical Report, University of Michigan, 996. [8] R. Mazl, L. Preucil, Sensor data fusion for inertial navigation of trains in GPS-dark areas, Intelligent Vehicles Symposium, 23. Proceedings. IEEE, pp , June 23. [9] M. Seyr, S. Jakubek, Mobile robot predictive trajectory tracking, in ICINCO, Barcelona, Spain, 25. [] Novak, G. and Mahlknecht, S., TINYPHOON - A Tiny Autonomous Mobile Robot, in IEEE ISIE 25 International Symposium on Industrial Electronics 25, Dubrovnik, Croatia, June , pp