Page History

...

MODEL AND DISTURBANCES IDENTIFICATION

MODEL STATEMENT

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 thus that its speed treats the effects of the lateral dynamics as a disturbance. Under this hypothesis, the speed of the car is equal to the its unidimensional speed along the path. The system is thuscar is represented by the the systems of differential equations

\dot{y}(t) = v(t)

\dot{v}(t) = av(t) + b + fu(t)

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 called "fitness" the gain f.

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.
  1. 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.
  2. 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 is not very stressful 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 impossible to estabilsh 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 http://www.acroname.com/robotics/parts/S11-3A-EMF-HBRIDGE_datasheet.pdf and encoder datasheet in the electronic components catalog on the shelf).
• Steer is biased towards a direction. The steer cannot be set straight only mechanically. You must set the offset parameter in RoboCar_Lib.tea.

SOFTWARE

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:

• "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.

...

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 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 steer 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 4 files which contains 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.
• "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.

...

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.

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.

...

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 Lab Documentation#GeneralFAQ).

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

cd /Desktop/brainstem/aProject/Andrea
make clean
make new

in case you do not want to recompile the whole project but only the changed files you can simply type

make

instead of make clean followed by make new. To run the code type

cd /Desktop/brainstem/aDebug/aUnix/i686
./ca2 figX_precise.txt

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. 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.

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.

Consequences of modifying the ca2 debug file format - DebugData class
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 scripts 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 script rely on the 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.

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 parse 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

1. 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.
2. Open plot_trajectory.py and set the car number and the paths to debug files you want to analyze.
3. Run the script by typing python plot_trajectory.py on the console.
4. 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 attribute in time. You can configure which attribute to plot with the following precedure.

1. Open "plot_trajectory.py".
2. 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".
3. 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 print 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 #PARAMETERS IDENTIFICATION.

path_disturb.py
This script is used to determine the path-dependent disturbances. For details see section #PATH-DEPENDENT DISTURBANCES IDENTIFICATION.

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 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 the disturbances are effective for the prediction.
• test_exact_supervisor(): simulate the execution of the exact supervisor and print 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.

view_paths.py
This simply plots the paths in "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.

DISTURBANCES ANALYSIS

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.

...

LONGITUDINAL MODEL

...

A linear model of the car along the longitudinal component of the path is represented in figure long_block.jpg. Symbols represent

• PWM: the motor input signal.
• const: a constant term representing friction.
• W(s): transfer function between PWM signal and speed.
• v: 2D speed of the car.
• y': monodimensional speed obtained by projecting the 2D speed of the car on the path.
• y: monodimensional position of the car obtained by projecting its 2D position on the path.
• ym: monodimensional position of the car measured by cameras.
• Da: actuator disturbances on the control variable.
• Dslope: the test-bed is not perfectly flat but it is slightly inclined. This has been found to considerably affect the speed caused by slopes of the test-bed.
• Dslip: disturbances on the speed caused by the coupling with the lateral dynamics of the car (i.e. with the steer).
• Dproj: projecting the 2D speed on the path is modeled here as a disturbance.
• Djump: the path followed by cars is composed by a polygonal chain. This means that when the car goes from a segment to the next one, there is an interval of time during which the car position is projected on the same exact point of the path. For example, in Figure Djump.png the car goes through the trajectory A-B-C-D. While in the arc B-C, the vehicle position is projected on the path always in the same point P. Similar considerations can be done when a car cuts a corner. This disturbance is modeled with a negative (or positive in case the vehicle cut the corner) step that corrects the position each time the vehicle changes the segment of a path. Essentially, this is a measure of the difference between the length of the nominal path and the lenght of the actualy trajectory that the car follows.
• Dm: measurement error of the cameras.

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.

...

DISTRUBANCES ANALYSIS CONCLUSION

...

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:

• steer controller design;
• camera measurement error correction.

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.

CAR MODEL IDENTIFICATION

...

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.

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

Average radius error: 70.57

CAR 3

...

EFFECT OF PWM, STEER AND TESTBED SLOPE ON THE SPEED DYNAMICS

