Project in Control Theory - FRT090

Reversing with a trailer

Per Ganestam - Nils Bengtsson
August 21, 2009

1
 Abstract
When reversing a vehicle with a trailer the driver has to take action to
keep the trailer in a chosen path. If the driver do not control the trailer
it will become unstable and so to say, fall off, towards the vehicle. If
this is not taken care of in time the angle between the vehicle and the
trailer will get more narrow and the joint connecting the vehicle and the
trailer will probably break or even worse the trailer or the vehicle can
take damage. This report describes an automated control solution to this
problem. With the help of a small remote controlled robot named RBbot,
a PD controller is developed to stabilize a trailer when going in reverse.

2
Contents
1 Introduction 4

2 RBbot 4
2.1 Micro controllers . . . . . . . . . . . . . . . . . . . . . . . . . . . 5
2.1.1 ATMEL AVR ATmega16 . . . . . . . . . . . . . . . . . . 5
2.1.2 ATMEL AVR ATmega8 . . . . . . . . . . . . . . . . . . . 5
2.1.3 T-Mote Sky . . . . . . . . . . . . . . . . . . . . . . . . . . 5
2.2 Contiki . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5
2.3 Main node . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5
2.4 Motor . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6
2.5 Angle sensor . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6
2.6 I2C . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6
2.7 Joystick . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7

3 The Model 8
3.1 Mathematical model . . . . . . . . . . . . . . . . . . . . . . . . . 8
3.2 Simulink model . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10

4 Implementation 11
4.1 Joystick . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11
4.2 Angle sensor . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11
4.3 Motors . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11
4.4 Main node . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11
4.5 Discretization . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 12
4.6 Tuning . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 12
4.7 Code extracts . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 13

5 Conclusion 14
5.1 Future improvements . . . . . . . . . . . . . . . . . . . . . . . . . 14

3
1 Introduction
In this project a controller that stabilize a vehicle with a trailer going in reverse
has been found and implemented. The problem with reversing a trailer is that
it becomes an unstable system, hence the trailer will fall towards the vehicle.
This is the same as the well known problem of reversing a car with a trailer. If
no action is taken by the driver the angle between the car and the trailer will
narrow and eventually the only solution to straighten up the car and trailer is
to go in forward direction.
In order to find and implement a suitable controller to the stated problem a
robot called RBbot is used. The RBbot is a small three wheeled robot designed
and constructed at the department of Automatic Control, LTH. It contains of a
few programmable micro controllers and two individual controlled motor driven
wheels as well as a remote joystick for steering. The RBbot used in this project
was modified with a trailer connected to it and with an angle sensor in the joint.
In the process of designing a controller for the RBbot, a mathematical model
was found and realized in Simulink. There different control ideas were tested
with different initial conditions and speeds to determine a controller and its pa-
rameters. When a candidate to control the system was found it was implemented
and further tested on the RBbot until a satisfying behavior was achieved.

2 RBbot

Figure 1: RBbot with trailer and joystick

4
2.1 Micro controllers
2.1.1 ATMEL AVR ATmega16
One ATMEL AVR ATmega16, called master, is mounted on the RBbot. The
masters task is to read the angle sensor and send this data to the T-Mote Sky
on the RBbot. In addition it can also send information about the battery status
and switch the two LEDs on the top of the RBbot on and off.

2.1.2 ATMEL AVR ATmega8

The RBbot use two ATmega8 micro controllers from ATMEL. One for each
motor. These receive data to set a speed reference or return the current step
position of the motors respectively. These also contain the wheel control which is
a PI signal feedback controller and a low pass filter to follow the speed reference.
This make sure that a given speed is maintained without stationary errors. The
PI controller affects the dynamics of the RBbot slightly but this is covered by
the model for the main controller.

2.1.3 T-Mote Sky

There is two T-Mote Sky platforms, one on the RBbot and one attached to the
joystick. The one mounted on-board works as the main processor, calculating
the control signal and communicating with the three AVR micro controllers. The
other one is attached to the remote joystick. This one converts the analogue
signals from the joystick to digital signals and transmit them to the RBbot. A
T-Mote Sky is a complete platform with a real-time operating system, in our
case one called Contiki, some LED indicators and a wireless communication
module. The wireless communication use the TCP/IP protocol.

