Professional Documents
Culture Documents
Iagnemma ICRA 07a
Iagnemma ICRA 07a
1
Robotics and Automation
Roma, Italy, 10-14 April 2007
Abstract— This paper introduces a model-based approach to current measurement and terrain-specific parameter tuning.
estimating longitudinal wheel slip and detecting immobilized A body of work exists in the automotive community related
conditions of autonomous mobile robots operating on outdoor to traction control and anti-lock braking systems (ABS);
terrain. A novel tire traction/braking model is presented and however, this work generally applies at significantly higher
used to calculate vehicle dynamic forces in an extended Kalman
filter framework. Estimates of external forces and robot
speeds than is typical for autonomous robots [6],[7].
velocity are derived using measurements from wheel encoders, A large amount of work has utilized Kalman filters with
IMU, and GPS. Weak constraints are used to constrain the inertial and absolute measurements to enhance dead
evolution of the resistive force estimate based upon physical reckoning and estimate lateral slip [8]-[10]. The notion of an
reasoning. Experimental results show the technique accurately effective tire radius, which can indirectly compensate for
and rapidly detects robot immobilization conditions while some longitudinal slip, is presented in [11]. None of this
providing estimates of the robot’s velocity during normal
driving. Immobilization detection is shown to be robust to
work, however, explicitly considers the effects of
uncertainty in tire model parameters. Accurate immobilization longitudinal wheel slip or vehicle immobilization.
detection is demonstrated in the absence of GPS, indicating the A potentially simple approach to detecting robot slip and
algorithm is applicable for both terrestrial applications and immobilization is to analyze GPS measurements. However,
space robotics. nearby trees and buildings can cause signal loss and
multipath errors and changing satellites can cause position
I. INTRODUCTION and velocity jumps [12],[13]. Additionally, GPS provides
I. DYNAMIC MODELS
A. Robot Configuration Fig. 1. Robot kinematic parameters and body-fixed coordinate system.
2725
ThC11.1
extended Kalman filter framework. The simplified model is: filter (EKF) to integrate sensor measurements with the
FTraction = N (sign( vrel )C1 (1- e -At vrel )+ C 2 vrel ) (8) nonlinear vehicle model. The EKF structure requires that the
where vrel is the velocity of the tire relative to the ground: discrete, nonlinear process model be written in the form
vrel = rω - v fwd , and vfwd is the tire’s forward velocity, xˆ k− = f ( xˆ k −1 , u k , wk −1 ) , where x̂ k− is the a priori estimate of
computed as: v fwd = v bx ± 0.5cψ& , where ψ& is the robot yaw the state vector, x, at time step k and f is a nonlinear function
rate, and C1, At, and C2 are constants. of the previous state estimate, xˆ k −1 , the current input vector,
The simplified model is continuously differentiable and u k , and process noise, wk −1 . The measurement vector, z, is a
can predict both traction and braking force, without a need to nonlinear function, h, of the true, current state vector and
distinguish the two cases. This model requires three sensor noise ν such that: z k = h( xk ,ν k ) .
terrain/tire dependant parameters as opposed to the popular The standard EKF time update equations using the
“Magic Formula” which requires six. C1 and At are positive notation of [21] and Joseph’s form of the covariance update
constants which can be viewed as the maximum tire-terrain equation [22] are used. The relations f ( xˆ k −1 ,u k ,0) and
traction coefficient and the slope of the traction curve in the h ( xˆ k− ,0) express the estimated state and measurement
low relative velocity region, respectively. C2 is the slope in vectors, x̂ k− and ẑ , by evaluating the nonlinear process and
the high relative velocity range and can be positive or measurement equations, assuming zero noise. Q and R are
negative depending on the terrain. process and measurement noise covariance matrices and Ak,
Fig. 3 shows a plot of traction/braking versus wheel slip Wk, Hk, and Vk are process and measurement Jacobians.
for lines of constant wheel velocity using the proposed B. State Space Model Formulation
traction/braking model, as well as a representative traction
The vehicle and sensor dynamics are formulated as a state
force curve from the literature [20]. Assuming the robot
space model using the following state vector:
typically operates near a nominal velocity, the proposed T T
model can be interpreted as a pseudo-linearization around x = [ x1 , x2 , x3 , x4 , x5 , x6 , x7 ] = [vbx , bax , adist , bx , ωl , ωr ,ψ& , bgψ ] ,
the nominal operating velocity. where ωl and ωr are the angular velocities of the left and
right front wheels and bax and bgψ are the accelerometer x-
E. Rolling Resistance Model
axis and yaw gyro walking biases, respectively, which are
Rolling resistance is generally modeled as a part of the IMU error model suggested in [23].
combination of static and velocity dependant forces [20], Using the above state vector, the vehicle dynamics can be
[17]. Here a function with form similar to (8) is proposed as written as:
a continuously differentiable formulation of the rolling
f tire (x,θ ,ϕ ) + x 3 − g sin(ϕ ) w1
resistance, with the static force smoothed at zero velocity to
1 2 f σ 2
w2
s abx
avoid a singularity. The rolling resistance is: − x2
τ ax τ ax
( (
Froll res = −sign (v fwd )N R1 1-e -Aroll v fwd
)+R 2 v fwd ) (9)
0 w 3
where R1, Aroll, and R2 are positive constants.
x& = f controller ,l (u ) + w4 (10)
f controller , r (u ) w 5
g tire (x,θ ,ϕ )
w 6
1 2 f s σ gbz
2
− x7 w
τ gz
7
τ gz
where wi are zero mean white noise and fcontroller(u) is the
wheel acceleration which is a function of the robot’s onboard
velocity controller and the desired velocity. When estimating
the vehicle dynamics, we neglect fcontroller, assuming it is
negligible compared with the process noise. The a priori
estimate of the robot state is found by discretizing the state
equations and neglecting the zero-mean process noise, wi.
C. Measurement Model
The algorithm utilizes measurements from the IMU, GPS,
Fig. 3. Comparison of traction force vs. wheel slip curves for the slip-based and front wheel encoders. The measurement vector is:
model and the proposed simplified model at various wheel speeds. T T
z = [z 1 , z 2 , z 3 , z 4 , z 5 ] = [&x&IMU ,ψ& IMU , x& GPS , ω l ,enc , ω r ,enc ] ,
II. SLIP DETECTOR ALGORITHM where &x&IMU and ψ& IMU are IMU measurements of x-axis
acceleration and yaw rate, x&GPS is the component of the GPS
A. Extended Kalman Filter velocity measurement along the body x-axis, and ωl,enc and
The slip detector algorithm utilizes an extended Kalman ωr,enc are the left and right front wheel encoder angular
2726
ThC11.1
velocities. The measurement vector can be written as: x3 appears in the calculation of x&1 while x2 does not, and
f tire (x,θ ,ϕ ) + cax + x 2 + x3 ν 1 that x3 is assigned a larger covariance in the matrix Q so that
x3 can evolve more quickly than x2.
x6 + c gz + x7 ν 2 Although a direct measure of the disturbance force is
z = h(x,ν ) = x1 + ν 3 (11)
generally not available, rules governing its evolution can be
x4 ν 4 developed based upon insight into the physical nature of the
ν 5 disturbance. These rules are implemented using weak
x5
where cax and cgz are constant offsets of the x-axis constraints described in [25] and implemented in a vehicle
accelerometer and yaw gyro respectively. To approximate model in [11]. Unlike ad hoc solutions, weak constraints are
the constant offsets, they are initialized to the average of the a principled method for integrating rules and constraints into
first n IMU measurements, subtracting out the acceleration the Kalman filter framework which correctly update the state
due to gravity from the acceleration measurement. When the vector and system covariance matrix. Weak constraints can
robot is at rest, the constant offsets are updated with new be viewed as virtual measurements or observations.
measurements using the exponential moving average [24]: Table I summarizes the weak constraints employed in this
2 work. The middle column presents the condition that must be
EMAcurrent = (measurementcurrent - EMAprev ) (12)
+ EMAprev met for the constraint to be applied and the right column
1+ p
which is an approximation of the time average of the gives the measurement innovation to be used in the EKF. For
measurement over the last p samples, with a higher weight some of the conditions the variable VelDir is used, defined
given to the most recent measurements. The EMA is not as: VelDir = sign (ω l + ω r ) , such that VelDir equals 1 if the
guaranteed to converge to the true value of the constant wheels are driving forward, 0 if the wheels are stopped, and -
offsets, however it is easily and recursively calculated and 1 if the wheels are driving in reverse. iEMA is the EMA (12)
has yielded good results in practice. The estimated of the average of the left and right front wheel slip.
measurement vector, ẑ , is found by evaluating h(xˆ −k ,0 ) . E. Slip and Immobilization Detection
D. Weak Constraints The extended Kalman filter provides an estimate of the
The disturbance, x3, and accelerometer walking bias, x2, robot’s forward velocity and the front wheels’ angular
have both been modeled as random walks. Practically, the velocities. Using these estimates, a criterion for detecting
only difference between these variables in the model are that when the robot is immobilized is desired. A natural choice
TABLE I. SUMMARY OF WEAK CONSTRAINTS USED.
Description Condition “Measurement” Innovation
1) The modeled disturbance force should only
oppose motion.
−
if (sign ( xˆ k , 3 ) = VelDir ) ( z k ,WC 1 − hWC 1 ( xˆ k− ,0)) = (0 − xˆ k−, 3 )
−
AND (sign ( xˆ k , 3 ) ≠ 0)
2) The disturbance should act quickly. The
∆iEMA ( z k ,WC 2 − hWC 2 ( xˆ k− ,0)) = (α xˆk−, 3 − xˆ k−, 3 )
disturbance should not gradually increase such that if 0 < < min
∆i
the wheel EMA of the slip slowly increases. Only ∆t 0 <α <1
applies when the average wheel slip is small. min∆i,
threshi, and α are user-defined constants AND i ( EMA
< thresh
i
)
(
3) The disturbance can stop the robot, but should not if sign ( xˆ − ) = −VelDir
k ,1 ) ( z k ,WC 3 − hWC 3 ( xˆ k− ,0)) =
pull the robot backwards. If the robot is moving
if sign ( f − g sin(ϕ )) = −VelDir a) (0 − xˆ k−, 3 )
backwards, then either it is sliding down a hill and tire
the disturbance should be zero, or the estimated ⇒ then → a) b) (max(VelDir[ g sin(ϕ ) − f tire , xˆ k−, 3 ]) − xˆ k−, 3 )
disturbance is too high and should be reduced. else → b)
4) When robot is fully stopped, the disturbance force
and walking biases should tend to zero for
if (ωl = 0) AND (ω r = 0) ( z k ,WC 4 − hWC 4 ( xˆ k− ,0)) =
calibration of the IMU constant biases. tstop is a
for T ≥ t stop ([ 0,0,0]T − [ xˆ k−, 2 , xˆ k−, 3 , xˆ k−, 7 ]T )
constant. T is the length of time the condition has
been met.
5) In practice there may be known limits on the
magnitude of a state. These can be implemented as if (abs( xˆ k−, i ) > max x ) ( z k ,WC 5 − hWC 5 ( xˆ k− ,0)) =
weak constraints as a precaution to keep the states i
bounded. Under normal operation, these constraints
(sign( xˆ k−,i ) max xi − xˆ k−,i )
In practice i = 1, 2, 3, and 7 have been used.
will rarely be called. maxxi are constants.
6) Similar to 5, we can also impose limits on the rate
abs( xˆ − − xˆ − ) ( z k ,WC 6 − hWC 6 ( xˆ k , 0 )) =
−
of change of a state. For example, we may know that
a robot’s acceleration will never exceed a certain if k,i k − 1 ,i
> max
(( xˆ k−−1, i + sign( xˆ k , i - xˆ k −1, i ) max ∆i ∆t ) − xˆ k , i )
− − −
∆t ∆i
limit.
In practice i = 1 and 2 have been used.
2727
ThC11.1
for an “immobilized” metric is the wheel slip (6). In practice, exception of two false positives. In total, less than 0.2% of
the calculated wheel slip can be noisy. To improve the data points were falsely labeled as immobilized.
robustness, the EMA (12) of the average of the left and right Fig. 5 shows a plot of the robot driving unconstrained over
wheel slips is calculated and immobilization is detected if the grass at 1 m/s until t ~ 16 s, after which the robot attempts to
EMA is larger than a threshold value. The threshold value is drive forward while restrained with a spring scale, producing
chosen empirically. A low threshold value allows the 100% wheel slip. In the top plot, it can be seen that the
detector to react quickly, however can be prone to falsely filter’s estimated robot velocity (x1) follows the measured
detecting immobilized conditions. In practice, since wheel velocity during the unconstrained driving. The middle
measurement noise can cause large variations in calculated plot shows the estimated disturbance (x3), which remains
slip at low speeds, the threshold can be chosen to vary with small while the robot is driving normally. Just after the robot
speed. Immobilization is not detected if the robot is braking stops at t ~ 14.5 s, suspension displacement creates a small
(i.e. vbx > rω). The above technique represents one possible spike in the disturbance. The bottom plot shows the EMA of
criterion for detecting immobilization which has worked well the wheel slip. While driving normally, the wheel slip is
in practice; however other criteria are possible. estimated at approximately 3%, which is physically
reasonable. The increased slip while accelerating and
III. EXPERIMENTAL RESULTS braking is also expected. The detector correctly labeled the
unconstrained driving as driving normally.
A. Robot Description After t ~ 16 s the velocity estimate shows that the robot
An autonomous mobile robot developed for the DARPA accelerates against the spring, but quickly becomes
LAGR (Learning Applied to Ground Robots) program [26] immobilized. The disturbance estimate approaches a near-
has been used to experimentally validate the algorithm (Fig. constant resistive value ranging from -2.8 to -3.1 m·s-2 while
4). The robot is 1.2 m long x 0.7 m wide x 0.5 m and has the the robot is immobilized, before returning to zero when the
kinematic configuration discussed in Section IIA. The robot wheels stop spinning. During this test, the spring scale
is equipped with 4096 count per revolution front wheel measured a 325 N force holding the robot back. The
encoders, an Xsens MT9 IMU, and a Garmin GPS 16 equivalent body acceleration for the 117 kg robot is 2.8 m·s-
2
differential GPS. The robot has been used to collect data to , which closely agrees with the estimated disturbance. The
process offline using a Matlab implementation of the slip slip EMA quickly approaches 100% and the detector
detector, as well as to run an online C++ implementation on identifies the robot as immobilized at t = 17.56 s.
one of the robot’s 2.0 GHz Pentium M computers.
B. Algorithm Performance
Fig. 5. Example of robot driving normally, then becoming immobilized.
The algorithm was applied to 21 experimental test runs. GPS velocity estimates for this test were available at 1 Hz,
During these tests, the robot traveled approximately 120 slower than desired for detection. Additionally, GPS returns
meters over a range of terrain types including loose mulch, the average velocity over the previous time step, and thus the
loose gravel over hard dry soil, mud, and various grasses. measurement is truly accurate for 0.5 seconds prior to the
The robot was driven at speeds ranging from 0.1 m/s to 1 reported measurement time. In this example, immobilization
m/s. The test runs include 20 instances of the robot coming could not be detected by GPS until t ~ 18.5 s, nearly one
to a complete stop with the wheels still spinning, which were second slower than the proposed algorithm.
initiated by holding the robot back using a spring scale.
The slip detector correctly identified each of these 20 C. Algorithm Performance without GPS
instances as immobilized with an average detection time of The 21 experimental datasets were reprocessed without
0.4 seconds. All data with the robot driving freely or sitting including GPS velocity measurements (i.e. using wheel
at rest was correctly labeled as normal driving, with the encoder and IMU measurements only). The algorithm again
2728
ThC11.1
2729