...

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. 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 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 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

1. The car speed depends indeed linearly on the PWM input signal as it is thus correctly modeled.
2. The steer and the testbed slope introduce a complex dynamics that is not taken into account by the model and should be considered as disturbance.

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.
  1. 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.
  2. 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 is not very stressful 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 impossible to estabilsh 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 http://www.acroname.com/robotics/parts/S11-3A-EMF-HBRIDGE_datasheet.pdf and encoder datasheet in the electronic components catalog on the shelf).
• Steer is biased towards a direction. The steer cannot be set straight only mechanically. You must set the offset parameter in RoboCar_Lib.tea.

SOFTWARE

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:

• "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.

...

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 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 steer 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 4 files which contains 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.
• "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.

...

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.

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.

...

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 Lab Documentation#GeneralFAQ).

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

cd /Desktop/brainstem/aProject/Andrea
make clean
make new

in case you do not want to recompile the whole project but only the changed files you can simply type

make

instead of make clean followed by make new. To run the code type

cd /Desktop/brainstem/aDebug/aUnix/i686
./ca2 figX_precise.txt

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. 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.

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.

Consequences of modifying the ca2 debug file format - DebugData class
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 scripts 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 script rely on the 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.

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 parse 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

1. 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.
2. Open plot_trajectory.py and set the car number and the paths to debug files you want to analyze.
3. Run the script by typing python plot_trajectory.py on the console.
4. 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 attribute in time. You can configure which attribute to plot with the following precedure.

1. Open "plot_trajectory.py".
2. 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".
3. 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 print 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 #PARAMETERS IDENTIFICATION.

path_disturb.py
This script is used to determine the path-dependent disturbances. For details see section #PATH-DEPENDENT DISTURBANCES IDENTIFICATION.

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 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 the disturbances are effective for the prediction.
• test_exact_supervisor(): simulate the execution of the exact supervisor and print 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.

view_paths.py
This simply plots the paths in "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.

DISTURBANCES ANALYSIS

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.

...

LONGITUDINAL MODEL

...

A linear model of the car along the longitudinal component of the path is represented in figure long_block.jpg. Symbols represent

• PWM: the motor input signal.
• const: a constant term representing friction.
• W(s): transfer function between PWM signal and speed.
• v: 2D speed of the car.
• y': monodimensional speed obtained by projecting the 2D speed of the car on the path.
• y: monodimensional position of the car obtained by projecting its 2D position on the path.
• ym: monodimensional position of the car measured by cameras.
• Da: actuator disturbances on the control variable.
• Dslope: the test-bed is not perfectly flat but it is slightly inclined. This has been found to considerably affect the speed caused by slopes of the test-bed.
• Dslip: disturbances on the speed caused by the coupling with the lateral dynamics of the car (i.e. with the steer).
• Dproj: projecting the 2D speed on the path is modeled here as a disturbance.
• Djump: the path followed by cars is composed by a polygonal chain. This means that when the car goes from a segment to the next one, there is an interval of time during which the car position is projected on the same exact point of the path. For example, in Figure Djump.png the car goes through the trajectory A-B-C-D. While in the arc B-C, the vehicle position is projected on the path always in the same point P. Similar considerations can be done when a car cuts a corner. This disturbance is modeled with a negative (or positive in case the vehicle cut the corner) step that corrects the position each time the vehicle changes the segment of a path. Essentially, this is a measure of the difference between the length of the nominal path and the lenght of the actualy trajectory that the car follows.
• Dm: measurement error of the cameras.

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.

...

DISTRUBANCES ANALYSIS CONCLUSION

...

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:

• steer controller design;
• camera measurement error correction.

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.

CAR MODEL IDENTIFICATION

...

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.

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.

...

EFFECT OF PWM AND STEER INPUT ON THE SPEED DYNAMICS

...

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.

...

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

Average radius error: 70.57

CAR 3

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.

EFFECT OF THE BATTERY AND THE POWER FILTER CAPACITOR ON THE SPEED DYNAMICS

...