2.2 Contiki
Since Contiki is a real-time operating system designed to some specific platforms,
among them the T-Mote Sky, there is already a lot of predefined functions and
hardware initializations. Contiki works with processes that reacts on different
events. A process can be triggered from e.g. a timer event or a TCP/IP event.
To send and receive data between two T-Mote Sky platforms is trivial because of
the built in communication functions. Also the I2C protocol is implemented in
Contiki and ready to use. All code is in C and compiled with the GCC compiler
and a Contiki add on.

2.3 Main node

The T-Mote Sky on the RBbot, from now on called the main node, do most of
the work. Initially it calibrates both the joystick and the angle sensor. Hence
the Rbbot and the trailer must be in line with each other and the joystick remain
untouched during start up of the RBbot. The main node has two different events
that can occur. It can react on a TCP/IP event triggered from the joystick or
it will calculate a new control signal on a timer event. A timer will finish every
50 ms to create the control event.

5
2.4 Motor
The motor is a DC step motor servo type and is programmed to follow a speed
reference set by the user. There is one DC motor to each of the two wheels on
the RBbot. To be sure that the speed reference is followed correct there is a
position feedback from the motors with a resolution off 80 steps per revolution.

Figure 2: One motor for each wheel

2.5 Angle sensor

To control the RBbot in a chosen direction, some information about the angle
between the Rbbot and the trailer is needed. This is done with an angle sensor
in the joint where the trailer is attached to the RBbot. The angle sensor is a
simple potentiometer and after calibration, an angle between -90 and 90 degrees
corresponds to a value between -270 and 270.

2.6 I2C
All communication between the T-Mote Sky and the AVR:s on the RBbot goes
via an I2C bus. This is a simple but efficient communication protocol that send
eight bits of data at the time. Since most data is larger than eight bit it has to
be cut in smaller pieces before sending it over the I2C bus. All platforms and
AVRs have the I2C protocol implemented.

6
 Figure 3: The angle sensor in the joint between the RBbot and the trailer

2.7 Joystick
The joystick has two axis, one axis is for forward and reverse speed and one is
for turning. The joystick is attached to a T-Mote Sky platform. This platform
has an internal AD converter which read and translate the positions of the
joystick with an x-value and a y-value each between negative one and one. This
information is then sent through a wireless communication to the T-Mote Sky
attached on the RBbot. A dead zone around zero (in all directions) had to
be implemented in the joystick to prevent none zero data to be sent when the
joystick is centered. The joystick also have two buttons implemented. One of
them switch the feedback on and off on the RBbot and the other activates the
Indiana Jones theme song.

7
 Figure 4: Joystick

3 The Model
3.1 Mathematical model
The idea behind the model is to find an equation that describe how β change
when the RBbot move around. The speeds of the two wheels were not modelled
individually but instead the average of the two were chosen as the speed, V ,
and their difference, ∆V , as the turning speed which also is the control signal.
The RBbot movement can be divided into two problems, straight forward
and rotation. The first three equations describe what happens with β when the
connection joint move dx straight forward.

sin(β + dβ)c = sin(β)c + sin(β)cos(β)dx (1)

1
sin(β)cos(dβ) + cos(β)sin(dβ) = sin(β) + sin(β)cos(β)dx (2)
c
1
dβ = sin(β)dx (3)
c
The following equations describe what happens with β when the connection
joint move sideway, which is the case when the RBbot rotate.

cos(β − dβ)c = cos(β)c + sin(β)cos(β)dy (4)

1
cos(β)cos(dβ) + sin(β)sin(dβ) = cos(β) + sin(β)cos(β)dy (5)
c
1
dβ = cos(β)dy (6)
c
The next two equations describe how the connection joint move when the
RBbot drive.

dx = V (7)

