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 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:
All the files (source code and data) are saved in the folder "Dropbox/Shared Andrea/", from now on simply "Andrea/". If you move the files remember to update the documentation here. It contains 4 subfolders:
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
CPS
Communication protocol
Paths
There are 4 files which contains basically all the constants in the code. These should be the ones to check whenever you want to configure something.
The specific effect of each constants is documented in the code.
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.
Compatibility
It is implemented in C and it surely works on the last version of Ubuntu. 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
cd scripts ./compile.sh |
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.
./simulator |
and then run the car code with the normal syntax on a different console.
./ca2 paths/figX_precise.txt |
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 #SCRIPTS to understand its usage.
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 section on creating new branch on main documentation).
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
The compilation of the new code does not differ from the previous versions. Just type make (or make new in case you have used make clean before) in the project folder. Make sure that the constant CODE_IN_CAR in file "car_src/const_car.h" is set to 1.
Paths
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 foflder "/Desktop/brainstem/aDebug/aUnix/i686".
The scripts are written in Python. Most of them rely on the debug file that ca2 leaves after execution. 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.
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" which is integrated by the script with the information in the car's file. 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 can be used to debug experimental data (not simulated) using the command line option "o" (see below).
Command line options
Simulator
ca2
In this section a quantitative analysis of the disturbances in the system is attempted. In order to deal in an easier way with the assumption of monotonicity and with the coupling between the steer and the motor input, the model is linearized and the lateral and the longitudinal components are separated. The coupling effect is seen as a disturbance. In the following the coordinate system will be relative to the path as represented in Figure coord_sys.jpg. In other words, y represents the position along the path while x the lateral distance from the path.
A linear model of the car along the longitudinal component of the path is represented in figure long_block.jpg. Symbols represent
Dslope and Dslip
Dslope and Dslip are path dependent. I plan to measure them experimentally and treat them as a path-dependent known disturbance.
Djump
The path fig8 is composed by 649 control points for a total length of 9465.53 millimeters. The angle difference between two consecutive segments is delta = 0.9 degrees. We set the maximum distance of the car from the path to be xmax = 30cm (which is realistic with the current steer control performances). Assuming the car x coordinate (distance from the path) to be constant at xmax, we can compute Djump = 649 * delta * xmax = 3058.34mm which is the 32.31% of the path length.
It is clear that the steer control performance affects considerably the behavior of the vehicle. Consequently lots of efforts should be done to improve it. Besides the designing of the controller, the ability of the car to stay close to the path is affected by the measurement error of cameras. Thus, limiting these disturbances should follow two directions:
Work in Progress. The complete analysis still have to be performed. Still, the partial conclusions I found are driving the research. This led to the camera measurement connection and it is currently focusing my attention on the implementation of a new steer controller.
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
I made car1 run on circles for 50 seconds with fixed PWM and steer 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 my folder on Dropbox in "../backup/data/circle_7-27-2013". All the images below are obtained by filtering the encoder signal with a moving average window to discard the large part 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 keep the steer constant and varying the PWM. The relationship between PWM and velocity is quite linear. For some reason, when the steer input is high, the speed observed with PWM 140 is slightly lower than I would expect. On the other hand, the steer effect seems to be a little bit more complicated (see steer.png). Velocities observed for steer 92 and 120 are always very close. This may be due to the fact that curvature radius for the two steer input are very similar (the curvature radius is not linear with respect to the steer signal). Still the speed is not linear w.r.t. the curvature radius because the discrepancy between the speed for steer 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 side note, 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.
Work in Progress. These experiments were performed before the effect of the power filter capacitor on speed was clear and they will likely be done again for a longer time in order to get useful data.
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 steer and engine input. The steer 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 voltage of the battery was measured both before turning on the car (while off) and just before run ca2 (while on). The full data gathered and the detailed description of how the experiments were performed can be found in my folder on Dropbox in "../backup/data/battery_7-30-2013".
The behavior dynamic 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.
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.
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.
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:
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.
Work In Progress. 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 and can be used for the model identification. These issues will be deepen in case the 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:
The linear model used for the steer controller design is described by the block diagram in Figure lat_block.jpg. Symbols represent
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.
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.
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.
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.txt the first two lines are now
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.
