Page History

...

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.

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.

1. 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.
2. 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.
3. 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.
4. 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.
5. 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.
6. 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.

...

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. In short, the filter detects when a leap occurs and use the model heading instead of using the measurement when that happens. The algorithm is the following:

1. recompute measured_heading // see discussion later to understand why this must be done
2. compute model_heading = prev_heading + tan(prev_steer) * prev_speed * 0.1 / wheelbase // previous_steer is in degrees, 0.1 is the car clock in seconds
3. compute measured_Dpos = sqrt((prev_x1 - curr_x1)^2 + (prev_x2 - curr_x2)^2)
4. compute model_Dpos = prev_speed * 0.1
5. if |measure_heading - model_heading| > 30
   1. keep model_heading
6. else if |measure_heading - model_heading| > 20 and |measure_Dpos - model_Dpos| > 0.35 * model_Dpos // this is the uncertain case
   1. keep model_heading
7. else
   1. keep measured_heading

We recompute the measured heading because the CPS filters it and, when the jump is very small, the measured heading becomes wrong but not enough wrong to discard it. For consistency reasons then we keep the newly computed measured heading instead of the CPS one. 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

...

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:

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

CAR MODEL

...

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)

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

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.

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.

...

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

• 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 experiment1.png. The speed always reaches a peak and then decrease 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 experiment2.png. From starting voltage 16.9V to 16.5V the speed has basically the same trend. The steady state value and the constant time becomes 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 experiment 3.png. 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 experiment 5.png. 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 experiment 6.png. The car speed has the same peak as the previous run.

...

CAR MODEL

...

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)

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

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.

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.

...

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

• 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 experiment1.png. The speed always reaches a peak and then decrease 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 experiment2.png. From starting voltage 16.9V to 16.5V the speed has basically the same trend. The steady state value and the constant time becomes 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 experiment 3.png. 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 experiment 5.png. 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 experiment 6.png. The car speed has the same peak as the previous run.

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.

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.

1. 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.
2. 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.
3. 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.
4. 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.
5. 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.
6. 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.

...

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. In short, the filter detects when a leap occurs and use the model heading instead of using the measurement when that happens. The algorithm is the following:

1. recompute measured_heading // see discussion later to understand why this must be done
2. compute model_heading = prev_heading + tan(prev_steer) * prev_speed * 0.1 / wheelbase // previous_steer is in degrees, 0.1 is the car clock in seconds
3. compute measured_Dpos = sqrt((prev_x1 - curr_x1)^2 + (prev_x2 - curr_x2)^2)
4. compute model_Dpos = prev_speed * 0.1
5. if |measure_heading - model_heading| > 30
   1. keep model_heading
6. else if |measure_heading - model_heading| > 20 and |measure_Dpos - model_Dpos| > 0.35 * model_Dpos // this is the uncertain case
   1. keep model_heading
7. else
   1. keep measured_heading

We recompute the measured heading because the CPS filters it and, when the jump is very small, the measured heading becomes wrong but not enough wrong to discard it. For consistency reasons then we keep the newly computed measured heading instead of the CPS one. 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

...

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:

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

DOCUMENTATION EXPANSION

These are likely to be expanded and moved on the main wiki page at some point.

...