Page History

...

• Implemented a simulator in C for Linux. This is integrated with the code used on cars (ca2). It simulates the cars dynamics and the behavior of the Camera Positioning Systems (CPS). Besides being very useful to verify the system model, it makes easy to test changes to code and the controller before applying those changes to cars.
• Implemented a script to visually debug cars behavior. It works both with the simulator and test-bed experiments data.
• ca2: grouped utility functions in a separate folder that is shared with the simulator. This makes the code more modular.
• ca2: partially implemented a Kalman filter in the attempt to remove part of the measurement noise of cameras. The model must be refined for it to prove its real effectiveness.
• CPSca2: fixed a bug that affected the initial target dectection for cars that were not tracked by computer 0.
• CPS: now compute and send also the 2D speed of the cars. This is likely to be needed for the predictor in the future. See also section #CPS MEASURED 2D SPEED.
• CPS: implemented linear error correction for the computation of camera position. This visibly improves the path following of cars. See section #CAMERA MEASUREMENT ERROR CORRECTION.

Note about CPS usage. Since CPS now compute and send the 2D speed to cars, AndreaCPS is not compatible to previous ca2 versions. Viceversa my ca2 is not compatible with previous version 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. This 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.

...

CURRENT LAB PROBLEMS

...

• Cameras should be screwd to the metal support. Now everytime they move, the calibration must be done again. An extrinsic calibration (at least) should probably be performed again after this job.
• Sometimes the two chargers close to the whiteboard display the green light when cars are connected (i.e. they're done charging) but when I move the wires the light turns red.

...

NEXT STEPS

...

Steer control

• Steer controller designing and testing.

Predictor design

• Model identification of motor_input -> speed.
• Model identification of path-dependent disturbances (steer, slope and projection) on speed.
• Quantification of non-predictable path-dependent disturbances.
• Predictor implementation and testing.

Experiment

• Debugging of the collision avoidance algorithm with the new predictor on the simulator.
• Setting-up and testing of the experiment with only autonomous vehicles.
• Setting-up and testing of the final experiment with also human-driven vehicles.

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

...

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.

• implemented a new target detection algorithm that allow the car to work with the path with lots of points.
• ca2: 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 compensator has been added.
• ca2: modified paths to add compensator directives (See #COMPENSATOR DIRECTIVES).
• ca2: implemented a new filter for the heading measure to cancel the effect of leaps (See #HEADING FILTERING).
• ca2: set the limit of steer input to (-100,100) since the curvature radius saturates above it.
• CPS: fixed a bug that affected the initial target dectection for cars that were not tracked by computer 0.
• CPS: now compute and send also the 2D speed of the cars. This is likely to be needed for the predictor in the future. See also section #CPS MEASURED 2D SPEED.
• CPS: implemented linear error correction for the computation of camera position. This visibly improves the path following of cars. See section #CAMERA MEASUREMENT ERROR CORRECTION.

Note about CPS usage. Since CPS now compute and send the 2D speed to cars, AndreaCPS is not compatible to previous ca2 versions. Viceversa my ca2 is not compatible with previous version 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. This 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.

...

CURRENT LAB PROBLEMS

...

• Cameras should be screwd to the metal support. Now everytime they move, the calibration must be done again. An extrinsic calibration (at least) should probably be performed again after this job.
• Sometimes the two chargers close to the whiteboard display the green light when cars are connected (i.e. they're done charging) but when I move the wires the light turns red.

...

NEXT STEPS

...

Steer control

• Steer controller designing and testing.

Predictor design

• Model identification of motor_input -> speed.
• Model identification of path-dependent disturbances (steer, slope and projection) on speed.
• Quantification of non-predictable path-dependent disturbances.
• Predictor implementation and testing.

Experiment

• Debugging of the collision avoidance algorithm with the new predictor on the simulator.
• Setting-up and testing of the experiment with only autonomous vehicles.
• Setting-up and testing of the final experiment with also human-driven vehicles.

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

...

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 The steer factor was computed in order to minimize the average error which lead to the value c = 0.2116466582.

...

p = - alpha * v0^2 * k1 / 2w +- i * sqrt(4w * alpha * v0^2 * k_2 - alpha^2 * v_0^4 * k1^2) / 2w

We want the final error ef, the settling time ts and the frequency of oscillation wd to be low or, in other words, we want to keep low the quantities ts = -4.6 / Re(p) and wd = Im(p). With k1 = k2 = 0.3 and v0 = 800 we obtain ef = 44.34mm, ts = 3.37s and wd = 0.9314.

...

RESULTS

...

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.

alpha^2 * v_0^4 * k1^2) / 2w

We want the final error ef, the settling time ts and the frequency of oscillation wd to be low or, in other words, we want to keep low the quantities ts = -4.6 / Re(p) and wd = Im(p). With k1 = k2 = 0.3 and v0 = 800 we obtain ef = 44.34mm, ts = 3.37s and wd = 0.9314.

...

RESULTS

...

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.

...

COMPENSATOR DIRECTIVES

...

In order to apply the compensation, the controller must know the curvature radius of the path. You can specify it by adding to the directive to the path definition file. For example, in fig8CL.txt the first two lines are now

1. 1 648 750.07285
2. 651 1298 -750.07285

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

DOCUMENTATION EXPANSION

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

...