8
Figure 5: a) Robot wheel distance center to center. b) Robot arm. c) Trailer
arm. β) Robot to trailer angle.

b
dy = ∆V (8)
a
The angle β is also affected by the rotation of the RBbot itself which add a
third component to the final equation.
∆V
dβ = (9)
a
Adding up all three dβ result in the final state equation.
V 1 b
β̇ = sinβ − (1 + cosβ)∆V (10)
c a c
After contemplated the above system it is clear that, when moving with
constant speed and without any turns, the system is stable in forward speed
direction and unstable in backward speed direction. It actually have some simi-
larity with the pendulum, were the inverted unstable pendulum correspond to a
backward speed direction and the hanging down stable pendulum to a forward
speed direction. The main problem with the project is also revealed here, when
moving in reverse the trailer will ”fall off” from the intended direction if no
compensation is made.

9
Figure 6: Left: The arrow indicate the trailer center movement sideway as
the connection joint move dx forward. Right: The arrow indicate the trailer
center movement forward as the RBbot rotate and the connection joint move
dy sideway.

3.2 Simulink model

A first simulation using the model given in the previous section resulted in an
easily controlled system where only an error feedback was needed for stabiliza-
tion. But this first model was to simple to result in a suitable controller for
the RBbot. To improve the model the dynamics of the motors had to be found.
This is done by some step response experiments. To find the system correspond-
ing to the motors dynamics a small program was implemented on the RBbot,
measuring and logging the speed during the experiments. The experiments was
done by driving the RBbot in the forward stable direction with the trailer con-
nected. Since the dynamics is different at different speeds a series of tests had
to be done, starting with low speed and in steps increasing the speed. A first
order linear system was approximated to fit the step response experiment data.
In the process of finding a good controller a range of various linear PID con-
trollers were tested in different starting angles and speeds. These experiments
resulted in three PD controllers suitable in three speed intervals. The reason
why no I-part was needed was because of the integrator in the wheel control in
the ATmega8s.

10
 Figure 7: Simulink model

4 Implementation
Regarding all programmable micro controllers, some base source code was ob-
tained and used as a start for the implementation in this project. A part of
the work process was to study and understand the existing codes. After this
improvement, redesigning and adding of new functions were done.

4.1 Joystick
Included in the joystick source code was hardware initializations of the T-Mote
Sky and an read out from the AD converter connected to the stick.
The first thing to be added was a dead zone compensation to remove un-
wanted signals due to the play in the stick. Initializations and read commands
for the two buttons were written and at last the message structure containing
all the joystick data was augmented with the button variables.

4.2 Angle sensor

The only source code to write on the master regarding the angle sensor was the
I2C commands to transmit the angle value to the main node.

4.3 Motors
No changes had to be made in the code for the ATmega8 micro controllers due
to some resent ambitious redesigning including a new PI controller combined
with a low pass filter.

4.4 Main node

In the main node all data regarding angle, speed, and joystick information is
collected and processed. This also where the PD controller to stabilize the trailer
is implemented. The entire process is scheduled for 50 ms sample time.

11
 Figure 8: Step response

4.5 Discretization
In order to implement a PD controller digital it has to be discretizised. The
P-part is trivial with the following discretization.

P (tk ) = K(yref (tk ) + y(tk )) (11)

Where K is the proportional constant, yref is the current reference value and
y(tk ) is the angle in the joint between the trailer and the RBbot at time tk .
However, the D-part is a bit more problematic due to the fact that there is a lot
of noise introduced when differentiating a discrete signal. The first experiment
implementing a controller with a D-part gave a to noisy control signal due to
the lack of a low pass filter. Instead the implementation of the D-part used on
the RBbot is a backward difference discretization with a low pass filter included.
Td KTd N
D(tk ) = D(tk−1 ) − (y(tk ) − y(tk−1 )) (12)
Td + N h Td + N h
Where Td is the derivative constant, K is the gain, h is the sample time and N
is the low pass filter constant. The constant N is typically in the range [1, 10]
where a value of one is no filter at all.

