INDEX
Table of Contents |
---|
THE GENERAL PROBLEM
The goal of the project is to set up an experiment that proves the effectiveness of an algorithm for collision avoidance at intersections. Three cars run on a test-bed following three different paths that all intersect in a single point. Cars are controlled by an on-board computer. Cars positions are measured by six cameras on the ceiling. Cars speed can be measured by an encoder mounted on the rear axis.
The ideal final experiment will have an autonomous vehicle and 2 human-driven ones. The experiment will last few minutes and it will be considered successful if the vehicles will never bump into each other, given few constraints on the path following performance (e.g. the distance of the vehicle from its path cannot be greater than 30cm) that will have to be satisfied by both the autonomous and the human-driven vehicles.
The algorithm requires the prediction of the time at which a car enters and exits the intersection. The algorithm is robust to noise, meaning that it takes into account the existence of disturbances that cannot be controlled, given that those disturbances are limited and monotonic with respect to the position along the path y. It is assumed that the input signal (i.e. the torque applied by the motor) is also monotonic with respect to y. The main tasks of this work are thus to:
- determine and limit the disturbances which make the prediction uncertain to a degree that make the experiment unfeasible;
- identify a model of the system that is suitable for the long term prediction that the algorithm requires.
CURRENT LAB PROBLEMS
- Cameras should be screwed to the metal support. Now every time they move, the whole calibration procedure must be done again. An extrinsic calibration (at least) should probably be performed again after this job.
- The two chargers close to the whiteboard seem to have some wiring problem. Sometimes the led is green while cars are connected (i.e. they're done charging) but it turns red when you move the wires.
- Sometimes the connection between computers and cars is lost. There are 2 main cause.
- There are 2 critical functions in the CPS code in main.cpp that occasionally takes up to 300-400ms in computer 0. This does not happen (or happens extremely rarely) with computers 1 and 2. I think the fault may simply be the computer's hardware because computer 0 is the slowest among the three. The two critical functions are flycaptureGrabImage2() and cvWaitKey(). The CPS currently prints a warning when one of this two functions takes more than 0.1s (the car's clock). I changed the CPS so that computer 1 now sends the data to cars so that the connection is always active, still this kind of delay makes cars tracked by computer 0 to receive out-of-date information.
- Sometimes car 2 becomes unreachable to ping tests while moving. This causes the time between two clock cycles to reach even 1-2 seconds. I never found the same problem with car 1 and 3. Most of the times this happens, there is no timeout of the recvfrom() function which receives the data from computers. This suggests that the problem resides in the on-board computer more than in the connection. The reason could be originated by both hardware (the motherboard of car 2 is an older and slower model with respect to car 1 and 3) and software (out-of-date operating system or some background program that takes too much resources).
- Car 2 cannot handle high engine voltages for a long time. After about 2 minutes at constant PWM 200 the car's speed dropped exponentially. This does not happen with cars 1 and 3. Substituting the h-bridge and the battery did not help. Anyway, a motor controller that avoids keeping the PWM signal high does not cause troubles.
- The encoder of car 1 often does not give signal at the beginning when the car starts running. It starts working only after about 10 seconds.
- There is a point on the part of path8CL tracked by camera 4 (the one closest to the computers and the door) in which there are "jumps" of the measured position even if the tracking of the car does not pass from a camera to another. I am not sure if this is the problem of the camera itself or of the calibration that must be redone.
...
TROUBLESHOOTING (LAB)
...
- Router It happened to me once that the computers kept connecting and disconnecting to the local network making it impossible to establish a connection between them and with the cars. When this happened, removing the internet cable from the router (the one that is plugged to the wall) fixed the problem.
- Motherboard You can test if a car's motherboard is working by connecting it to the screen and turning the car on. If it doesn't, usually the cause is the power supply unit (PSU). Try substituting that piece.
- Brainstem ca2 console displays the message "Host-Stem Link: DOWN". Sometimes the baudrate of the brainstem resets itself for no apparent reason, so first try to set back the baudrate as explained in section "Hardware Reset Procedure" in the main page of the lab wiki. If you cannot make it work, proceed with an hardware reset (described in the same section). Note that if you do not have to configure a brand new card, there is usually no need to load the tea scripts again.
- Engine When a car is particularly slow, check that h-bridge connections to the power and the motor are solid.
- Encoder gives no signal, check that the serial cable is connected in the proper way (you can find the h-bridge datasheet
...
- and
...
- encoder
...
- datasheet
...
- in
...
- the
...
- electronic
...
- components
...
- catalog
...
- on
...
- the
...
- shelf).
...
- Steering is biased towards a direction. The steering cannot be set straight only mechanically. You must set the offset parameter in RoboCar_Lib.tea.
WHERE IS ALL THE STUFF
All the files (source code and data) are saved in the folder "Dropbox/Shared Andrea/". From here on this folder will be simply indicated as "Andrea/" so that if you move the files you will just have to update this point of the documentation. Please, do update the documentation if you decide to move it. Do not contribute to increasing the lab entropy.
CAR MODEL IDENTIFICATION
...
BASIC MODEL
...
The model is based on the decoupling of the lateral and longitudinal dynamics of the car w.r.t.
...
the
...
path.
...
In
...
other
...
words,
...
the
...
model
...
assumes
...
that
...
the
...
car
...
follows
...
exactly
...
its
...
path
...
and
...
treats
...
the
...
effects
...
of
...
the
...
lateral
...
dynamics
...
as
...
a
...
disturbance.
...
Under
...
this
...
hypothesis,
...
the
...
speed
...
of
...
the car can be described by its longitudinal component only (i.e. its unidimensional speed along the path). The car is represented by the the system of differential equations
Mathdisplay |
---|
car is equal to its unidimensional speed along the path. The car is represented by the the systems of differential equations {mathdisplay}\dot{y}(t) = v(t){mathdisplay} {mathdisplay} |
Mathdisplay |
---|
\dot{v}(t) = av(t) + b + fu(t){mathdisplay}
where _ |
where y(t)
...
is
...
the
...
unidimensional
...
position
...
along
...
the
...
path,
...
v(t)
...
is
...
the
...
car
...
speed
...
(the
...
one
...
measured
...
by
...
the
...
encoder),
...
and
...
u(t)
...
is
...
the
...
PWM
...
signal
...
given
...
as
...
input
...
to
...
the
...
DC
...
motor.
...
The
...
parameter
...
b
...
accounts
...
for
...
friction
...
while
...
f
...
is
...
the
...
gain
...
of
...
the
...
motor
...
which
...
is
...
different
...
from
...
car
...
to
...
car
...
and
...
can
...
slowly
...
change
...
in
...
time
...
depending
...
on
...
the
...
status
...
of
...
the
...
electric
...
circuits
...
and
...
the
...
battery
...
charge
...
level.
...
For
...
this
...
reason,
...
from
...
here
...
on
...
I consider the gain f time-dependent, and call it "fitness"
...
.
...
EFFECT OF PWM, STEERING AND TESTBED SLOPE ON THE SPEED DYNAMICS
...
I made car1 run on circles for 50 seconds with fixed PWM and steering input for a total of 16 runs. Every run was performed starting from the same battery voltage of 16.7V. The full data gathered and the detailed description of how the experiments were performed can be found in "Andrea/data/circle_7-27-2013/".
...
All
...
the
...
images
...
below
...
are
...
obtained
...
by
...
filtering
...
the
...
encoder
...
signal
...
with
...
a
...
moving
...
average
...
window
...
to
...
discard
...
the high frequency components of the noise.
...
The
...
oscillation
...
of
...
the
...
speed
...
is
...
largely
...
due
...
to
...
the
...
fact
...
that
...
the
...
test-bed
...
is
...
not
...
perfectly
...
flat
...
but
...
inclined
...
in
...
some
...
areas.
...
Figure pwm.png
...
shows
...
the
...
speed
...
of
...
the
...
car
...
obtained
...
by
...
keeping the steering constant and varying the PWM. The relationship between PWM and velocity is quite linear. For some reason, when the steering input is high, the speed observed with PWM 140 is slightly lower than I would expect. The reason why the steady state speed slowly decreases in time will be clearer in the section below (#EFFECT OF THE BATTERY AND THE POWER FILTER CAPACITOR ON THE SPEED DYNAMICS).
On the other hand, the steering effect seems to be a little bit more complicated (see steer.png). Velocities observed for steering 92 and 120 are always very close. This may be due to the fact that curvature radius for the two steering input are very similar (the curvature radius is not linear with respect to the steering signal). Still the speed is not linear w.r.t. the curvature radius because the discrepancy between the speed for steering 36 and 64 (which I measured to have respectively a curvature radius of roughly 200cm and 100cm) tends to be reduced by decreasing the PWM.
As a final observation, the low frequency oscillations should be noted. These oscillation are caused by the fact that the testbed is not completely flat. There are slight slopes that can be easily observed with a spirit level.
Conclusions
- The car speed depends indeed linearly on the PWM input signal as it is thus correctly modeled.
- The steering and the testbed slope introduce a complex dynamics that is not taken into account by the model and should be considered as disturbance.
...
EFFECT OF THE BATTERY AND THE POWER FILTER CAPACITOR ON THE SPEED DYNAMICS
...
I made an investigation on the effect that the battery has on the car speed. Data has been acquired by running car 1 on a circle with constant steering and engine input. The steering has always been set to -64. The starting position of the car has always been about (3000, 3600) in global coordinates (this if you want to make considerations connected to the test-bed slope). The battery voltage was measured both before turning the car on and with the car on but just before it started moving. The full data gathered and the detailed description of how the experiments were performed can be found in "Andrea/data/battery_7-30-2013/".
...
- Experiment
...
- 1:
...
- the
...
- initial
...
- voltage
...
- was
...
- kept
...
- constant
...
- while
...
- varying
...
- the
...
- pwm
...
- signal.
...
- The
...
- car
...
- is
...
- run
...
- twice
...
- in
...
- a
...
- row
...
- for
...
- 250
...
- seconds
...
- from
...
- a
...
- starting
...
- voltage
...
- of
...
- 16.8V
...
- (turned
...
- off).
...
- See
...
- figure
...
...
- .
...
- The
...
- speed
...
- always
...
- reaches
...
- a
...
- peak
...
- and
...
- then
...
- decreases to
...
- a
...
- steady
...
- state
...
- value.
...
- The
...
- second
...
- run
...
- starts
...
- more
...
- or
...
- less
...
- from
...
- the
...
- steady
...
- state
...
- value.
...
- The
...
- PWM
...
- does
...
- not
...
- seem
...
- to
...
- affect
...
- neither
...
- the
...
- time
...
- constant
...
- nor
...
- the
...
- steady
...
- state
...
- value
...
- that
...
- is
...
- more
...
- or
...
- less
...
- always
...
- 200
...
- mm/s
...
- below
...
- the
...
- peak.
...
- Experiment
...
- 2:
...
- the
...
- pwm
...
- was
...
- kept
...
- constant
...
- while
...
- varying
...
- the
...
- initial
...
- voltage.
...
- The
...
- car
...
- is
...
- run
...
- only
...
- once.
...
- See
...
- figure
...
...
- .
...
- From
...
- starting
...
- voltage
...
- 16.9V
...
- to
...
- 16.5V
...
- the
...
- speed
...
- has
...
- basically
...
- the
...
- same
...
- trend.
...
- The
...
- steady
...
- state
...
- value
...
- and
...
- the
...
- time constant become lower below 16.4V.
...
- Experiment
...
- 3:
...
- the
...
- car
...
- is
...
- run
...
- 3
...
- times
...
- in
...
- a
...
- row
...
- to
...
- check
...
- if
...
- the
...
- third
...
- time
...
- the
...
- speed
...
- dynamics
...
- changes
...
- again.
...
- See
...
...
- .
...
- The
...
- third
...
- time
...
- the
...
- car
...
- has
...
- the
...
- same
...
- behavior
...
- of
...
- the
...
- previous
...
- run.
...
- Experiment
...
- 5:
...
- the
...
- car
...
- is
...
- left
...
- turned
...
- on
...
- for
...
- 25
...
- minutes
...
- before
...
- running
...
- to
...
- check
...
- if
...
- the
...
- capacitor
...
- gets
...
- discharged
...
- even
...
- if
...
- the
...
- car
...
- is
...
- not
...
- running.
...
- See
...
...
- .
...
- The
...
- car
...
- speed
...
- is
...
- not
...
- affected.
...
- Experiment
...
- 6:
...
- the
...
- car
...
- is
...
- run
...
- for
...
- 200
...
- seconds,
...
- then
...
- it
...
- is
...
- left
...
- on
...
- for
...
- other
...
- 200
...
- seconds
...
- without
...
- running
...
- and
...
- then
...
- run
...
- for
...
- other
...
- 200
...
- seconds.
...
- See
...
...
- .
...
- The
...
- car
...
- speed
...
- has
...
- the
...
- same
...
- peak
...
- as
...
- the
...
- previous
...
- run.
...
Conclusions
- This dynamics of the car speed is likely caused by the capacitor that filter the power source. When the car is not running the circuit is in steady state condition. When it runs the equilibrium moves and the capacitor slowly discharges. The same behavior was confirmed by experiments with both cars 2 and 3. The actual speed dynamics is given by
Mathdisplay \dot{v}(t) = av(t) + b + (f + g e^{-\frac{t}{\tau}})u(t)
...
where
...
- the
...
- gain
...
- decreases
...
- exponentially
...
- to
...
- reach
...
- the
...
- steady
...
- state
...
- value
...
- f
...
- with
...
- time
...
- constant
...
, and g is a parameter defining the amplitude of the exponential decay.Mathinline - The parameter f depends on the battery charge level.
...
EFFECT OF THE LATERAL DYNAMICS
...
In reality the car cannot follow the path perfectly and thus
Mathinline |
---|
...
While
...
in
...
the
...
arc
...
B-C,
...
the
...
vehicle
...
position
...
is
...
projected
...
on
...
the
...
path
...
always
...
in
...
the
...
same
...
point
...
P
...
and
...
the
...
unidimensional
...
speed
...
results
...
0.
...
Similar
...
considerations
...
lead
...
to
...
notice
...
that
...
the
...
unidimensional speed
...
reaches
...
infinity when
...
a
...
car
...
cuts
...
a
...
corner.
...
In
...
order
...
to
...
estimate
...
the
...
effect
...
of
...
the
...
lateral
...
dynamics on the unidimensional speed,
...
let
...
us
...
consider
...
the
...
path
...
fig8
...
which
...
has
...
a
...
total
...
length
...
of
...
S_1D
...
=
...
9465.53
...
mm.
...
In
...
the
...
following
...
the
...
coordinate
...
system
...
will
...
be
...
relative
...
to
...
the
...
path
...
as
...
represented
...
in
...
Figure
...
...
.
...
In
...
other
...
words,
...
y(t)
...
represents
...
the
...
position
...
along
...
the
...
path
...
while
...
x(t)
...
the
...
lateral
...
distance
...
from
...
the
...
path.
...
If
...
the
...
car
...
actually
...
goes through
...
S_2D
...
millimeters
...
(see
...
Figure
...
...
),
...
the
...
average
...
unidimensional
...
speed
...
from
...
the
...
beginning
...
to
...
the
...
end
...
of
...
the
...
path
...
is
...
given
...
by
Mathdisplay |
---|
}\bar{\dot{y}} = \bar{v} + (\frac{S_{1D}}{S_{2D}} - 1)\bar{v} = \bar{v} + \bar{d}_{proj}{mathdisplay} |
The
...
first
...
term
...
of
...
the
...
sum
...
is
...
the
...
actual
...
average
...
speed
...
of
...
the
...
car
...
as
...
measured
...
by
...
the
...
encoder,
...
the
...
second
...
one
...
can
...
be
...
considered
...
as
...
a
...
disturbance
...
term
...
due
...
to
...
the
...
projection
...
on
...
the
...
path
...
of
...
the
...
actual
...
car
...
speed.
...
If
...
we
...
assume
...
the
...
car
...
to
...
maintain
...
a
...
lateral
...
distance
...
of
...
x=20cm
...
(which
...
is
...
very
...
reasonable
...
considering
...
the
...
current
...
performances
...
of
...
the
...
path
...
following)
...
it
...
is
...
easy
...
to
...
compute
...
a
...
value
...
for
...
S_2D
...
and
...
obtain
Mathdisplay |
---|
}\bar{d}_{proj} = 0.25 \bar{v}{mathdisplay} In other words the disturbance on the unidimensional speed would be the 25% of the actual speed of the car. It is clear that the lateral dynamics affects considerably the behavior of the vehicle. h6. FINAL MODEL WITH DISTURBANCES A linear model of the car along the longitudinal component of the path is represented in figure [^long_block.jpg]. Symbols represent * _v_: 2D speed of the car as measured by the encoder. * _y'_: unidimensional speed obtained by projecting the 2D speed of the car on the path. * _y_: unidimensional position of the car obtained by projecting its 2D position on the path. * _ym_: unidimensional position of the car measured by cameras. * _Dslope_: the disturbance due to the fact that the testbed is not perfectly flat. * _Dsteer_: the disturbance due to the steer effect on the speed. * _Dcap_: the effect of the capacitor on the speed. * _Dproj_: the disturbance caused by the not-ideal path following. * _Dmeas_: measurement error of the cameras. _Dslope_ and _Dsteer_ are treated as a known path-dependent disturbance (see section [#PATH-DEPENDENT DISTURBANCES IDENTIFICATION]). _Dcap_ is canceled by the engine compensator (see section [#ENGINE COMPENSATOR]). In order to reduce the effect of _Dproj_ two directions have been followed * the designing and implementation of a new steer controller (see [#STEER CONTROLLER]); * the reduction of the camera measurement error (i.e. _Dmeas_) which is fed to the steer controller (see [#CAMERA MEASUREMENT ERROR CORRECTION] and [#HEADING FILTERING]). Finally, the fact the the fitness parameter changes in time is handled by an online fitness estimator (see [#FITNESS ONLINE ESTIMATION]). h4. SOFTWARE h6. WHERE IS ALL THE STUFF All the files (source code and data) are saved in the folder "Dropbox/Shared Andrea/". From here on this folder will be simply indicated as "Andrea/" so that if you move the files you will just have to update this point of the documentation. Please, do update the documentation if you decide to move it. Do not contribute to increase the lab entropy. The main folder contains 4 subfolders: * "Andrea/src": contains the C code divided in 3 folders ** "Andrea/src/car_src": contains the code specific for the car, the ca2 program (See [#CA2]). ** "Andrea/src/sim_src": contains the code of the simulator (See [#SIMULATOR]). ** "Andrea/src/util": contains the code shared between the car and the simulator (See [#C UTILITIES]). * "Andrea/scripts": contains all the python scripts that can be used to compile the simulator, identify parameters and debug the experiment (See [#SCRIPTS]). * "Andrea/paths": contains the new paths definition files used by the cars which specify also the steer compensator directives and the path-dependent disturbances functions (See [#PATH SPECIFICATION]). * "Andrea/data": various data gathered during experiments on the test-bed. h6. SUMMARY OF CHANGES *Note about previous software versions*. AndreaCPS is not compatible to previous versions of ca2. Viceversa my ca2 is not compatible with previous versions of CPS. This is because the communication protocol between CPS and ca2 is now a little bit different. In general I would suggest using AndreaCPS instead of KevinCPS or LeoCPS because of the bug that involved the initial target detection. I did not check CPS versions that are previous to KevinCPS but it is likely that those versions do not have this bug since computer 1 and 2 do not send their information to computer 0 before sending them to cars. *ca2* * Grouped utility functions in a separate folder that is shared with the simulator. This makes the code more modular. * Reimplemented the target detection algorithm so that the car and CPS can work with paths defined by a large number of points. * Steer controller has been reworked, the monodimensional speed has been substituted by the one read from the encoder, the PD has been calibrated and a path-dependent compensator has been added. The compensator can be configured through the path specification file (See [#COMPENSATOR DIRECTIVES]). * Implemented a new filter for the heading measure to cancel the effect of leaps (See [#HEADING FILTERING]). * Set the limit of steer input to (-100,100) since the curvature radius saturates above it. * Implemented the online estimation of the engine gain parameter (fitness) (See [#FITNESS ONLINE ESTIMATION]). * Implemented speed disturbances correction which uses a path-dependent disturbance function defined in the path specification file. *CPS* * Fixed a bug that affected the initial target dectection for cars that were not tracked by computer 0 and buffer overflow bug. * Implemented linear error correction for the computation of camera position. This visibly improves the path following of cars. See section [#CAMERA MEASUREMENT ERROR CORRECTION]. * The computer that sends the data to cars is now computer 1 (not the slower computer 0 anymore). This is easily configurable through the constant SENDER_COMP_NUM in CPS.h. * The sender computer now knows which camera is currently tracking which car and this information is send to cars. *Communication protocol* * Now the car sends its personal data (encoder speed, estimated fitness, desired pwm, etc.) to CPS that forwards this data to all the cars. If the encoder of a car does not work you can set to 0 the constant USE_ENCODER in |
In other words the disturbance on the unidimensional speed would be the 25% of the actual speed of the car. It is clear that the lateral dynamics affects considerably the longitudinal components of the car's state.
...
FINAL MODEL WITH DISTURBANCES
...
A linear model of the car along the longitudinal component of the path is represented in figure long_block.jpg. Symbols represent
- v: 2D speed of the car as measured by the encoder.
- y': unidimensional speed obtained by projecting the 2D speed of the car on the path.
- y: unidimensional position of the car obtained by projecting its 2D position on the path.
- ym: unidimensional position of the car measured by cameras.
- Dslope: the disturbance due to the fact that the testbed is not perfectly flat.
- Dsteer: the disturbance due to the steering effect on the speed.
- Dcap: the effect of the capacitor on the speed.
- Dproj: the disturbance caused by the not-ideal path following.
- Dmeas: measurement error of the cameras.
Dslope and Dsteer are treated as a known path-dependent disturbance (see section #PATH-DEPENDENT DISTURBANCES IDENTIFICATION). Dcap is canceled by the engine compensator (see section #ENGINE COMPENSATOR). In order to reduce the effect of Dproj two directions have been followed
- the designing and implementation of a new steering controller (see #STEERING CONTROLLER);
- the reduction of the camera measurement error (i.e. Dmeas) which is fed to the steering controller (see #CAMERA MEASUREMENT ERROR CORRECTION and #HEADING FILTERING).
Finally, the fact the the fitness parameter changes in time is handled by an online fitness estimator (see #FITNESS ONLINE ESTIMATION).
SOFTWARE
The main folder contains 4 subfolders:
- "Andrea/src": contains the C code divided in 3 folders
- "Andrea/src/car_src": contains the code specific for the car, the ca2 program (See #CA2).
- "Andrea/src/sim_src": contains the code of the simulator (See #SIMULATOR).
- "Andrea/src/util": contains the code shared between the car and the simulator (See #C UTILITIES).
- "Andrea/scripts": contains all the python scripts that can be used to compile the simulator, identify parameters and debug the experiment (See #SCRIPTS).
- "Andrea/paths": contains the new paths definition files used by the cars which specify also the steering compensator directives and the path-dependent disturbances functions (See #PATH SPECIFICATION).
- "Andrea/data": various data gathered during experiments on the test-bed.
...
SUMMARY OF CHANGES
...
Note about previous software versions. AndreaCPS is not compatible to previous versions of ca2. Viceversa my ca2 is not compatible with previous versions of CPS. This is because the communication protocol between CPS and ca2 is now a little bit different. In general I would suggest using AndreaCPS instead of KevinCPS or LeoCPS because of the bug that involved the initial target detection. I did not check CPS versions that are previous to KevinCPS but it is likely that those versions do not have this bug since computer 1 and 2 do not send their information to computer 0 before sending them to cars.
ca2
- Grouped utility functions in a separate folder that is shared with the simulator. This makes the code more modular.
- Reimplemented the target detection algorithm so that the car and CPS can work with paths defined by a large number of points.
- Steering controller has been reworked, the monodimensional speed has been substituted by the one read from the encoder, the PD has been calibrated and a path-dependent compensator has been added. The compensator can be configured through the path specification file (See #COMPENSATOR DIRECTIVES).
- Implemented a new filter for the heading measure to cancel the effect of leaps (See #HEADING FILTERING).
- Set the limit of steering input to (-100,100) since the curvature radius saturates above it.
- Implemented the online estimation of the engine gain parameter (fitness) (See #FITNESS ONLINE ESTIMATION).
- Implemented speed disturbances correction which uses a path-dependent disturbance function defined in the path specification file.
CPS
- Fixed a bug that affected the initial target dectection for cars that were not tracked by computer 0 and buffer overflow bug.
- Implemented linear error correction for the computation of camera position. This visibly improves the path following of cars. See section #CAMERA MEASUREMENT ERROR CORRECTION.
- The computer that sends the data to cars is now computer 1 (not the slower computer 0 anymore). This is easily configurable through the constant SENDER_COMP_NUM in CPS.h.
- The sender computer now knows which camera is currently tracking which car and this information is send to cars.
Communication protocol
- Now the car sends its personal data (encoder speed, estimated fitness, desired pwm, etc.) to CPS that forwards this data to all the cars. If the encoder of a car does not work you can set to 0 the constant USE_ENCODER in "car_src/const_car.h" and the CPS will estimate the car speed from differentials which is currently not that accurate (See #CPS MEASURED 2D SPEED).
- Each car listen on a different port (base_port_number + car_number). This was required to make the simulator handle multiple cars and I extended it to the CPS for coherence.
Paths
- Modified the path specification file format in order to include the steering compensator directives and the definition of the speed disturbance function.
- Slightly narrowed the path fig0 since car3 used to go too close to the computers where the camera distortion is very high and the tracking was often lost.
...
CONFIGURATION
...
There are 5 files which contain basically all the constants in the code. These should be the ones to check whenever you want to configure something.
- "car_src/const_car.h"
...
- : contains the compilation options that activate/deactivate the features of the code in the car and few other constants specific for the car code such as the car number, the initial pwm and when it has to stop.
- "car_src/supervisor.c": contains few types that only the supervisor uses and the constants that define the boundaries for the disturbances.
- "sim_src/const_sim.h": this contains very few simulator-specific constants which include the real fitness parameters of the cars, the paths followed by each car and whether to apply the measurement noise during the simulation.
- "sim_src/model.c": contains the amplitudes of the noise signals that are applied to the physical quantities by the simulator.
- "util/constants.h": this basically contains all the others. They range from network configuration to control parameters to physical constants.
The specific effect of each constants is documented in the code. Beside the types defined in "car_src/supervisor.c", all the types definition (and their documentation) can be found in "util/types.h".
...
SIMULATOR
...
The simulator emulates the laboratory environment by computing the dynamics of the vehicles, taking into account disturbances and measurement noise, and communicating with ca2 exactly like CPS does. It is transparent to ca2, meaning that the car code basically does not need to know if it is interfacing with the lab CPS or with the simulator. This allows to test the changes in the code in a controlled environment and speed up significantly the identification of problems, testing the system model and debugging.
The program simulates very accurately the effects of the capacitor, the ones of the lateral dynamics and the position measurement error which include the "jumps" of the position when the tracking of the car pass from a camera to another (see #CPS). It also simulates the effects of the steering and the testbed slope but it does not take into account the part of the disturbance which is unpredictable. Basically it assumes the path-dependent disturbances to be the one specified in the path file (see #PATH-DEPENDENT SPEED DISTURBANCE SPECIFICATION) which are the same that the engine compensator cancel. Moreover the encoder speed measurement noise is not implemented.
Compatibility
It is implemented in C and it surely works on Ubuntu Desktop 12.04 LTS. It should not have problems also with other distros of Linux but I did not have time to test it. Since it uses the POSIX library for threading and network communication, it cannot work on Windows unless one uses some (untested) workarounds such as using a POSIX porting for windows (see article on Wikipedia http://en.wikipedia.org/wiki/POSIX#POSIX_for_Windows
...
).
...
However
...
it
...
should
...
work
...
on
...
Mac
...
since
...
it
...
is
...
Unix-based.
...
Compilation
To compile both ca2 and the simulator there is the shell script "scripts/compile.sh".
...
First
...
of
...
all,
...
check
...
that
...
the
...
constant
...
CODE_IN_CAR
...
in
...
"src/car_src/const_car.h"
...
is
...
set
...
to
...
0.
...
This
...
allows
...
to
...
compile
...
the
...
code
...
without
...
the
...
requirement
...
of
...
the
...
Brainstem
...
library
...
and
...
setups
...
the
...
sockets
...
to
...
connect
...
to
...
localhost
...
instead
...
of
...
the
...
IP
...
addresses
...
of
...
the
...
CPS
...
and
...
the
...
cars.
...
From
...
the
...
main
...
folder,
...
just
...
type
...
in
...
the
...
console
No Format |
---|
{code} cd scripts ./compile.sh {code} |
The
...
two
...
executable
...
ca2
...
and
...
simulator
...
should
...
appear
...
in
...
your
...
main
...
folder.
...
Other
...
than
...
the
...
standard
...
POSIX
...
library,
...
the
...
simulator
...
uses
...
GSL
...
(Gnu
...
Scientific
...
Library)
...
so
...
if
...
you
...
have
...
problems
...
compiling,
...
make
...
sure
...
it
...
is
...
installed.
Run
From the main folder, to run a simulated experiment first run the simulator.
No Format |
---|
./simulator |
and then run the car code with the normal syntax on a different console.
No Format |
---|
*Run* From the main folder, to run a simulated experiment first run the simulator. {code}./simulator{code} and then run the car code with the normal syntax on a different console. {code}./ca2 paths/figX_precise.txt{code} |
Where
...
X
...
can
...
be
...
"8AL",
...
"8CL"
...
or
...
"0".
...
Note
...
that
...
I
...
added
...
the
...
suffix
...
"_precise"
...
to
...
the
...
path
...
specification
...
files
...
to
...
differentiate
...
them
...
from
...
the
...
previous
...
ones
...
which
...
define
...
the
...
path
...
with
...
a
...
limited
...
number
...
of
...
points.
...
The
...
path
...
files
...
I
...
have
...
used
...
are
...
contained
...
in
...
the
...
folder
...
"Andrea/paths".
...
Specifying
...
great
...
number
...
of
...
points
...
allows
...
the
...
car
...
to
...
follow
...
its
...
path
...
more
...
effectively.
...
The
...
simulator
...
automatically
...
stops
...
running
...
when
...
you
...
press
...
the
...
"Space"
...
key
...
in
...
the
...
ca2
...
console
...
to
...
stop
...
the
...
car.
...
The
...
simulator
...
can
...
handle
...
experiments
...
with
...
more
...
than
...
a
...
single
...
car.
...
You
...
just
...
have
...
to
...
compile
...
different
...
ca2
...
executable
...
(make
...
sure
...
that
...
the
...
constant
...
CAR_NUM
...
in
...
"src/car_src/const_car.h"
...
is
...
different
...
for
...
each
...
compiled
...
ca2
...
and
...
that
...
NUM_CARS_EXP
...
in
...
"src/util/constants.h"
...
is
...
set
...
appropriately).
...
Then
...
run
...
the
...
simulator
...
normally
...
and
...
run
...
the
...
car
...
executables,
...
each
...
one
...
on
...
a
...
different
...
console
...
and
...
with
...
its
...
own
...
path.
...
Debug
I wrote a python script called "plot_trajectory.py"
...
that
...
can
...
be
...
used
...
to
...
speed
...
up
...
considerably
...
the
...
debugging
...
of
...
the
...
experiment
...
both
...
simulated
...
and
...
on
...
test-bed.
...
See
...
...
to
...
understand
...
its
...
usage.
...
CAR
...
SOFTWARE
...
Since
...
the
...
code
...
of
...
the
...
car
...
is
...
split
...
into
...
modules,
...
there
...
are
...
a
...
couple
...
of
...
differences
...
concerning
...
the
...
uploading
...
and
...
compilation
...
of
...
the
...
source
...
code.
...
Create
...
your
...
project
...
The
...
folder
...
on
...
cars'
...
computers
...
that
...
I
...
have
...
used
...
is
...
located
...
at
...
"/Desktop/brainstem/aProject/Andrea".
...
The
...
structure
...
of
...
the
...
code
...
is
...
changed
...
so
...
to
...
make
...
the
...
compilation
...
working
...
I
...
had
...
to
...
motify
...
the
...
makefiles.
...
As
...
a
...
consequence,
...
if
...
you
...
want
...
to
...
create
...
your
...
own
...
folder
...
to
...
expand
...
my
...
branch
...
of
...
the
...
project
...
you
...
should
...
start
...
by
...
copying
...
mine
...
(See
...
...
...
).
...
Upload
...
a
...
new
...
code
...
version
...
The
...
car
...
sorce
...
to
...
compile
...
and
...
run
...
needs
...
both
...
the
...
folders
...
"car_src"
...
and
...
"util".
...
If
...
you
...
write
...
a
...
new
...
version
...
of
...
the
...
code
...
and
...
you
...
want
...
to
...
upload
...
it
...
on
...
the
...
cars
...
with
...
sftp,
...
you
...
must
...
upload
...
both
...
those
...
folders.
...
Of
...
course
...
you
...
can
...
avoid
...
updating
...
both
...
if
...
the
...
changes
...
affect
...
only
...
the
...
code
...
in
...
one
...
of
...
the
...
two
...
folders.
...
Compilation
...
and
...
execution
...
The
...
compilation
...
and
...
the
...
execution
...
of
...
the
...
new
...
code
...
does
...
not
...
differ
...
from
...
the
...
previous
...
versions.
...
Make
...
sure
...
that
...
the
...
constant
...
CODE_IN_CAR
...
in
...
file
...
"car_src/const_car.h"
...
is
...
set
...
to
...
1
...
and
...
then
...
type
...
on
...
the console
No Format |
---|
console {code} cd /Desktop/brainstem/aProject/Andrea make clean make new {code} |
in
...
case
...
you
...
do
...
not
...
want
...
to
...
recompile
...
the
...
whole
...
project
...
but
...
only
...
the
...
changed
...
files
...
you
...
can
...
simply type
No Format |
---|
make |
instead of make clean followed by make new. To run the code type
No Format |
---|
type {code}make{code} instead of _make clean_ followed by _make new_. To run the code type {code} cd /Desktop/brainstem/aDebug/aUnix/i686 ./ca2 figX_precise.txt {code} |
The
...
path
...
specification
...
files
...
I
...
have
...
used
...
with
...
have
...
the
...
suffix
...
"_precise"
...
in
...
their
...
name
...
in
...
the
...
same
...
way
...
of
...
the
...
simulator.
...
As
...
usual,
...
you
...
can
...
find
...
these
...
files
...
in
...
the
...
folder
...
"/Desktop/brainstem/aDebug/aUnix/i686".
...
...
SCRIPTS
...
The scripts are written in Python. At the beginning of the script, after the imports there is always a "configuration" section (delimited by comments) which contains all the (documented) constant variables that must be set appropriately to make the script do exactly what you want.
Most of the scripts rely on the debug file that ca2 leaves after execution. As you can imagine, the parsing of the debug file is strongly dependent on its format. This means that if you change the name of the attributes written in the file for example, the script might not work as expected. The parsing does not depend on the order of the attributes so it is safe to change it. All the scripts rely on the parser class DebugData
in "scripts/util/parser.py".
...
This
...
design
...
makes
...
it
...
easy
...
to
...
make
...
adjustments
...
in
...
case
...
the
...
file
...
format
...
changes.
...
However,
...
if
...
you
...
modify
...
the
...
internal
...
data
...
structure
...
of
...
DebugData
...
(e.g.
...
change
...
the
...
name
...
of
...
one
...
of
...
its
...
member
...
variables)
...
you
...
will
...
have
...
to
...
modify
...
the
...
scripts
...
that
...
access
...
to
...
that
...
data.
...
Check
...
the
...
class
...
documentation
...
in
...
the
...
code
...
for
...
more
...
information.
...
Note
...
that
...
DebugData
...
can
...
be
...
used
...
to
...
parse
...
only
...
the
...
debug
...
file
...
produced
...
by
...
ca2
...
,
...
not
...
the
...
one
...
written
...
by
...
the
...
simulator.
...
External
...
dependencies
...
The
...
scripts
...
often
...
uses
...
matplotlib
...
to
...
plot
...
graphics
...
and
...
numpy
...
and
...
scipy
...
for
...
advanced
...
numerical
...
routines.
...
If
...
you
...
have
...
problems
...
running
...
the
...
scripts
...
, make
...
sure
...
these
...
Python
...
libraries
...
are
...
installed
...
on
...
your
...
system.
...
plot_trajectory.py
...
This
...
script
...
allows
...
to
...
analyze
...
offline
...
the
...
data
...
of
...
an
...
experiment.
...
The
...
program
...
ca2
...
saves
...
a
...
file
...
in
...
the
...
same
...
folder
...
of
...
the
...
executable
...
called
...
"data_carX.txt"
...
which
...
contains
...
lots
...
of
...
data
...
describing
...
position,
...
speed,
...
heading,
...
etc.
...
of
...
the
...
car
...
in
...
each
...
car
...
cycle.
...
Similarly,
...
the
...
simulator
...
produces
...
a
...
file
...
called
...
"debug_sim.txt".
...
The
...
main
...
difference
...
between
...
the
...
two
...
files
...
is
...
that
...
the
...
first
...
one
...
reports
...
values
...
of
...
the
...
physical
...
quantities
...
that
...
are
...
affected
...
by
...
measurement
...
noise
...
while
...
the
...
second
...
one
...
report
...
also
...
the
...
real
...
state
...
of
...
the
...
system.
...
Moreover,
...
the
...
car
...
can
...
records
...
the
...
status
...
of
...
the
...
internal
...
variables
...
which
...
are
...
unknown
...
to
...
the
...
simulator.
...
The
...
script
...
normally
...
parses only
...
the
...
"data_carX.txt"
...
and
...
thus
...
it
...
can
...
be
...
used
...
to
...
debug
...
both
...
simulated
...
and
...
experimental
...
data.
...
You
...
can
...
have
...
a
...
graphical
...
representation
...
of
...
the
...
information
...
contained
...
in
...
"debug_sim.txt"
...
using
...
the
...
command
...
line
...
option
...
"-b"
...
(see
...
below
...
for
...
full
...
description).
...
In
...
this
...
case
...
the
...
script
...
integrates
...
the
...
information
...
in
...
the
...
two
...
files.
...
Configuration
...
and
...
usage
...
- Run
...
- a
...
- simulation
...
- (or
...
- an
...
- experiment)
...
- to
...
- obtain
...
- the
...
- debug
...
- files.
...
- Do
...
- not
...
- stop
...
- the
...
- program
...
- with
...
- CTRL+C
...
- or
...
- the
...
- debug
...
- file
...
- will
...
- not
...
- be
...
- terminated
...
- correctly.
...
- Open
...
- plot_trajectory.py
...
- and
...
- set
...
- the
...
- car
...
- number
...
- and
...
- the
...
- paths
...
- to
...
- debug
...
- files
...
- you
...
- want
...
- to
...
- analyze.
...
- Run
...
- the
...
- script
...
- by
...
- typing
...
python
...
plot_trajectory.py
...
- on
...
- the
...
- console.
...
- Focusing
...
- the
...
- console,
...
- use
...
- keys
...
- (lowercase)
...
- 'n'
...
- to
...
- go
...
- to
...
- the
...
- next
...
- step,
...
- 'p'
...
- to
...
- go
...
- to
...
- the
...
- previous
...
- one,
...
- 'q'
...
- to
...
- quit.
...
The
...
script
...
displays
...
four
...
plots.
...
On
...
the
...
top
...
left
...
corner
...
there
...
is
...
the
...
path
...
and
...
the
...
trajectory
...
of
...
the
...
car
...
with
...
some
...
information.
...
The
...
little
...
red
...
cross
...
represents
...
the
...
target
...
point
...
of
...
the
...
path.
...
The
...
other
...
3
...
graphics
...
show
...
the
...
evolution
...
of
...
some
...
attributes in
...
time.
...
You
...
can
...
configure
...
which
...
attribute
...
to
...
plot
...
with
...
the
...
following
...
precedure.
...
- Open
...
- "plot_trajectory.py".
...
- Use
...
- your
...
- editor's
...
- search
...
- feature
...
- (usually
...
- CTRL+F)
...
- to
...
- find
...
- the
...
- string
...
- "TODO".
...
- You
...
- should
...
- find
...
- a
...
- code
...
- section
...
- titled
...
- "TODO
...
- configure
...
- which
...
- info
...
- to
...
- plot".
...
- Assign
...
- to
...
- variables
...
graphX_vals
...
- the
...
- arrays
...
- containing
...
- the
...
- info
...
- you
...
- want
...
- to
...
- be
...
- plotted.
...
- The
...
- array
...
- will
...
- likely
...
- be
...
- a
...
- member
...
- variable
...
- of
...
- the
...
- class
DebugData
defined in "scripts/util/parser.py"
...
- or
...
- one
...
- of
...
- the
...
*
...
_sim_vals
...
- arrays
...
- defined
...
- inside
...
- plot_trajectory.py
...
- that
...
- represent
...
- the
...
- data
...
- extracted
...
- from
...
- the
...
- simulator
...
- debug
...
- file.
...
- Check
...
- the
...
- code
...
- documentation
...
- in
...
DebugData
...
- and
...
- plot_trajectory.py
...
- (section
...
- "SIMULATOR
...
- DEBUG
...
- VARIABLES")
...
- for
...
- details.
...
- Of
...
- course
...
- you
...
- can
...
...
- any
...
- array-like
...
- structure
...
- as
...
- long
...
- as
...
- it
...
- as
...
- the
...
- same
...
- length
...
- of
...
lab_data.time
...
- .
...
The
...
little
...
red
...
crosses
...
show
...
the
...
value
...
of
...
the
...
specified
...
attribute
...
at
...
the
...
current
...
step.
...
Command
...
line
...
options
...
- -b:
...
- parse
...
- and
...
- integrate
...
- both
...
- "data_carX.txt"
...
- and
...
- "debug_sim.txt".
...
- When
...
- this
...
- option
...
- is
...
- used,
...
- the
...
- trajectory
...
- shown
...
- and
...
- the
...
- information
...
- presented
...
- in
...
- the
...
- top
...
- left
...
- corner
...
- graphic
...
- are
...
- the
...
- real
...
- one,
...
- not
...
- the
...
- ones
...
- affected
...
- by
...
- measurement
...
- noise
...
- that
...
- cars
...
- perceive.
...
- This
...
- is
...
- useful
...
- to
...
- check
...
- that
...
- everything
...
- is
...
- going
...
- fine
...
- with
...
- the
...
- simulator.
...
- Note
...
- that
...
- the
...
- simulator
...
- and
...
- ca2
...
- are
...
- asynchronous
...
- (the
...
- clock
...
- rate
...
- of
...
- the
...
- simulator
...
- is
...
- much
...
- higher
...
- than
...
- the
...
- car's
...
- one),
...
- thus,
...
- in
...
- order
...
- to
...
- integrate
...
- the
...
- two
...
- files,
...
- the
...
- script
...
- must
...
- replicate/cut
...
- some
...
- data
...
- according
...
- to
...
- the
...
- desired
...
- clock
...
- cycle
...
- specified
...
- through
...
- the
...
- option
...
- "-c".
...
- -c
...
- (default
...
- 0.1):
...
- the
...
- time
...
- between
...
- two
...
- represented
...
- steps.
...
- For
...
- example,
...
- if
...
- you
...
- call
...
- the
...
- script
...
- by
...
- typing
...
python
...
plot_trajectory.py
...
-c1.0
...
- ,
...
- pressing
...
- 'n'
...
- will
...
- show
...
- the
...
- status
...
- of
...
- the
...
- car
...
- 1
...
- second
...
- later.
...
- -t
...
- (default
...
- 0.0):
...
- the
...
- initial
...
- time
...
- shown
...
- by
...
- the
...
- script.
...
- For
...
- example,
...
- if
...
- you
...
- want
...
- to
...
- investigate
...
- an
...
- event
...
- that
...
- happened
...
- 40
...
- seconds
...
- after
...
- the
...
- beginning
...
- of
...
- the
...
- experiment,
...
- you
...
- can
...
- invoke
...
- the
...
- script
...
- as
...
python
...
plot_trajectory.py
...
-t40.0
...
- instead
...
- of
...
- starting
...
- from
...
- the
...
- beginning
...
- and
...
- press
...
- 'n'
...
- until
...
- you
...
- reach
...
- that
...
- point.
...
- -s
...
- (default
...
- 20):
...
- the
...
- number
...
- of
...
- segments
...
- in
...
- the
...
- trail
...
- of
...
- the
...
- represented
...
- trajectory.
...
identify_model.py
...
This
...
script
...
is
...
used
...
to
...
identify
...
the
...
car's
...
model
...
parameters.
...
For
...
details
...
see
...
section
...
...
...
...
.
...
path_disturb.py
...
This
...
script
...
is
...
used
...
to
...
determine
...
the
...
path-dependent
...
disturbances.
...
For
...
details
...
see
...
section
...
...
...
...
.
...
util/predictor.py
...
and
...
test_supervisor.py
...
The
...
code
...
in
...
"scripts/util/predictor.py"
...
is
...
a
...
reimplementation
...
in
...
Python
...
of
...
the
...
supervisor
...
algorithms
...
in
...
"src/car_src/supervisor.c".
...
It
...
is
...
very
...
helpful
...
for
...
testing
...
the
...
supervisor
...
offline
...
instead
...
of
...
running
...
the
...
experiment
...
on
...
the
...
test-bed
...
over
...
and
...
over.
...
This
...
allows
...
to
...
analyze
...
the
...
very
...
same
...
experiment
...
instead
...
of
...
a
...
series
...
of
...
similar
...
executions
...
and
...
it speeds
...
up
...
considerably
...
the
...
debugging
...
of
...
the
...
supervisor.
...
Of
...
course,
...
this
...
script
...
is
...
useful
...
only
...
as
...
long
...
as
...
it
...
is
...
kept
...
updated
...
with
...
the
...
changes
...
of
...
"src/car_src/supervisor.c".
...
The
...
Config
...
section
...
of
...
the
...
script
...
contains
...
various
...
constants
...
that
...
in
...
the
...
car
...
code
...
are
...
defined
...
in
...
both
...
"src/car_src/supervisor.c"
...
and
...
"src/util/constants.c".
...
Be
...
sure
...
that
...
these
...
values
...
are
...
updated
...
too
...
if
...
you
...
want
...
to
...
use
...
this
...
script.
...
In
...
particular,
...
the
...
script
...
implements
...
two
...
useful
...
routines.
...
test_long_term_prediction()
...
- :
...
- test
...
- that
...
- in
...
- each
...
- point
...
- the
...
- robust
...
- prediction
...
- (i.e.
...
- that
...
- include
...
- the
...
- effect
...
- of
...
- the
...
- disturbances)
...
- of
...
- the
...
- time
...
- at
...
- which
...
- the
...
- car
...
- enters
...
- and
...
- exits
...
- the
...
- intersection
...
- is
...
- correct.
...
- This
...
- is
...
- useful
...
- to
...
- check
...
- that
...
- the
...
- upper
...
- and
...
- lower
...
- bounds
...
- given
...
- to
...
- disturbances
...
- are
...
- effective
...
- for
...
- the
...
- prediction.
...
test_exact_supervisor()
...
- :
...
- simulate
...
- the
...
- execution
...
- of
...
- the
...
- exact
...
- supervisor
...
- and
...
...
- in
...
- detail
...
- the
...
- status
...
- of
...
- the
...
- internal
...
- variables.
...
- This
...
- is
...
- useful
...
- to
...
- debug
...
- the
...
- supervisor.
...
The
...
script
...
"test_supervisor.py"
...
is
...
a
...
simple
...
piece
...
of
...
code
...
that
...
calls
...
these
...
two
...
functions.
...
Open
...
it,
...
set
...
the
...
car
...
number
...
and
...
the
...
debug
...
file
...
path
...
in
...
the
...
configuration
...
section
...
and
...
run
...
it
...
on
...
the
...
console
...
to
...
see
...
the
...
result
...
of
...
the
...
test.
...
predictor_disturb.py
...
This
...
is
...
a
...
script
...
I
...
originally
...
wrote
...
to
...
estimate
...
upper
...
and
...
lower
...
bounds
...
for
...
the
...
disturbances
...
but
...
it
...
turned
...
out
...
to
...
overestimate
...
them
...
a
...
lot.
...
You
...
might
...
still
...
use
...
it
...
to
...
have
...
a
...
rough
...
quantitative
...
idea
...
of
...
their
...
magnitude though.
...
view_paths.py
...
This
...
simply
...
plots
...
the
...
paths
...
specified in
...
the folder "Andrea/paths".
...
TROUBLESHOOTING
...
(SOFTWARE)
...
Simulator
- The car does not follow the path correctly. Make sure that the paths specified in "src/sim_src/const_sim.h"
...
- by
...
- the
...
- constant
...
- FILE_NAMES
...
- are
...
- consistent
...
- with
...
- the
...
- experiment
...
- configuration.
...
ca2
- The car does not follow the path correctly. Make sure that the constant CAR_NUM in "car_src/const_car.h"
...
- is
...
- set
...
- coherently
...
- with
...
- the
...
- symbol
...
- at
...
- the
...
- top
...
- of
...
- the
...
- car
...
MODEL
...
PARAMETERS
...
IDENTIFICATION
...
You
...
must
...
identify
...
the
...
parameters
...
of
...
the
...
complete
...
model
...
Mathinline |
---|
Mathinline |
---|
To speed up the process there is a script called "identify_model.py"
...
in
...
"Andrea/scripts"
...
that
...
estimate
...
the
...
parameters
...
to
...
fit
...
the
...
model
...
to
...
the
...
experimental
...
data
...
by
...
minimizing
...
the
...
mean
...
average
...
error.
...
The
...
script
...
needs
...
as
...
input
...
two
...
debug
...
files
...
from
...
ca2
...
taken
...
by
...
running
...
the
...
car
...
in
...
circle
...
for
...
about
...
3
...
-4 minutes
...
at
...
constant
...
PWM.
...
The
...
two
...
experiments
...
must
...
be
...
run
...
at
...
different
...
PWM
...
with
...
the
...
same
...
steering input.
...
In
...
order
...
to
...
ensure
...
that
...
the
...
fitness
...
parameter
...
is
...
the
...
same
...
in
...
both
...
the
...
experiments,
...
it
...
is
...
important
...
for
...
the
...
battery
...
charge
...
level
...
to
...
be
...
the
...
same.
...
The
...
detailed
...
procedure
...
is
...
below.
...
- Unplug
...
- the
...
- charger,
...
- use
...
- the
...
- multimeter
...
- to
...
- record
...
- the
...
- voltage
...
- level
...
- of
...
- the
...
- battery
...
- and
...
- plug
...
- it
...
- in
...
- again.
...
- Run the car in circle as described in "Run the car in circle: configuration". For the PWM and steering input I would suggest respectively 160 and -50 (negative values make the car steering left).
- Connect through sftp and save the "data_carX.txt"
...
- file.
...
- Plug
...
- the
...
- charger
...
- again
...
- and
...
- charge
...
- the
...
- battery
...
- to
...
- the
...
- same
...
- voltage
...
- measured
...
- before
...
- the
...
- first
...
- experiment.
...
- Once
...
- the
...
- voltage
...
- is
...
- the
...
- same
...
- as
...
- before,
...
- run
...
- the
...
- car
...
- in
...
- circle
...
- for
...
- the
...
- first
...
- time.
...
- Keep
...
- the
...
- steering input
...
- at
...
- the
...
- same
...
- level
...
- (-50)
...
- but
...
- change
...
- the
...
- PWM
...
- (I
...
- suggest
...
- 140).
...
- It
...
- is
...
- very
...
- important
...
- to
...
- run
...
- this
...
- second
...
- experiment
...
- as
...
- soon
...
- as
...
- possible.
...
- Try
...
- to
...
- perform
...
- it
...
- the
...
- same
...
- day
...
- of
...
- the
...
- first
...
- one
...
- otherwise
...
- the
...
- fitness
...
- parameter
...
- might
...
- have
...
- changed.
...
- Collect
...
- the
...
- new
...
- "data_carX.txt".
...
- Pay
...
- attention
...
- to
...
- not
...
- overwrite
...
- the
...
- previous
...
- file
...
- or
...
- you
...
- will
...
- have
...
- to
...
- perform
...
- the
...
- first
...
- experiment
...
- again.
...
- Open
...
- the
...
- script
...
- "identify_model.py"
...
- and
...
- set
...
- the
...
- configuration
...
- constants.
...
- You
...
- simply
...
- have
...
- to
...
- specify
...
- whose
...
- car
...
- you
...
- want
...
- to
...
- identify
...
- the
...
- parameters
...
- and
...
- the
...
- paths
...
- to
...
- the
...
- two
...
- debug
...
- files
...
- you
...
- just
...
- obtained.
...
- Run
...
- the
...
- script
...
- "identify_model.py".
...
- It
...
- will
...
...
- on
...
- the
...
- console
...
- the
...
- five
...
- parameters
...
- you
...
- need
...
- and
...
- show
...
- you
...
- a
...
- graphic
...
- with
...
- the
...
- experimental
...
- data
...
- and
...
- the
...
- model
...
- fitting
...
- that
...
- you
...
- can
...
- use
...
- to
...
- check
...
- that
...
- everything
...
- went
...
- fine
...
- with
...
- the
...
- identification.
...
- You
...
- now
...
- have
...
- to
...
- make
...
- the
...
- rest
...
- of
...
- the
...
- code
...
- (the
...
- scripts,
...
- ca2
...
- and
...
- the
...
- simulator)
...
- aware
...
- of
...
- the
...
- new
...
- parameters.
...
- Open
...
- "Andrea/scripts/util/model.py"
...
- and
...
- set
...
- the
...
- variables
...
- a
...
- ,
...
- b
...
- ,
...
- g
...
- ,
...
- T
...
- as
...
- documented
...
- in
...
- the
...
- code.
...
- Similarly,
...
- open
...
- "Andrea/src/util/constants.h"
...
- and
...
- set
...
- the
...
- constants
...
- ALPHA,
...
- _BETA
...
- ,
...
- GAMMA
...
- ,
...
- TAU
...
- (i.e.
...
- a,
...
- b,
...
- g,
...
- tau).
...
Run
...
the
...
car
...
in
...
circle:
...
configuration
...
- Decide
...
- which
...
- constant
...
- PWM
...
- and
...
- steering signal
...
- to
...
- use
...
- for
...
- the
...
- experiment.
...
- Turn
...
- on
...
- the
...
- car
...
- and
...
- connect
...
- to
...
- it.
...
- You
...
- do
...
- not
...
- need
...
- to
...
- turn
...
- on
...
- the
...
- CPS.
...
- Move
...
- to
...
- the
...
- project
...
- folder
...
- (e.g.
...
cd
...
/Desktop/brainstem/aProject/Andrea
...
- ).
...
- Open
...
- the
...
- const_car.h
...
- file
...
- with
...
- an
...
- editor
...
- (e.g.
...
nano
...
car_src/const_car.h
...
- ).
...
- Set
...
- the
...
- constants
...
START_PWM
...
- and
START_STEER
...
- are
...
- set
...
- to
...
- the
...
- values
...
- you
...
- have
...
- decided
...
- at
...
- step
...
- 1.
...
- Be
...
- sure
...
- that
...
- the
...
- following
...
- configuration
...
- constants
...
- are
...
- set
...
- as
...
- below
No Format #define DURATION_EXP 2400 # This is flexible but consider that the transient given by the capacitor ends after about 200 seconds #define CODE_IN_CAR
...
1 #define GET_POSITIONS 0 #define USE_ENCODER 1 #define SUPERVISOR 0 # This is not mandatory but it will save computational resources and avoid printing error messages on the console #define PATHPLAN_ON 0 #define DEBUG 1
...
- Compile the code with
make
. - Move to the executable folder (e.g.
...
cd
...
../../aDebug/aUnix/i686
...
- ).
...
- Put
...
- the
...
- car
...
- on
...
- the
...
- testbed
...
- and
...
- run
...
- it.
...
- You
...
- can
...
- specify
...
- any
...
- path
...
- as
...
- argument
...
- of
...
- ca2
...
- ,
...
- it
...
- will
...
- be
...
- ignored
...
- (e.g.
...
./ca2
...
fig8CL_precise.txt
...
- ).
...
- During
...
- the
...
- whole
...
- experiment
...
- check
...
- the
...
- car.
...
- Sometimes
...
- the
...
- center
...
- of
...
- the
...
- circle
...
- that
...
- the
...
- car
...
- follows
...
- moves,
...
- and
...
- the
...
- car
...
- can
...
- hit
...
- the whole.
All the data used for the parameters identification are stored in "Andrea/data/identification/".
...
FITNESS ONLINE ESTIMATION
...
The fitness parameter is very unstable. The performances of the car can change from one day to another even if the battery charge level is the same. For this reason it is estimated online by using the adaptive control technique called "MIT rule" which basically adopts gradient descent to minimize a cost function. The cost function is defined as:
Mathdisplay |
---|
whole. h6. FITNESS ONLINE ESTIMATION The fitness parameter is very unstable. The performances of the car can change from one day to another even if the battery charge level is the same. For this reason it is estimated online by using the adaptive control technique called "MIT rule" which basically adopts gradient descent to minimize a cost function. The cost function is defined as: {mathdisplay}J = \frac{1}{2} e(f)^2 = \frac{1}{2} (v_m - v_p(f))^2{mathdisplay} where _vm_ is the measured speed, _ |
where vm is the measured speed, vp(f)
...
is
...
the
...
speed
...
predicted
...
by
...
the
...
model
...
(which
...
depends
...
on
...
the
...
fitness
...
parameter
...
f
...
).
...
Applying
...
the
...
gradient
...
descent
...
method
...
gives
...
the
...
fitness
...
updating
...
rule
Mathdisplay |
---|
}f(k+1) = f(k) - \gamma e(k+1) \frac{de}{df}(k+1){mathdisplay} where {mathinline}\gamma{mathinline} is the gain which can be very low since the fitness does not change very rapidly. h4. STEER CONTROLLER h6. STEER INPUT - CURVATURE RADIUS RELATIONSHIP +Model+ If one considers the car to have an ideal lateral dynamics then the vehicle can be represented as in Figure [^car.png], where _w_ is the wheelbase and _delta_ is the steer angle. Actually the dynamics is affected by some slip angles that I am going to assume negligible for the purpose of this model. I am interested in the curvature radius of the center of the car (i.e. _R_) because that is the position tracked by the CPS. I assume also that the steer angle _delta_ is proportional to the steer input or, in other words, that _delta = c * u_, where _c_ is the steer factor and _u_ is the steer input which belongs to the interval (-100, 100). With some simple trigonometry one can obtain _Rr = w / tan(c * u)_ which means that _R = sqrt(Rr^2 + w^2/4) = sqrt(w^2 / tan(c * u) + w^2 / 4)_ The wheelbase can be directly measured on car. The only unknown parameter of the model is the steer factor _c_. +Parameter identification+ I run car 1 on circles with constant steer input and I manually measured the diameter of the circle it went through. Results are reported in the table below. All measures are reported in millimiters (with the exception of the input of course). Measured diameters for right and left turns is separated. The error in the table is the difference between the radius (not the diameter) predicted by the model and the real radius. || input || diameter right (mm) || diameter left (mm) || error right (mm) || error left (mm) || | 100 | 1360 | 1380 | 4.02 | \-5.98 | | 90 | 1450 | 1420 | 39.18 | 54.18 | | 80 | 1710 | 1680 | 8.90 | 23.90 | | 70 | 1940 | 1880 | 21.56 | 51.56 | | 60 | 2400 | 2290 | \-38.88 | 16.12 | | 50 | 2980 | 2960 | \-92.27 | \-82.27 | The steer factor was computed in order to minimize the average error which lead to the value _c_ = 0.2116466582. +Model validation+ The model has been validated with cars 2 and 3. Data is reported in the table below. CAR 2 || input || diameter right (mm) || diameter left (mm) || error right (mm) || error left (mm) || | 100 | 1340 | 1370 | 14.02 | \-0.98 | | 90 | 1340 | 1370 | 94.18 | 79.18 | | 80 | 1520 | 1600 | 103.90 | 63.90 | | 50 | 2440 | 2730 | 177.73 | 32.73 | Average radius error: 70.57 CAR 3 || input || diameter right (mm) || diameter left (mm) || error right (mm) || error left (mm) || | 100 | 1670 | 1560 | \-150.98 | \-95.98 | | 90 | 1670 | 1560 | \-70.82 | \-15.82 | | 80 | 1830 | 1720 | \-51.10 | 3.90 | | 50 | 2730 | 3030 | 32.73 | \-117.27 | Average radius error: \-58.17 +Notes+ * It should be noted that the reported error refers to curvature radius not diameter. This means that when car 2 completes a semicircle, on average the model mispredicts its position by 7.057cm * 2 = 14.114cm. According to the partial data gathered, the misprediction can be up to 36cm. Still, it is not required for the model to be 100% correct. Indeed the main idea behind the steer controller is to associate a reference steer input given by the identified model with a PID or a PI that corrects its error over time. Considering that it takes about 4 seconds for the car with steer input 50 and motor input 150 to complete the semicircle, the PID will have 4 seconds to correct an error of 36cm. * The trajectory followed by the car with constant steer is not a perfect circle but more similar to an ellipse with very low eccentricity. * After some tests, it seems that the curvature radius is not affected by the motor input. * The curvature radius of car 1 saturates for input above 100. Signals 110 and 120 gives exactly the same curvature radius. For cars 2 and 3 this happens even before at input 90. * The current diameter of the curve part of path fig8 is about 148cm. This means that car 1 and 2 can follow it while car 3 cannot. h6. LINEAR MODEL The linear model used for the steer controller design is described by the block diagram in Figure [^lat_block.jpg]. Symbols represent * _U(s)_: desired lateral distance from the path which is always 0. * _C(s) = c / s_: constant compensation term of the controller computed by using the model steer input-curvature radius. * _M(s) = m / s_: constant term that accounts for the error of the model used to determine the compensation. * _D(s) = d / s^2_: disturbance term that models the curve in the path. * _w_: the wheelbase. * _alpha_: the steer factor. * _delta_: steer input in (-100, 100). * _X(s)_: controlled variable, the lateral distance from the path. The Laplace transform of the system is _X(s) = (-v0 * d * w + alpha * v0^2 * c + alpha * v0^2 * m) / (s * (w * s^2 + alpha * v0^2 * k2 * s + alpha * v0^2 * k1))_ If we set c to cancel the effect of _d_, then the error is caused by _m_. By applying the final value theorem and cancel the effect of _d_ with _c_, one obtains _lim x(t) = m / k2_ for _t -> inf_ Thus the final error depends only on k2 as reasonable to expect. Recalling that _m_ must be a steer input, it is easy to compute the maximum value of _m_. By looking at the maximum error in the validation data for the [#STEER INPUT - CURVATURE RADIUS RELATIONSHIP] I obtained _m = 13.3045_. For the derivation of the model check [^formulae.pdf]. The model does not take into account the measurements disturbances (leaps included) and it considers negligible disturbances associated with the actuator, projection on the path and slip angles. h6. CONTROLLER CALIBRATION The controller used is a PD in the form _delta = k1 * x' + k2 * x = k1 * (beta - gamma) + k2 * x_ where _beta_ and _gamma_ are respectively the direction of the car and of the path in radians. The system has a pole in 0 and a pair of complex poles for _|k1| < +- sqrt(4 * w * k2 / (alpha * v0^2)_ The pair of poles are _p = - alpha * v0^2 * k1 / 2w +- i * sqrt(4w * alpha * v0^2 * k_2 - alpha^2 * v_0^4 * k1^2) / 2w_ We want the final error _ef_, the settling time _ts_ and the frequency of oscillation _wd_ to be low or, in other words, we want to keep low the quantities _ts = -4.6 / Re(p)_ and _wd = Im(p)_. With _k1 = k2 = 0.3_ and _v0 = 800_ we obtain _ef = 44.34mm_, _ts = 3.37s_ and _wd = 0.9314_. h6. RESULTS Figure [^distgamma.png] shows the measured distance from the path and the difference between the car and the path direction (gamma). The controller keeps the error of the distance from the path below 10cm and gamma below 10 degrees. This does not happen only when leaps occur because of the change of the tracking camera. The (signed) average error during a curve is -3.57mm for the distance and -0.07 degrees for gamma which is very good for the predictor. The average squared error is 49.64mm for the distance and 5.83 degrees for gamma. Figure [^distold.png] shows the error of the distance with the old controller instead. In curve the average error was about 180mm. It should be noted that this is the distance measured with the cameras which is affected by an error. h4. CPS h6. CAMERA MEASUREMENT ERROR CORRECTION +Problem+ The position of the car on the test-bed computed by the CPS is affected by a considerable error. I made some manual measurement of this error by finding the real position with the measuring tape and checking the computed position on the CPS and I found it to be be up to 25cm. Moreover, when the tracking of a car pass from a camera to another, the global coordinates "leap" because the position error in the transition point is different for the two cameras. From the experiments on the path fig8, this leap can be up to 30cm. This negatively affects the ability of the car to accurately follow a path which is a crucial aspect in limiting disturbances. I made some investigation on the trend of this error. The results are shown in [^error_trend.png]. In the figure, "x_loc" indicate the x coordinate in pixels in the local coordinate system of the camera (i.e. the horizontal one), while "x_glob" is the x coordinate in cm in the global coordinate system. Remember that the axises of the local and the global coordinate systems are inverted. Data where gathered for two cameras. +Solution+ I decided to apply a linear correction to the computed global coordinates error along both direction. The correction is thus applied in the form err = a*x_loc*y_loc + b*x_loc + c*y_loc + d. The parameters a, b, c, d are computed by the CPS on start by loading a file where the real global coordinates of four points must be saved. This is the procedure that must be followed to configure this file. This procedure must be repeated for each camera. It is calibration-independent, meaning that you do not have to repeat it if you have to perform a new instrinsic/extrinsic calibration, but if the camera is moved, the procedure must be done again. # Set the variable RECORD_OBJECT_DATA = 1 in the file CPS.h in the computer responsible for the camera that you want to configure and compile it. For information about compiling the CPS, check the [original lab documentation|DelVecchioLab:Lab Documentation]. # Run CPS.exe. The RECORD_OBJECT_DATA mode was designed to take pictures of the cars symbols so it will ask you the car number and the section number, just put a negative number. The only thing you must insert correctly is the camera number that you want to configure. # Determine the four points to record. Considering the error trend, the four points *should be chosen to form the broadest rectangle of interest*, that is the rectangle with the biggest area contained in the camera view where the car can be tracked. To clarify, an example of how to choose the points is found in [^point_selection.png]. Walls, obstacles and the end of the sections limit the rectangle. The console gives you information about the pixel coordinates of the top left corner of the small squared boxes, in the figure it is located close to (x2, y2). You can move that box with keys "a", "s", "d" and "w". Write down the pixel coordinates of those four points. # Now measure the global position of those four points with a measure tape and record them. You should now have written down 4 pixel coordinates (x1, y1, x2, y2) and 8 global coordinates, 2 for each point of the rectangle. Pay attention when writing down the global coordinates. Since the x and y axises of the local and the global coordinate system are inverted, it is easy to make confusion. # Open the folder "Desktop/camera_programs/Andrea CPS/calib_data". There 5 files called "error_camx.txt", where "x" is the number of the camera. Open the one that refers to the camera you are configuring. The format of the file is very easy to understand and consistent with this explanation. Write there the pixel coordinates of x1, y1, x2, y2 and the global coordinates of those points. # Now you can set RECORD_OBJECT_DATA to 0 and you are good to go. +Results+ The error correction gave a visible improvement to the steer control performance. The car stays much closer to the given path. The error in camera 5 and 2 is constantly below 10cm and rarely above 5cm. Moreover the measured position "leaps" are reduced. I was not able to measure a leap above 15cm. h6. HEADING FILTERING Leaps affect the measured heading too since it is computed from positions. I have thus implemented a model-based filter to eliminate heading jumps. The filter is applied in the car, not in the CPS because it needs the speed from the encoder. In short, the filter detects when a leap occurs and use the model heading instead of using the measurement when that happens. The algorithm computes the following quantities: _measured_heading = arctan((curr_x2 - prev_x2) / (curr_x1 - prev_x1))_ This is the measured heading which is affected by leaps. The CPS actually computes and sends this but it makes some filtering first. When jumps are very small, this filtering causes the measured heading to become wrong but not enough to discard it. For consistency reasons then we keep the newly computed measured heading instead of the CPS one. _model_heading = prev_speed * dT / R = 2 * prev_speed * tan(c*prev_steer) * dT / (w * sqrt(4 * tan^2(c*prev_steer)))_ Where _R_ is the curvature radius obtained from the model described in section [STEER INPUT - CURVATURE RADIUS RELATIONSHIP], _c_ is the steer factor and _w_ is the wheelbase of the car. This is the current heading according to the model. _measured_Dpos = sqrt((prev_x1 - curr_x1)^2 + (prev_x2 - curr_x2)^2)_ The distance covered measured from the previous step. _model_Dpos = prev_speed * dT_ The distance that the car should have covered. The algorithm is the following: # if _|measure_heading - model_heading| > 30degrees_ ## keep _model_heading_ # else if _|measure_heading - model_heading| > 20degrees and |measure_Dpos - model_Dpos| > 0.35 * model_Dpos_ // this is the uncertain case ## keep _model_heading_ # else ## keep _measured_heading_ Some leaps occurs more or less in the direction of the car but they can be still detected because the car covers a much longer (or shorter) distance than it should. The second if-statement detects this kind of leaps. Thresholds have been determined empirically. You can see this filter as a simple camera change detector that applies a Kalman filter with time-varying error covariance. The measurement error covariance is 0 when camera change is not detected (i.e. the Kalman filter keeps the measurement) and non-zero otherwise. On the contrary, the a priori estimate error covariance is 0 when camera change is detected (i.e. the Kalman filter keeps the model) and non-zero otherwise. Figure [^Dheading.png] shows _Dheading = |filtered_heading - previous_heading|_ in time. As you can see basically all the leaps caused by the change of the tracking camera are removed. This improved the path following performances. h6. CPS MEASURED 2D SPEED +The issues with CPS measured 2D speed detected in this testing are still unresolved. This is not currently a priority since all the encoders are working but these issues should be deepen in case the estimated 2D speed will actually be used for the prediction.+ CPS now compute from a difference of position the 2D speed of cars. The reason I implemented this is because the encoder of car 2 (which is now fixed) was not working and I needed to measure its speed. I made a test to see if this new measurement was reliable. I run car2 with fixed steer and PWM in a circle whose diameter I manually measured to be about 200cm. The car completed 5 circles. Results are in figure [^speed_cps_encoder.png]. The left one is the raw speed, in the right one peaks are removed and the CPS speed is filtered with a average filter window (the window width is 5). The manually computed average speed is 835.66 mm/s, the encoder average speed is 857.69 mm/s and the cps average speed is 895.65 mm/s. In order for the encoder to be the real average speed, the actual diameter should be 5.4 cm greater. In case of the CPS one it should be 14.4cm greater which is less likely. Few observations: * peaks in CPS speed are likely to be caused by position leaps between cameras; * CPS speed average is above the one measured with the encoder. h4. ENGINE COMPENSATOR h6. PATH-DEPENDENT DISTURBANCES IDENTIFICATION h4. PATH SPECIFICATION SYNTAX h6. COMPENSATOR DIRECTIVES In order to apply the compensation, the controller must know the curvature radius of the path. You can specify it by adding to the directive to the path definition file. For example, in _fig8CL_precise.txt_ the first two lines are now {code} # 1 648 750.07285 # 651 1298 -750.07285 {code} which means that the curvature radius of the path from target 1 to target 648 is 750.07285mm, while from target 651 to 1298 is -750.07285. The curvature radius specified must be positive if the car must turn right, negative to make the vehicle turn left. h6. PATH-DEPENDENT SPEED DISTURBANCE SPECIFICATION |
where
Mathinline |
---|
Important The fitness online estimation works on the assumption that the mean of the speed disturbance is 0. If this does not hold, the estimation is not reliable.
STEERING CONTROLLER
...
STEERING INPUT - CURVATURE RADIUS RELATIONSHIP
...
Model
If one considers the car to have an ideal lateral dynamics then the vehicle can be represented as in Figure car.png, where w is the wheelbase and delta is the steering angle. Actually the dynamics is affected by some slip angles that I am going to assume negligible for the purpose of this model. I am interested in the curvature radius of the center of the car (i.e. R) because that is the position tracked by the CPS. I assume also that the steering angle delta is proportional to the steering input or, in other words, that delta = c * u, where c is the steering factor and u is the steering input which belongs to the interval (-100, 100).
With some simple trigonometry one can obtain
Rr = w / tan(c * u)
which means that
R = sqrt(Rr^2 + w^2/4) = sqrt(w^2 / tan(c * u) + w^2 / 4)
The wheelbase can be directly measured on car. The only unknown parameter of the model is the steering factor c.
Parameter identification
I run car 1 on circles with constant steering input and I manually measured the diameter of the circle it went through. Results are reported in the table below. All measures are reported in millimiters (with the exception of the input of course). Measured diameters for right and left turns is separated. The error in the table is the difference between the radius (not the diameter) predicted by the model and the real radius.
input | diameter right (mm) | diameter left (mm) | error right (mm) | error left (mm) |
---|---|---|---|---|
100 | 1360 | 1380 | 4.02 | -5.98 |
90 | 1450 | 1420 | 39.18 | 54.18 |
80 | 1710 | 1680 | 8.90 | 23.90 |
70 | 1940 | 1880 | 21.56 | 51.56 |
60 | 2400 | 2290 | -38.88 | 16.12 |
50 | 2980 | 2960 | -92.27 | -82.27 |
The steering factor was computed in order to minimize the average error which lead to the value c = 0.2116466582.
Model validation
The model has been validated with cars 2 and 3. Data is reported in the table below.
CAR 2
input | diameter right (mm) | diameter left (mm) | error right (mm) | error left (mm) |
---|---|---|---|---|
100 | 1340 | 1370 | 14.02 | -0.98 |
90 | 1340 | 1370 | 94.18 | 79.18 |
80 | 1520 | 1600 | 103.90 | 63.90 |
50 | 2440 | 2730 | 177.73 | 32.73 |
Average radius error: 70.57
CAR 3
input | diameter right (mm) | diameter left (mm) | error right (mm) | error left (mm) |
---|---|---|---|---|
100 | 1670 | 1560 | -150.98 | -95.98 |
90 | 1670 | 1560 | -70.82 | -15.82 |
80 | 1830 | 1720 | -51.10 | 3.90 |
50 | 2730 | 3030 | 32.73 | -117.27 |
Average radius error: -58.17
Notes
- It should be noted that the reported error refers to curvature radius not diameter. This means that when car 2 completes a semicircle, on average the model mispredicts its position by 7.057cm * 2 = 14.114cm. According to the partial data gathered, the misprediction can be up to 36cm. Still, it is not required for the model to be 100% correct. Indeed the main idea behind the steering controller is to associate a reference steering input given by the identified model with a PID or a PI that corrects its error over time. Considering that it takes about 4 seconds for the car with steering input 50 and motor input 150 to complete the semicircle, the PID will have 4 seconds to correct an error of 36cm.
- The trajectory followed by the car with constant steering is not a perfect circle but more similar to an ellipse with very low eccentricity.
- After some tests, it seems that the curvature radius is not affected by the motor input.
- The curvature radius of car 1 saturates for input above 100. Signals 110 and 120 gives exactly the same curvature radius. For cars 2 and 3 this happens even before at input 90.
- The current diameter of the curve part of path fig8 is about 148cm. This means that car 1 and 2 can follow it while car 3 cannot.
...
LINEAR MODEL
...
The linear model used for the steering controller design is described by the block diagram in Figure lat_block.jpg. Symbols represent
- U(s): desired lateral distance from the path which is always 0.
- C(s) = c / s: constant compensation term of the controller computed by using the model steering input-curvature radius.
- M(s) = m / s: constant term that accounts for the error of the model used to determine the compensation.
- D(s) = d / s^2: disturbance term that models the curve in the path.
- w: the wheelbase.
- alpha: the steering factor.
- delta: steering input in (-100, 100).
- X(s): controlled variable, the lateral distance from the path.
The Laplace transform of the system is
X(s) = (-v0 * d * w + alpha * v0^2 * c + alpha * v0^2 * m) / (s * (w * s^2 + alpha * v0^2 * k2 * s + alpha * v0^2 * k1))
If we set c to cancel the effect of d, then the error is caused by m. By applying the final value theorem and cancel the effect of d with c, one obtains
lim x(t) = m / k2 for t -> inf
Thus the final error depends only on k2 as reasonable to expect. Recalling that m must be a steering input, it is easy to compute the maximum value of m. By looking at the maximum error in the validation data for the #STEERING INPUT - CURVATURE RADIUS RELATIONSHIP I obtained m = 13.3045. For the derivation of the model check formulae.pdf. The model does not take into account the measurements disturbances (leaps included) and it considers negligible disturbances associated with the actuator, projection on the path and slip angles.
...
CONTROLLER CALIBRATION
...
The controller used is a PD in the form
delta = k1 * x' + k2 * x = k1 * (beta - gamma) + k2 * x
where beta and gamma are respectively the direction of the car and of the path in radians. The system has a pole in 0 and a pair of complex poles for
|k1| < +- sqrt(4 * w * k2 / (alpha * v0^2)
The pair of poles are
p = - alpha * v0^2 * k1 / 2w +- i * sqrt(4w * alpha * v0^2 * k_2 - alpha^2 * v_0^4 * k1^2) / 2w
We want the final error ef, the settling time ts and the frequency of oscillation wd to be low or, in other words, we want to keep low the quantities ts = -4.6 / Re(p) and wd = Im(p). With k1 = k2 = 0.3 and v0 = 800 we obtain ef = 44.34mm, ts = 3.37s and wd = 0.9314.
...
RESULTS (STEERING CONTROLLER)
...
Figure distgamma.png shows the measured distance from the path and the difference between the car and the path direction (gamma). The controller keeps the error of the distance from the path below 10cm and gamma below 10 degrees. This does not happen only when leaps occur because of the change of the tracking camera. The (signed) average error during a curve is -3.57mm for the distance and -0.07 degrees for gamma which is very good for the predictor. The average squared error is 49.64mm for the distance and 5.83 degrees for gamma.
Figure distold.png shows the error of the distance with the old controller instead. In curve the average error was about 180mm. It should be noted that this is the distance measured with the cameras which is affected by an error.
CPS
...
CAMERA MEASUREMENT ERROR CORRECTION
...
Problem
The position of the car on the test-bed computed by the CPS is affected by a considerable error. I made some manual measurement of this error by finding the real position with the measuring tape and checking the computed position on the CPS and I found it to be be up to 25cm. Moreover, when the tracking of a car pass from a camera to another, the global coordinates "leap" because the position error in the transition point is different for the two cameras. From the experiments on the path fig8, this leap can be up to 30cm. This negatively affects the ability of the car to accurately follow a path which is a crucial aspect in limiting disturbances.
I made some investigation on the trend of this error. The results are shown in error_trend.png. In the figure, "x_loc" indicate the x coordinate in pixels in the local coordinate system of the camera (i.e. the horizontal one), while "x_glob" is the x coordinate in cm in the global coordinate system. Remember that the axises of the local and the global coordinate systems are inverted. Data where gathered for two cameras.
Solution
I decided to apply a linear correction to the computed global coordinates error along both direction. The correction is thus applied in the form err = a*x_loc*y_loc + b*x_loc + c*y_loc + d. The parameters a, b, c, d are computed by the CPS on start by loading a file where the real global coordinates of four points must be saved. This is the procedure that must be followed to configure this file. This procedure must be repeated for each camera. It is calibration-independent, meaning that you do not have to repeat it if you have to perform a new instrinsic/extrinsic calibration, but if the camera is moved, the procedure must be done again.
- Set the variable RECORD_OBJECT_DATA = 1 in the file CPS.h in the computer responsible for the camera that you want to configure and compile it. For information about compiling the CPS, check the original lab documentation.
- Run CPS.exe. The RECORD_OBJECT_DATA mode was designed to take pictures of the cars symbols so it will ask you the car number and the section number, just put a negative number. The only thing you must insert correctly is the camera number that you want to configure.
- Determine the four points to record. Considering the error trend, the four points should be chosen to form the broadest rectangle of interest, that is the rectangle with the biggest area contained in the camera view where the car can be tracked. To clarify, an example of how to choose the points is found in point_selection.png. Walls, obstacles and the end of the sections limit the rectangle. The console gives you information about the pixel coordinates of the top left corner of the small squared boxes, in the figure it is located close to (x2, y2). You can move that box with keys "a", "s", "d" and "w". Write down the pixel coordinates of those four points.
- Now measure the global position of those four points with a measure tape and record them. You should now have written down 4 pixel coordinates (x1, y1, x2, y2) and 8 global coordinates, 2 for each point of the rectangle. Pay attention when writing down the global coordinates. Since the x and y axises of the local and the global coordinate system are inverted, it is easy to make confusion.
- Open the folder "Desktop/camera_programs/Andrea CPS/calib_data". There 5 files called "error_camx.txt", where "x" is the number of the camera. Open the one that refers to the camera you are configuring. The format of the file is very easy to understand and consistent with this explanation. Write there the pixel coordinates of x1, y1, x2, y2 and the global coordinates of those points.
- Now you can set RECORD_OBJECT_DATA to 0 and you are good to go.
Results
The error correction gave a visible improvement to the steering control performance. The car stays much closer to the given path. The error in camera 5 and 2 is constantly below 10cm and rarely above 5cm. Moreover the measured position "leaps" are reduced. I was not able to measure a leap above 15cm.
...
HEADING FILTERING
...
Leaps affect the measured heading too since it is computed from positions. I have thus implemented a model-based filter to eliminate heading jumps. The filter is applied in the car, not in the CPS because it needs the speed from the encoder. In short, the filter detects when a leap occurs and use the model heading instead of using the measurement when that happens. The algorithm computes the following quantities:
measured_heading = arctan((curr_x2 - prev_x2) / (curr_x1 - prev_x1))
This is the measured heading which is affected by leaps. The CPS actually computes and sends this but it makes some filtering first. When jumps are very small, this filtering causes the measured heading to become wrong but not enough to discard it. For consistency reasons then we keep the newly computed measured heading instead of the CPS one.
model_heading = prev_speed * dT / R = 2 * prev_speed * tan(c*prev_steer) * dT / (w * sqrt(4 * tan^2(c*prev_steer)))
Where R is the curvature radius obtained from the model described in section STEERING INPUT - CURVATURE RADIUS RELATIONSHIP, c is the steering factor and w is the wheelbase of the car. This is the current heading according to the model.
measured_Dpos = sqrt((prev_x1 - curr_x1)^2 + (prev_x2 - curr_x2)^2)
The distance covered measured from the previous step.
model_Dpos = prev_speed * dT
The distance that the car should have covered. The algorithm is the following:
- if |measure_heading - model_heading| > 30degrees
- keep model_heading
- else if |measure_heading - model_heading| > 20degrees and |measure_Dpos - model_Dpos| > 0.35 * model_Dpos // this is the uncertain case
- keep model_heading
- else
- keep measured_heading
Some leaps occurs more or less in the direction of the car but they can be still detected because the car covers a much longer (or shorter) distance than it should. The second if-statement detects this kind of leaps. Thresholds have been determined empirically. You can see this filter as a simple camera change detector that applies a Kalman filter with time-varying error covariance. The measurement error covariance is 0 when camera change is not detected (i.e. the Kalman filter keeps the measurement) and non-zero otherwise. On the contrary, the a priori estimate error covariance is 0 when camera change is detected (i.e. the Kalman filter keeps the model) and non-zero otherwise.
Figure Dheading.png shows Dheading = |filtered_heading - previous_heading| in time. As you can see basically all the leaps caused by the change of the tracking camera are removed. This improved the path following performances.
...
CPS MEASURED 2D SPEED
...
The issues with CPS measured 2D speed detected in this testing are still unresolved. This is not currently a priority since all the encoders are working but these issues should be deepen in case the estimated 2D speed will actually be used for the prediction.
CPS now compute from a difference of position the 2D speed of cars. The reason I implemented this is because the encoder of car 2 (which is now fixed) was not working and I needed to measure its speed. I made a test to see if this new measurement was reliable. I run car2 with fixed steering and PWM in a circle whose diameter I manually measured to be about 200cm. The car completed 5 circles. The debug file is stored in "Andrea/data/cps_2d_speed_8-17-2013/".
Results are in figure speed_cps_encoder.png. The left one is the raw speed, in the right one peaks are removed and the CPS speed is filtered with a average filter window (the window width is 5). The manually computed average speed is 835.66 mm/s, the encoder average speed is 857.69 mm/s and the cps average speed is 895.65 mm/s. In order for the encoder to be the real average speed, the actual diameter should be 5.4 cm greater. In case of the CPS one it should be 14.4cm greater which is less likely.
Few observations:
- peaks in CPS speed are likely to be caused by position leaps between cameras;
- CPS speed average is above the one measured with the encoder.
ENGINE COMPENSATOR
The engine compensator reduces the transient due to the capacitor and the effects of steering and testbed inclination on the speed by introducing a compensation term in the PWM signal. The model takes the form
Mathdisplay |
---|
\dot{v}(t) = av(t) + b + (f + g e^{-\frac{t}{\tau}}) (u(t) + c(t, y)) + \dot{D}(y) |
where D(y) = Dsteer + Dslope (see #FINAL MODEL WITH DISTURBANCES) and c(t, y) is the compensation term of the input. The reason why D(y) depends on the position y is related to the fact that the slope and the steering input used are about the same point of the path. The reason why it does not depend on time will be clear in the next section. By imposing the cancellation of the capacitor effect and D(y) it is easy to obtain:
Mathdisplay |
---|
c(t, y) = \frac{-g e^{-t / \tau} - \dot{D}(y)}{g e^{-t / \tau} + f} |
The constant parameters g and T can be estimated by following the instructions in #MODEL PARAMETERS IDENTIFICATION. The parameter f is estimated online (see #FITNESS ONLINE ESTIMATION). To determine D'(y) instead, follow the procedure described in the next section.
Note that the engine compensator assumes that the car starts with the capacitor in equilibrium. When you stop the car, the capacitor starts recharging again. If you want the compensator to work, you need to wait until this process is done. It takes about 200 seconds for the transient to reach its steady state but if you have run the car for a shorter time you can reduce the await. Plug the charger to speed up the process.
...
PATH-DEPENDENT DISTURBANCES IDENTIFICATION
...
Figure dist_time_dependency.png shows the trend of the disturbance D in time calculated as the subtraction between the speed measured by the encoder and the speed of the model. As one can see, the disturbance exhibits roughly the same pattern in time. This is why the model consider D as independent of time. Figure dist_position_dependency.png instead plots D on the position along the path. The dependency is clear. Moreover, the disturbance seems to be slightly influenced by the PWM, or in other words by the car speed. The model neglects it and assume D to depend only on the position. The data of these experiments are stored in "Andrea/data/path_dependent_15-9-2013/".
To identify the function D(y) I wrote a script called "path_disturb.py" in "Andrea/scripts" which takes two debug files from ca2 and prints on the console the disturbance function in the format that ca2 understands (see #PATH-DEPENDENT SPEED DISTURBANCE SPECIFICATION). The first debug file is obtained by running the car in circle. This is needed in order to estimate the fitness value. Indeed, since at this point we cannot assume the disturbances to have average 0, the fitness online estimation does not work. The second debug file must be obtained by running the car on the path with the engine compensator canceling the transient due to the capacitor. The detailed procedure is below. The data used for the disturbance funcion identification are stored in "Andrea/data/path_disturb/".
Detailed procedure
- If you did not do it yet, add to the path specification file the steering compensator directives (see #COMPENSATOR DIRECTIVES).
- If you did not do it yet, identify the model parameters as described in #MODEL PARAMETERS IDENTIFICATION. If you have performed the model parameters identification today, you can skip to step 6 and use the debug file obtained from one of the two running in circle to estimate the fitness parameter.
- Unplug the charger, use the multimeter to record the voltage level of the battery and plug it in again.
- Run the car in circle as described in the model parameters identification section. For the input signal I suggest 140 PWM, -50 steering. The steering should be the same as the one used for the model parameters identification. The PWM can be different.
- Plug the charger again, connect to the car with sftp save the "data_carX.txt" file.
- Open "path_disturb.py" on your computer with a text editor and fill the configuration section. You must set
ONLY_FITNESS
toTrue
and specify the path to the path specification file, the car number, the name of the debug file and its containing folder. It is not important to setPATH_PATH_DATA
since you do not have that file yet. - Run "path_disturb.py". It will output the estimated fitness value.
- Go back to the car console and move to the project folder (e.g.
cd Desktop/brainstem/aProject/Andrea
) - Open "constants.h" with a text editor (e.g.
nano util/constants.h
). Set the correct element of the arraySTART_FIT
to the fitness value that the script output. - Open "const_car.h" with a text editor (e.g.
nano car_src/const_car.h
and setSTART_PWM
to the same value used for the experiment in circle. Be sure that the following constants are set as belowNo Format #define START_STEER 0 #define DURATION_EXP 2100 #define CODE_IN_CAR 1 #define GET_POSITIONS 1 #define USE_ENCODER 1 #define SUPERVISOR 0 #define PATHPLAN_ON 1 #define STEER_COMPENSATOR 1 #define ENGINE_COMPENSATOR 2 # Set this to 2 makes the compensator cancel the capacitor effect while ignoring the online estimation and keeping the fitness value you just specified #define DEBUG 1
- Compile the code with
make
. - Move to the executable directory (e.g.
cd ../../aDebug/aUnix/i686
). - Open the path specification file and delete all the lines that start with a '*' character which represent the previous identified disturbance function.
- Now turn off the car and let the battery charge level reach the same value as before the first experiment (if you have just identified the model parameters, charge it to the voltage used for the experiment in circle you performed for the identification).
- When it is done, run the car on the path and save the "data_carX.txt" file (pay attention to not overwrite the previous one) and plug the charger.
- On you computer, open "path_disturb.py". Set
ONLY_FITNESS
toFalse
andPATH_PATH_DATA
. The other variables should be already correctly configured from the previous execution. - Run "path_disturb.py" saving the result on a file (e.g.
python path_disturb.py >> temp.txt
). - Copy the content of the newly created file "temp.txt" into the path specification file in "Andrea/paths", just below the compensator directives (the lines that start with '#').
- Go back on the car console. Upload the new path specification file in the executable folder of the car (overwriting the previous one if you do not need it anymore).
- Remember to set the constant
ENGINE_COMPENSATOR
in "car_src/const_car.h" back to 1 (I kept forgetting this) or the fitness estimation will not be considered. - Turn off the car.
- Delete the file "temp.txt"
When to repeat the procedure
You will have to repeat the procedure every time you:
- modify the path;
- modify the steering compensator directives;
- change the model parameters.
Results
Figure v_disturb.png shows the disturbances after the application of the compensator. The spike at the beginning is mainly due to a defect in the encoder measurement. The speed than does not get further than 50mm/s from the model.
PATH SPECIFICATION SYNTAX
The path specification file now does not contain only the points of the path but it also specifies other path-dependent quantities that are parsed by ca2 at execution.
...
COMPENSATOR DIRECTIVES
...
In order to apply the steering compensation, the controller must know the curvature radius of the path. The syntax used to specify is the following.
No Format |
---|
# 1 648 750.07285
# 651 1298 -750.07285
|
These two lines mean that the curvature radius of the path from target point 1 to target 648 is 750.07285mm, while from target 651 to 1298 is -750.07285mm. Note that the steering compensator directives lines must start with '#'. The curvature radius specified is positive if the car must turn right, negative to make the vehicle turn left.
...
PATH-DEPENDENT SPEED DISTURBANCE SPECIFICATION
...
The disturbance D(y) (see section #ENGINE COMPENSATOR) is represented by a linear piecewise function defined in the path specification file. The syntax is the following.
No Format |
---|
* 50
* -1.48690195077
* -0.764317798416
* ...
* 28.9035206284
|
Each line start with a '*' character. The first line is the y step, all the others are the values of the function. In the example above the step is 50mm and D(0) = -1.48690195077, D(50) = -0.764317798416, D(50n) = 28.9035206284.
SUPERVISOR
...
STATE AND DISTURBANCES DEFINITION
...
The state consist of a bidimensional vector (y, v) where y is the unidimensional position along the path and v is the car speed. The model adopted by the supervisor is the #BASIC MODEL with the addition of two disturbances.
Mathdisplay |
---|
\dot{y}(t) = v(t) + d_y(t) |
Mathdisplay |
---|
\dot{v}(t) = av(t) + b + fu(t) + d_u(t) |
dy(t) accounts for the non ideal path following while du(t) represents the effects on the speed of the steering and the slope that the engine compensator is not able to cancel.
...
DETERMINING DISTURBANCES BOUNDARIES
...
The upper and lower bounds for the disturbances as they are defined above are very high. dy(t) can reach 250-300 mm/s while du(t) can get up to 350 mm/s^2. These boundaries makes the prediction too conservative for the experiment to work on the testbed. Indeed, as soon as a car exits the intersection and makes a brand new prediction, the algorithm detects an inevitable collision, thus the safe input remains undefined and the code makes the cars to stop. The confined space available for the experiment makes it difficult to solve the problem by changing the paths drastically.
What we truly care is to be able to predict the time at which a car enters and exits the intersection. I exploited this fact to circumvent the problem. Let tf be the time at which the car enters (or exits) the intersection. We define dymin and dymax as the disturbances bounds such that
Mathdisplay |
---|
\int_{t_0}^{t_f}v(t) + d_{ymin} dt \le y(t_f) = \int_{t_0}^{t_f}\dot{y}(t)dt = \int_{t_0}^{t_f}v(t) + d_y(t)dt \le \int_{t_0}^{t_f}v(t) + d_{ymax}dt
|
for every t0, state x0 = (y0, v0), input u(t) and disturbance dy(t). One can define dvmin and dvmax similarly. It should be noted that defining the boundaries in this way allows the state to get temporarily outside its predicted boundaries. In other words, we do not care what happens between t0 and tf as long as we know that the prediction will be correct at the intersection.
The only problem with this approach is that the one-step prediction may be wrong because the boundaries the predictor uses are not the actual upper and lower bounds of the system. This is not a real problem for the deadline and release time prediction, but it may happen that the interval of states measured and the interval of state predicted at the previous step do not have an intersection. If this happens, it means that you probably have to increase the measurement uncertainty bounds of the model. Note that, with the disturbances bounds definition above, the final interval of states that the measurement corrector outputs may not contain the real state of the car. Again, the algorithm still works fine as long as the prediction to the intersection works.
I first tried to write a script to estimate quantitatively the boundaries by analyzing the experimental data (you can find it in "scripts/predictor_disturb.py"). However this script largely overestimate the bounds so I used a trial and error approach and I determined them in order to pass the test implemented in test_long_term_prediction()
(in "scripts/test_supervisor.py").
...
ONLINE FITNESS ESTIMATION CONSEQUENCES
...
The algorithm requires the model to not change in time. However, with the online fitness estimation this cannot be satisfied. For this reason I was forced to implement a workaround and update the fitness parameter only if it did not affect negatively the supervisor. Essentially when the supervisor detects an unavoidable collision with the newly estimated fitness, the parameter is brought back to its value at the previous step and the algorithm is repeated.
...
RESULTS (SUPERVISOR)
...
The algorithm worked with car1 and car2 running respectively on fig8AL_precise and fig8CL_precise. The prediction still resulted too much conservative for the third car to work. There is a video of the successful experiment in the dropbox folder. Figure bad_set.png shows how the trajectories of the two cars avoid the bad set (red square). The debug files of the experiment can be found in "Andrea/data/successful_2car/".