...
- determine and limit the disturbances which make the prediction uncertain to a degree that make the experiment unfeasible;
- identify a model of the system that is suitable for the long term prediction that the algorithm requires.
SUMMARY OF CHANGES
...
SOFTWARE
...
- 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.
- ca2: 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 screwed to the metal support. Now every time they move, the whole calibration procedure must be done again. An extrinsic calibration (at least) should probably be performed again after this job.
- The two chargers close to the whiteboard seem to have some wiring problem. Sometimes the led is green while cars are connected (i.e. they're done charging) but it turns red when you move the wires.
- Sometimes the connection between computers and cars is lost. There are 2 main cause.
- There are 2 critical functions in the CPS code in main.cpp that occasionally takes up to 300-400ms in computer 0. This does not happen (or happens extremely rarely) with computers 1 and 2. I think the fault may simply be the computer's hardware because computer 0 is the slowest among the three. The two critical functions are flycaptureGrabImage2() and cvWaitKey(). The CPS currently prints a warning when one of this two functions takes more than 0.1s (the car's clock). I changed the CPS so that computer 1 now sends the data to cars so that the connection is always active, still this kind of delay makes cars tracked by computer 0 to receive out-of-date information.
- Sometimes car 2 becomes unreachable to ping tests while moving. This causes the time between two clock cycles to reach even 1-2 seconds. I never found the same problem with car 1 and 3. Most of the times this happens, there is no timeout of the recvfrom() function which receives the data from computers. This suggests that the problem resides in the on-board computer more than in the connection. The reason could be originated by both hardware (the motherboard of car 2 is an older and slower model with respect to car 1 and 3) and software (out-of-date operating system or some background program that takes too much resources).
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
...
- 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.
...
TROUBLESHOOTING
...
- motherboard: substitute power supply unit (PSU) when there no signal from motherboard (connect to the screen to test it).
- car: ca2 "Host-Stem Link: DOWN" message: try to reset baudrate console and set it back. Otherwise hardware reset. Suggestions about how to perform the hardware reset procedure (attach resistor to serial cables). Differentiate between reset brainstem (tea and reflex files are not touched) and configuring a brand new card.
- car: car is slower than others. Check that h-bridge connections to power and motor are solid.
- car: encoder no signal, check serial cable connected in the proper way (link to h-bridge and encoder datasheet).
- car: motor doesn't run. Check h-bridge connection to power and to motor or reset brainstem.
- car: steer is biased. Cannot be corrected completely mechanically. Set offset in RoboCar_Lib.tea.
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 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)
...
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.
STEER CONTROLLER
...
LINEAR MODEL
...
The linear model used for the steer controller design is described by the block diagram in Figure lat_block.jpg. Symbols represent
- U(s): desired lateral distance from the path which is always 0.
- C(s) = c / s: constant compensation term of the controller computed by using the model steer input-curvature radius.
- M(s) = m / s: constant term that accounts for the error of the model used to determine the compensation.
- D(s) = d / s^2: disturbance term that models the curve in the path.
- w: the wheelbase.
- alpha: the steer factor.
- delta: steer input in (-100, 100).
- X(s): controlled variable, the lateral distance from the path.
The Laplace transform of the system is
X(s) = (-v0 * d * w + alpha * v0^2 * c + alpha * v0^2 * m) / (s * (w * s^2 + alpha * v0^2 * k2 * s + alpha * v0^2 * k1))
If we set c to cancel the effect of d, then the error is caused by m. By applying the final value theorem and cancel the effect of d with c, one obtains
lim x(t) = m / k2 for t -> inf
Thus the final error depends only on k2 as reasonable to expect. Recalling that m must be a steer input, it is easy to compute the maximum value of m. By looking at the maximum error in the validation data for the #STEER INPUT - CURVATURE RADIUS RELATIONSHIP I obtained m = 13.3045. For the derivation of the model check formulae.pdf. The model does not take into account the measurements disturbances (leaps included) and it considers negligible disturbances associated with the actuator, projection on the path and slip angles.
...
CONTROLLER CALIBRATION
...
The controller used is a PD in the form
delta = k1 * x' + k2 * x = k1 * (beta - gamma) + k2 * x
where beta and gamma are respectively the direction of the car and of the path in radians. The system has a pole in 0 and a pair of complex poles for
|k1| < +- sqrt(4 * w * k2 / (alpha * v0^2)
The pair of poles are
p = - alpha * v0^2 * k1 / 2w +- i * sqrt(4w * alpha * v0^2 * k_2 - alpha^2 * v_0^4 * k1^2) / 2w
We want the final error ef, the settling time ts and the frequency of oscillation wd to be low or, in other words, we want to keep low the quantities ts = -4.6 / Re(p) and wd = Im(p). With k1 = k2 = 0.3 and v0 = 800 we obtain ef = 44.34mm, ts = 3.37s and wd = 0.9314.
...
RESULTS
...
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 648 750.07285
- 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 left.
DOCUMENTATION EXPANSION
These are likely to be expanded and moved on the main wiki page at some point.
...
TROUBLESHOOTING
...
- CPS: one must pay attention whether there is some overlapping area between two adjacent cameras.
- motherboard: substitute power supply unit (PSU) when there no signal from motherboard (connect to the screen to test it).
- car: ca2 "Host-Stem Link: DOWN" message: try to reset baudrate console and set it back. Otherwise hardware reset. Suggestions about how to perform the hardware reset procedure (attach resistor to serial cables). Differentiate between reset brainstem (tea and reflex files are not touched) and configuring a brand new card.
- car: car is slower than others. Check that h-bridge connections to power and motor are solid. Check that all wheels spin fine and eventually clean and grease the faulty bearing.
- car: encoder no signal, check serial cable connected in the proper way (link to h-bridge and encoder datasheet).
- car: motor doesn't run. Check h-bridge connection to power and to motor or reset brainstem.
- car: steer is biased. Cannot be corrected completely mechanically. Set offset in RoboCar_Lib.tea.
...
TO DOCUMENT
...
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.
STEER CONTROLLER
...
LINEAR MODEL
...
The linear model used for the steer controller design is described by the block diagram in Figure lat_block.jpg. Symbols represent
- U(s): desired lateral distance from the path which is always 0.
- C(s) = c / s: constant compensation term of the controller computed by using the model steer input-curvature radius.
- M(s) = m / s: constant term that accounts for the error of the model used to determine the compensation.
- D(s) = d / s^2: disturbance term that models the curve in the path.
- w: the wheelbase.
- alpha: the steer factor.
- delta: steer input in (-100, 100).
- X(s): controlled variable, the lateral distance from the path.
The Laplace transform of the system is
X(s) = (-v0 * d * w + alpha * v0^2 * c + alpha * v0^2 * m) / (s * (w * s^2 + alpha * v0^2 * k2 * s + alpha * v0^2 * k1))
If we set c to cancel the effect of d, then the error is caused by m. By applying the final value theorem and cancel the effect of d with c, one obtains
lim x(t) = m / k2 for t -> inf
Thus the final error depends only on k2 as reasonable to expect. Recalling that m must be a steer input, it is easy to compute the maximum value of m. By looking at the maximum error in the validation data for the #STEER INPUT - CURVATURE RADIUS RELATIONSHIP I obtained m = 13.3045. For the derivation of the model check formulae.pdf. The model does not take into account the measurements disturbances (leaps included) and it considers negligible disturbances associated with the actuator, projection on the path and slip angles.
...
CONTROLLER CALIBRATION
...
The controller used is a PD in the form
delta = k1 * x' + k2 * x = k1 * (beta - gamma) + k2 * x
where beta and gamma are respectively the direction of the car and of the path in radians. The system has a pole in 0 and a pair of complex poles for
|k1| < +- sqrt(4 * w * k2 / (alpha * v0^2)
The pair of poles are
p = - alpha * v0^2 * k1 / 2w +- i * sqrt(4w * alpha * v0^2 * k_2 - alpha^2 * v_0^4 * k1^2) / 2w
We want the final error ef, the settling time ts and the frequency of oscillation wd to be low or, in other words, we want to keep low the quantities ts = -4.6 / Re(p) and wd = Im(p). With k1 = k2 = 0.3 and v0 = 800 we obtain ef = 44.34mm, ts = 3.37s and wd = 0.9314.
...
RESULTS
...
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 648 750.07285
- 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 left
...
.