4.6 Tuning
The controller constants found from the simulation was used as a first try to get
a working controller on the RBbot. These constants were not too bad and only
some small changes had to be done in order to get the system tuned up in all
three PD controllers. One while standing still, one for moving forward and one
for reversing. As expected some new problems were introduced on the RBbot
that was not covered in the model.

12
 One of these problems were that when a too rapid change in angle reference
was done by the driver, the RBbot would overshout and then the compensation
made by the driver caused driver-induced oscillations. The chosen solution to
this problem was to implement a low pass filter on the angle reference signal.
This made the system slightly slower but fast enough for convenient driving.
Another problem was introduced during high acceleration. The controller
were unable to keep the system stable and the trailer fell towards the RBbot.
The control signal, hence the turning speed, is applied by increased speed on
one wheel and decreased on the other. The problem was due to saturation in
acceleration which resulted in reduced control action on the wheel that tries to
accelerate even more in speed direction. To solve this problem two new functions
had to be implemented. First the acceleration had to be messured. This was
done by reading and differentiate the wheel speed and pass it through a low
pass filter for noise reduction. Secondly, the control action is transfered to the
retarding wheel. This is done in three steps. In low acceleration no control
action is transfered and in high acceleration all control action is transfered.
In between, the proportion of control action transfered is decided by a ramp
function.

4.7 Code extracts

The following code is the low pass filter used on the angle reference value on
RBbot.
filter_input = angle_ref;
filter_reg = filter_reg - (filter_reg >> FILTER_SHIFT_REF) +
filter_input;
filter_output = filter_reg >> FILTER_SHIFT_REF;
angle_ref_filter = filter_output;
The implementation of the D-part in the PD controller where on off is a help
variable to prevent divide by zero.
D = on_off*(Td*D_prev/(Td + N_filt*h) - (K*Td*N_filt) *
(prev_ref_error-ref_error)/(Td + N_filt*h));
This piece of code is the implementation of the acceleration compensation.
The variable prop spans from zero to one where zero is full control action on the
left wheel and one is full control action on the right. The ramp function is used
between 0.1 − 0.8 m/s2 and for higher accelerations full control action transfer
is applied. The constant 0.7143 give the ramp function the correct gain. When
the speed is in the stable direction no compensation has to be done.
if(ABS(acc) < 0.1)
prop = 0;
else if(ABS(acc) < 0.8)
prop = (ABS(acc) - 0.1)*0.7143;
else
prop = 0.5;

if(ref_error >= 0)
prop = 0.5 - prop;

13
 else
prop = 0.5 + prop;

if(speed > 0)
prop = 0.5;

5 Conclusion
The controller managed to stabilize the trailer as intended in all possible speeds
and accelerations. There was a big difference in the ability to drive the RBbot
in reverse with the feedback switched on. The Simulink model turned out to
be useful in the level of complexity it was designed for. Many different control
designs and ideas could easily be tested to find some suitable controllers. The
problems that was found later when implementing on the RBbot could have
been covered by the model but perhaps with a greater effort than testing and
designing it on-board.
The main node T-Mote Sky failed to run the entire process with a sampling
time of 10 ms which was tried at first. The final choice of sampling time, 50 ms,
turned out to good both for the discretization of the controller and the time to
make all calculations and communications.
Contiki was a good help when using the T-Mote Sky. It made the handle
of threads, float numbers and TCP/IP communication easy. In disadvance the
lack of documentation made the learning process slow, also the way of defining
variables was different to ordinary C-programming.

5.1 Future improvements

A possible improvement could be to put the acceleration estimations directly
on the ATmega16 and improve the algorithm with a polynomial approximation
if the processor resources is enough.
To get an even better controller a more advanced gain scheduling could be
implemented with more controllers at different speeds and with an interpolation
between them.
During high acceleration the RBbot might change direction slightly. This
could be solved with a feedback on the wheel positions to compensate for the
error.
There is a possible risk of stationary errors in the angle due to the lack
of an integral part in the controller. This could be because of changes in the
surface causing different frictions. A solution could be an adaptive control which
increase the gain when there is more friction.

14