A Motion Model for Articulated Vehicles and a Distributed ...
A Motion Model for Articulated Vehicles and a Distributed ...
A Motion Model for Articulated Vehicles and a Distributed ...
You also want an ePaper? Increase the reach of your titles
YUMPU automatically turns print PDFs into web optimized ePapers that Google loves.
A<strong>Motion</strong><strong>Model</strong><strong>for</strong><strong>Articulated</strong><strong>Vehicles</strong><strong>and</strong>a<br />
<strong>Distributed</strong> Acceleration Measurement System<br />
Abstract—A motion model is developed <strong>for</strong> a trailer-truck<br />
combination that fuses the differential equations governing the<br />
motion of the trailer with the dynamics model of the truck. Three<br />
different parametrisations of the vehicle state are proposed. The<br />
resulting model is a stepping stone <strong>for</strong> a filtering solution <strong>for</strong><br />
tracking the position <strong>and</strong> attitude of articulated vehicles, such<br />
as trailer-trucks, articulated buses or work machines, or even<br />
trains.<br />
Niilo Sirola <strong>and</strong> Jarkko Rouhe<br />
I. INTRODUCTION<br />
FOR vehicle positioning <strong>and</strong> tracking, it is often convenient<br />
to model the vehicle as a singular point <strong>and</strong> compute<br />
its position, velocity, or other quantities of interest. When<br />
building positioning or tracking systems <strong>for</strong> articulated vehicles,<br />
such as trailer-trucks, articulated buses, work machines or<br />
trains <strong>for</strong> example, it may be advantageous to model some the<br />
vehicle as a system of connected bodies instead of abstracting<br />
it as a single point. Additionally, in our application the sensors<br />
are not attached to a single rigid body but to different parts<br />
of the vehicle, so that it is necessary to solve <strong>for</strong> the attitude<br />
of the vehicle in order to fuse the measurements. It would<br />
also be possible to refine the models all the way to tires <strong>and</strong><br />
suspension, but this level of detail is not necessary here.<br />
The motion of trailer-trucks, or articulated vehicles in general,<br />
has been studied <strong>for</strong> the purposes of building automated<br />
vehicle control systems <strong>and</strong> driver assistance devices [1].<br />
There are some theoretical papers modelling the motion of an<br />
idealized articulated vehicle <strong>for</strong> e.g. studying jack-knifing [2]<br />
or deriving rules-of-thumb <strong>for</strong> road design [3]. Often, the goal<br />
of modelling is to build some control system <strong>for</strong> the vehicle,<br />
such as in [4] or [5].<br />
This study aims at tracking the motion <strong>and</strong> behaviour of<br />
a trailer-truck by using several accelerometers installed in<br />
different parts of the vehicle. We present a dynamics model<br />
<strong>for</strong> a trailer-truck in a <strong>for</strong>m that is applicable in nonlinear<br />
filtering using Extended Kalman filter of Particle filters. To our<br />
knowledge there are no articulated vehicle models applicable<br />
to filtering presented in the literature so far.<br />
II. RAW DATA<br />
Test data was collected from a fuel trailer-truck driving its<br />
normal scheduled route in July-August 2009. The trailer-truck<br />
(hereafter referred to as the vehicle) was equipped with four<br />
independent sensor packs, one of which was a gps receiver<br />
N. Sirola was with Department of Mathematics, Tampere University of<br />
Technology, Finl<strong>and</strong>. He is now with Taipale Telematics, PO Box 42, 33901<br />
Tampere, Finl<strong>and</strong> (e-mail: niilo.sirola@taipaletelematics.com)<br />
J. Rouhe is with Taipale Telematics, Finl<strong>and</strong>.<br />
Manuscript received December 1, 2009; revised March 16, 2010.<br />
fixed inside the cab, <strong>and</strong> the rest three were st<strong>and</strong>alone batterypowered<br />
3D accelerometer packs (Taipale Telematics Sensior),<br />
one fixed rigidly to the side of the truck <strong>and</strong> two to the trailer.<br />
See Figure 1 <strong>for</strong> the approximate dimensions of the vehicle<br />
<strong>and</strong> the locations of the sensors.<br />
For practical reasons, each sensor logged its measurements<br />
on a SD memory card, <strong>and</strong> the data from different sensors<br />
was combined in post-processing. As the clocks in the sensors<br />
were not exactly synchronized, the time offsets of the data<br />
streams were compensated off-line. The gps position <strong>and</strong><br />
velocity were logged once a minute, <strong>and</strong> the raw accelerations<br />
from the sensors at a frequency of 10 Hz. The acceleration<br />
measurements were further down-sampled to one measurement<br />
per second, with the measurement value averaged from the raw<br />
measurements occurring during that second.<br />
Although inertial navigation systems are well-understood,<br />
the data available in this set-up is somewhat challenging<br />
because there are no gyroscopes <strong>and</strong> the alignment of each<br />
of the three accelerometers has to be implicitly solved. Also,<br />
the noise level of the accelerometers used is quite high <strong>for</strong><br />
traditional INS approaches.<br />
III. DYNAMICS OF AN ARTICULATED VEHICLE<br />
Figure 1 shows the structure of the truck-trailer vehicle<br />
under study. State variables consist of the 2D position p <strong>and</strong><br />
velocity v of the centre point of the front axis, the heading<br />
angle of the truck ψ1, thedollyψ2 <strong>and</strong> the trailer ψ3. The<br />
steering angle of the front wheels is not a part of the state,<br />
<strong>and</strong> it is assumed to be parallel to v. For simplicity, it is<br />
assumed that the steering angle is independent of the heading<br />
of the truck, i.e. the front wheels are like casters. Additional<br />
nuisance variables to be solved are the biases <strong>and</strong> alignment<br />
errors of the accelerometers, as well as the longitudinal <strong>and</strong><br />
latitudinal tilt of the road surface that is assumed to show up<br />
as a common offset in all the accelerometers.<br />
When none of the tires slip, the track of the centre point of<br />
the front axis p(t) determines the track <strong>and</strong> heading of all of<br />
the trailers according to the differential equations [3]:<br />
˙ψ1 = − u⊥ 1<br />
˙ψ2 = − u⊥ 2<br />
˙ψ3 = − u⊥ 3<br />
L1<br />
L2<br />
· ˙ p0<br />
· ˙ p1<br />
where u ⊥ i = − sin ψi cos ψi<br />
.Sincethepositionsp1 <strong>and</strong><br />
p2 depend on the position of the front axis p0 <strong>and</strong> the lengths<br />
L3<br />
· ˙<br />
p2<br />
,<br />
1
s3<br />
B3<br />
p3<br />
W<br />
u3<br />
ψ3<br />
Fig. 1. The truck, dolly <strong>and</strong> semitrailer under study, with the locationsofthesensorunits.<br />
L3<br />
<strong>and</strong> angles of connecting structures, we can write<br />
˙ψ1 = − u⊥ 1<br />
L1<br />
· ˙p0<br />
˙ψ2 = − u⊥ 2 · (˙p0 + B1 ˙ ψ1u ⊥ 1 )<br />
L2<br />
˙ψ3 = − u⊥ 3 · (˙p0 + B1 ˙ ψ1u ⊥ 1 + B2 ˙ ψ2u ⊥ 2 )<br />
This model gives a constraint that the motion of the front axis<br />
of the vehicle <strong>and</strong> the positions <strong>and</strong> headings of all the moving<br />
parts have to satisfy. Un<strong>for</strong>tunately, the relationship is through<br />
asystemofdifferentialequationsthatdonothaveananalytic<br />
solution, so it is not trivial to implement.<br />
There are three different approaches to implement the<br />
constraint:<br />
1) Choose the variables such that they are independent, <strong>and</strong><br />
represent the dependent variables as functions of independent<br />
variables. This will however lead to very complicated<br />
measurement models <strong>for</strong> the sensors mounted<br />
in the trailer.<br />
2) Use the equation as a constraint <strong>and</strong> en<strong>for</strong>ce it between<br />
iterations. This has the drawback of requiring that ψi<br />
˙<br />
are included as variables, which increases the state<br />
dimension <strong>and</strong> requires a dynamics model <strong>for</strong> them as<br />
well.<br />
3) Use the propagation equation directly in the dynamics<br />
model. Then ψi will be variables but their derivatives<br />
need not be. The time propagation step is then done<br />
through solving the differential equation system <strong>for</strong> one<br />
step. This approach shows most promise <strong>for</strong> subsequent<br />
filtering applications.<br />
Depending on the parametrisation <strong>and</strong> the choice of nuisance<br />
parameters, the state dimension is of the order of 20–<br />
30. The resulting filtering problem is very challenging <strong>for</strong><br />
nonlinear filters, because of the high state dimension, <strong>and</strong><br />
additionally both the dynamics model <strong>and</strong> measurement model<br />
are very nonlinear <strong>and</strong> computation-intensive.<br />
L3<br />
.<br />
s2<br />
p2<br />
B2<br />
(1)<br />
ψ2<br />
u2<br />
L2<br />
p1<br />
B1<br />
ψ1<br />
u1<br />
L1<br />
s1<br />
p0<br />
IV. MOTION MODEL<br />
In this work, the state xk <strong>and</strong> the measurements yk of the<br />
vehicle are assumed to follow the models<br />
xk+1 = fk(xk,wk)<br />
yk = hk(xk)+vk<br />
where the <strong>for</strong>mer is called the dynamics model <strong>and</strong> the latter<br />
the measurement model. Thestochasticnoisetermsvk<strong>and</strong> wk<br />
are assumed white, zero-mean, <strong>and</strong> independent of each other<br />
<strong>and</strong> of the initial state x0. Thispaperconcentratesonfinding<br />
an applicable <strong>and</strong> tractable <strong>for</strong>m <strong>for</strong> fk.<br />
In continuous <strong>for</strong>m, the dynamics model can be written as<br />
⎡ ⎤ ⎡ ⎤<br />
p0 ˙p0<br />
⎢ ˙p0 ⎥ ⎢ ¨p0 ⎥<br />
⎢ ⎥ ⎢ ⎥<br />
˙x = ∂<br />
∂t<br />
= f(t, x)<br />
˙p0<br />
⎢ ¨p0 ⎥ ⎢g(t,<br />
x) ⎥<br />
⎢ ⎥ ⎢ ⎥<br />
⎢<br />
⎢ψ1⎥<br />
⎢ ψ1<br />
˙ ⎥<br />
⎥ ⎢ ⎥<br />
⎢ψ2⎥<br />
⎢ ⎥ = ⎢ ψ2<br />
˙ ⎥<br />
⎢ψ3⎥<br />
⎢<br />
⎢ ⎥ ⎢<br />
˙ ⎥ ψ3 ⎥<br />
⎢ φ ⎥ ⎢<br />
⎥ ⎢ 0 ⎥<br />
⎣ θ ⎦ ⎣ 0 ⎦<br />
b 0<br />
where the function g(t, x) is used to inject the steering<br />
behaviour of the truck. One example would be g(t, x) =<br />
(u1 · ˙p0) ˙ ψ1<br />
The time propagation relations can be written as [6]<br />
˙x(t) =f(x(t),t)<br />
˙<br />
P (t) =F [t; x(t)]P (t)+P (t)F T [t; x(t)] + GQG T<br />
where F = ∂<br />
∂x f(x, t). Thetimeupdatestepcanbesolved<br />
from this with numerical differential equation solvers methods.<br />
Let the state vector be<br />
V. MEASUREMENT MODEL<br />
x = p0, ˙p0, ¨p0, ψ1, ψ2, ψ3, ˙<br />
ψ1, ˙<br />
ψ2, ˙<br />
ψ3, φ, θ, b ,<br />
where φ is the rotation of the vehicle around the x axis (roll)<br />
<strong>and</strong> θ around the y axis (pitch), which depend on the slope<br />
2<br />
(2)
g<br />
g<br />
g<br />
0.2<br />
0.1<br />
0<br />
−0.1<br />
−0.2<br />
0.2<br />
0<br />
−0.2<br />
0.2<br />
0.1<br />
0<br />
−0.1<br />
−0.2<br />
along−track acceleration<br />
12:34 12:35<br />
cross−track acceleration<br />
truck trailer front trailer rear<br />
12:34 12:35<br />
vertical acceleration<br />
12:34 12:35<br />
Fig. 2. Accelerations at the different sensors during three minutes. The vehicle starts from rest at a traffic light, makes atightrightturn<strong>and</strong>acceleratesto<br />
about 60 km/h. After 12:34 starts a ramp to a motorway that curves to the right.<br />
of the road <strong>and</strong> are assumed constant in both the truck <strong>and</strong><br />
trailer. The state vector additionally contains the biases b of<br />
each accelerometer axis, a total of 9 additional variables in<br />
this case.<br />
The attitude of the first accelerometer is C(ψ1), thus it<br />
measures<br />
a1 = C(ψ1) T (¨s1 + g)+b1.<br />
Note that the sensor logic automatically adds one g to the z<br />
axis measurement to compensate <strong>for</strong> the normal <strong>for</strong>ce when<br />
the sensor is level, i.e. 0=φ = θ. Thishoweveraddsbiasto<br />
measurements whenever the sensor is tilted. Here we used the<br />
shorth<strong>and</strong> g = 0 0 g T .<br />
Exp<strong>and</strong>ing the second derivative of the sensor position <strong>for</strong><br />
each sensor(see Appendix 1 <strong>for</strong> details), the measurement<br />
models <strong>for</strong> the three sensors are<br />
a1 = C(ψ1) T ¨p0 + ˙<br />
2<br />
ψ1 I2C(ψ1)q1 + g + b1 + v1<br />
a2 = C(ψ3) T ¨p0 + B1 ˙<br />
2<br />
ψ1 I2C(ψ1)u1<br />
+ B2 ˙<br />
2<br />
ψ2 I2C(ψ2)u2 − ˙<br />
2<br />
ψ3 I2C(ψ3)q2 + g + b2 + v2<br />
a3 = C(ψ3) T ¨p0 + B1 ˙<br />
2<br />
ψ1 I2C(ψ1)u1<br />
+ B2 ˙<br />
2<br />
ψ2 I2C(ψ2)u2 − ˙<br />
2<br />
ψ3 I2C(ψ3)q3 + g + b3 + v3.<br />
(3)<br />
The gps position <strong>and</strong> velocity measurements can be written<br />
simply as<br />
pgps = p0 + v4<br />
vgps =˙p0 + v5.<br />
(4)<br />
Note that the gps receiver has already applied some kind of<br />
smoothing or filter to the position <strong>and</strong> velocity outputs, so any<br />
additional processing has to be done with care.<br />
Figure 2 shows the accelerations measured at the three<br />
sensors during a short interval of normal driving.<br />
VI. CONCLUSIONS<br />
This paper presents some initial work on building a comprehensive<br />
model <strong>for</strong> tracking <strong>and</strong> estimating the attitude of an<br />
articulated truck. The differential equations that <strong>for</strong>m the basis<br />
of the motion model are presented, <strong>and</strong> measurement equations<br />
<strong>for</strong> accelerometers fixed into arbitrary positions within the<br />
vehicle combination were derived.<br />
The current problem is very challenging <strong>for</strong> nonlinear filters,<br />
because the dimension of the state, 26, is fairly high, <strong>and</strong> both<br />
the dynamics <strong>and</strong> measurement models are very nonlinear <strong>and</strong><br />
computation-intensive. As <strong>for</strong> the subsequent filter implementation,<br />
the Unscented Kalman Filter (UKF) [7] is a promising<br />
alternative, because it does not require the derivatives of the<br />
models to be computed, <strong>and</strong> works with a moderate number<br />
of function evaluations compared to particle filters.<br />
ACKNOWLEDGEMENTS<br />
Niilo Sirola was funded by the Academy of Finl<strong>and</strong>.<br />
A. Measurement model<br />
APPENDIX<br />
Each sensor measures the net acceleration at its position in<br />
the sensor coordinates. Each sensor is assumed to be perfectly<br />
3
lined up within the body it is connected to. The acceleration<br />
measurement at ith sensor is<br />
ai = C(ψ) T (¨si + g)+bi, (5)<br />
where C(ψ) =R(ψ, φ, θ) is the rotation matrix, <strong>and</strong> the roll<br />
<strong>and</strong> pitch angles φ <strong>and</strong> θ are assumed constant.<br />
The measurement depends on the accelerations at the sensor<br />
coordinates s1, s2,<strong>and</strong>s3,whichcanbewrittenintermsofthe<br />
coordinates of the front axis <strong>and</strong> the angles <strong>and</strong> lengths in the<br />
system. Referring to Figure 1 <strong>and</strong> denote the relative position<br />
of a sensor in the coordinates of the part of the vehicle it is<br />
attached to with qi, theabsolutepositionsofthesensorsare<br />
s1 = p0 + C(ψ1)q1<br />
s2 = p0 − C(ψ1)B1u1 − C(ψ2)B2u2 + C(ψ3)q2<br />
s3 = p0 − C(ψ1)B1u1 − C(ψ2)B2u2 + C(ψ3)q3<br />
Then, assuming the angle velocities are constant, that is,<br />
¨ψi =0,weget<br />
¨s1 =¨p0 + ˙<br />
2 ′′<br />
ψ1 C (ψ1)q1<br />
¨s2 =¨p0 − B1 ˙<br />
2 ′′<br />
ψ1 C (ψ1)u1<br />
− B2 ˙<br />
2 ′′<br />
ψ2 C (ψ2)u2 + ˙<br />
2 ′′<br />
ψ3 C (ψ3)q2<br />
¨s3 =¨p0 − B1 ˙<br />
2 ′′<br />
ψ1 C (ψ1)u1<br />
− B2 ˙<br />
2 ′′<br />
ψ2 C (ψ2)u2 + ˙<br />
2 ′′<br />
ψ3 C (ψ3)q3<br />
Note that we can simplify the second derivatives of the<br />
rotation matrices a bit by noting that<br />
⎡<br />
⎤<br />
cos ψ − sin ψ 0<br />
C(ψ) = ⎣sin ψ cos ψ 0⎦<br />
R(0, φ, θ)<br />
0<br />
C<br />
0 1<br />
′′ ⎡<br />
− cos ψ<br />
(ψ) = ⎣− sin ψ<br />
sin ψ<br />
− cos ψ<br />
⎤<br />
0<br />
0⎦<br />
R(0, φ, θ)<br />
0<br />
⎡<br />
1<br />
= − ⎣0 0<br />
1<br />
0 0<br />
⎤<br />
0<br />
0⎦C(ψ)<br />
=−I2C(ψ)<br />
0 0 0<br />
Thus, substituting to Eq. (5), the measurements at each<br />
sensor are<br />
a1 = C(ψ1) T ¨p0 + ˙<br />
2<br />
ψ1 I2C(ψ1)q1 + g + b1<br />
a2 = C(ψ3) T ¨p0 + B1 ˙<br />
2<br />
ψ1 I2C(ψ1)u1<br />
+ B2 ˙<br />
2<br />
ψ2 I2C(ψ2)u2 − ˙<br />
2<br />
ψ3 I2C(ψ3)q2 + g + b2<br />
a3 = C(ψ3) T ¨p0 + B1 ˙<br />
2<br />
ψ1 I2C(ψ1)u1<br />
+ B2 ˙<br />
2<br />
ψ2 I2C(ψ2)u2 − ˙<br />
2<br />
ψ3 I2C(ψ3)q3 + g + b3<br />
These <strong>for</strong>mulae may be simplified further, but it would make<br />
it harder to modify them if the sensor configuration changes.<br />
REFERENCES<br />
[1] C. Chen <strong>and</strong> M. Tomizuka, “Dynamic modling of articulated vehicles <strong>for</strong><br />
automated highway systems,” in Proceedings of the American Control<br />
Conference, Seattle,Washington,June1995,pp.653–657.<br />
[2] T. V. Fossum <strong>and</strong> G. N. Lewis, “A mathematical model <strong>for</strong> trailer-truck<br />
jackknifing,” SIAM Review, vol. 23, no. 1, pp. 95–99, 1981. [Online].<br />
Available: http://www.jstor.org/stable/2029841<br />
[3] G. E. Prince <strong>and</strong> S. P. Dubois, “Mathematical models <strong>for</strong> motion of the<br />
rear ends of vehicles,” Mathematical <strong>and</strong> Computer <strong>Model</strong>ling, vol.49,<br />
no. 9-10, pp. 2049–2060, May 2009.<br />
[4] M. Sampei, T. Tamura, T. Kobayashi, <strong>and</strong> N. Shibui, “Arbitrary path<br />
tracking control of articulated vehicles using nonlinear control theory,”<br />
IEEE Transactions on Control Systems Technology, vol.3,no.1,pp.125–<br />
131, 1995.<br />
[5] C. Chen <strong>and</strong> M. Tomizuka, “<strong>Model</strong>ing <strong>and</strong> control of articulated vehicles,”<br />
University of Cali<strong>for</strong>nia, Berkeley, Cali<strong>for</strong>nia PATH Research Report<br />
UCB-ITS-PRR-97-42, 1997.<br />
[6] P. S. Maybeck, Stochastic models, estimation <strong>and</strong> control, Volume 2.<br />
Academic Press, 1982.<br />
[7] S. Särkkä, “On unscented Kalman filtering <strong>for</strong> state estimation of<br />
continuous-time nonlinear systems,” IEEE Transactions on Automatic<br />
Control, vol.52,no.9,pp.1631–1641,September2007.<br />
4