These last two days we mainly worked on tuning the PID coefficients to improve the stability. In this way, we tried to find the best set of PID coefficients so that RoseWheel could raise from an almost horizontal position and recover stability as soon as possible and then maintain this stability over time, even under hard external perturbation. We also worked on improving the coefficients we found for the human driving mode to make the right compromise between smooth-driving and responsiveness.
During this test and improve phase, Alexis advised us to plot the curve of the different values critical for our system. Thus, we plotted over time the value of the angle and the angle rate before and after the kalman filtering and also the command sent to the motor calculated by our PID algorithm.
It made us find 2 problems in our software that explains why at the beginning RoseWheel was so unstable:
The Kalman filter wasn’t configured correctly. We first configured it for the testbench but we forgot to change the time step that was indeed ten times smaller than the correct value. That led to latency and incorrectness in the output of the Kalman filtering. Changing this parameter to its correct value made the system become really more stable.
Even after the Kalman filtering, we noticed that the angular rate was still a lot noisy. This noise caused our gyropod to vibrate a lot when increasing the derivative coefficient Kd in the PID. That is a problem because increasing the derivative coefficient is the only way we have to lower the amplitudes of the oscillations induced by a big proportionnal term Kp in the PID. Thus, we decided to smooth the angular rate value by using a low-pass filter after the Kalman filtering. It’s really a simple filter as it’s only made of 2 coefficients, but according to the plots, it made its work correctly and the angular rate seems to be really smoother than before. But while testing, increasing the derivative coefficient still leads to oscillations and vibrations, so we still have to work on it.
As we obtained satisfying results at making RoseWheel maintain its equilibrium, we started working on other features :
We implemented the safety switch and elaborated a protocol to avoid as much dangerous situations as possible :
At the very beginning, when one presses the power on button, RoseWheel is in self-driving-mode by default. That means it will try to reach its equilibrium position as soon as possible and then remain in this position. We assume that the user isn’t too silly and, for instance, will not try to power on RoseWheel in an almost horizontal position whereas he is facing it. Then, as soon as the safety switch that is located on the base is pressed, that means that someone has its foot on RoseWheel and wants to ride it : RoseWheel switches to its human-drive mode. If suddenly the safety switch is released, RoseWheel checks the angle. If it’s almost 0°, that means that RoseWheel’s speed is almost 0 and the person maybe wants to get out, then calmly RoseWheel switches to its self-driven mode (we still need to implement it). If the angle is under 75°, that could means two things: the person has felt down or the person has temporarily raised its foot. Thus, if the safety switch isn’t pressed within 1 second, RoseWheel makes a special noise, then, if it’s still not switched on within another second, RoseWheel switch to its self-driven mode. Finally, if the angle is greater than 75°, that means that the person has felt down and RoseWheel could represent a menace for people around, motors are disabled.
2) Concerning the obstacle detection, we almost finished to implement the sharps and sonar drivers. As these detectors are really sensitive to the external conditions, we still have to test them and see how they react in the different environnements RoseWheel will evolve in.
3) Concerning the remote control, we almost finished to implement the drivers for the Bluetooth, we made some tests yesderday, but we still need to continue them today and we will talk more about it in the next article.
Today, we tried to set the coefficients of our PID algorithm. In order to do this, we used the Ziegler-Nichols method. The first step is to set the integral and the derivative gains to zero. Then, we increase the value of the proportional gain, to obtain the control loop oscillating constantly. Unfortunatly, we didn’t succeed in obtaining the special value of P (proportional gain). It seemed we had still some bugs on the Kalman filter. Samuel and Axel tested again the good working of the Kalman filter (we had to change some coefficients of the Kalman since we put the card on the Copter), and fixed it.
Samuel made a basic program, in order to change easily the PID coefficients, while the Copter is running : we can now observe the effects of the changes in live.
We worked also on some improvements of the code: for the STM32 and for the Kanade algorithm, to prepare the future work & debug. For example, the UART was configured at 1 000 000 bps, so the STM32 had only 72 instructions to perform between each UART interrupt. Since we only send 150 packets/s (~20 bytes), 57600 bps speed would be fine, and more convenient.
This week we tried to put together our developments and make them interact with each other. It includes sensors communication, kalman filter, control feedback, motors control, CAN communication protocol. We were dogged by bad luck as we realized the heatsink of our mainboard was too big to fit in the Zzaag… A few millimeters required us to order for a new one with a more suitable shape. Fortunately Alexis and Samuel helped us to find a new one. Because of that we were not able to validate the following « feedback control tuning » PSSC for Saturday as planned. Adviced by Samuel we also decided to postpone the « encoders drivers » PSSC as it wasn’t really in our first order priorities.
We tried to make profit of this situation and decided to prepare everything for the first tests. Alexis helped us defining our tests procedure to be as safe as possible for both hardware and people. Motors need special attention as they can be broken with unsuitable commands. Because of the wheels and the weight of the chassis they have a really high inertia. Asking them sharply to reverse their direction of rotation when they are rotating fast can seriously damage them and the H-bridges. If we damage them we wouldn’t be able to order new ones before the end of the course, it would be fatal for our project. Therefore we need at first to make some testing at low speed so as to define the maximum acceptable speed at which we can run the motors before they get damaged. To this extent we need an interpreter running on the mainboard and communicating using RS232 to be able to give motors various commands and define this maximum speed. Then we need to limit our commands to this maximum speed using software limitations. Finally the interpreter should be able to change the feedback control values for our testing to be more flexible. We implemented such an interpreter and made a video. The motors commands in the video are given between 0 and 1000 such as:
ml0 = 0%: max negative speed
ml500 = 50%: stop
ml1000 = 100%: max positive speed
Before trying the Zzaag motors we do some testings on other DC motors with less inertia that we cannot damage as easily as the Zzaag’s ones. Putting all the pieces together we were able to shoot a video which shows the motors reacting appropriately to the inclination of the sensorboard. As for the Kalman filter, we implemented a lighter version than the one of RoseWheel as it works best when tested on the real system. This version was only able to track the gyroscope drift and correct it. As far as we could see during the testing, it did it well. Concerning the PID, we tried to test the version that we are going to use in the RoseWheel but it still needs to be improved during the next tests.
Tomorrow we will be able to work on the Zzaag motors using our safe procedure and the tools developped. We look forward to it as it is a major step further for our project…
Today, I finalized the emission of the commands for the motors, from the Gumstix to the STM32 and their reception (check the validity of the packet and if it’s valid, update the commands sent to the motors).
Then, with Samuel, we added 0MQ exchanges on the Gumstix between Kalman and the PID and between the PID and the UART.
On the Gumstix, the program that handles the UART has two threads. One receives data from the sensors from the STM32 and sends it -thanks to 0MQ- to the program computing Kalman. The other one, receives the commands of the motors from the PID and sends them to the STM32.
We also attached the PCBs on the copter:
We configured Kalman with the new axes and the offset of roll and pitch angles.
Finally, we all tested the PID. The algorithm seems okay, however, we’ll have to set each coefficient properly.
We’re trying to control roll and pitch angles with a low thrust (in order to prevent any accident). Then, we’ll add a control on Z thanks to the Sharp sensor.
The Sharp, the distance sensor, works in 1-5m range and we get values with 3cm precision
Inertial Measurement Unit (gyros, accelerometers and magnetometers return correct values)
Radio Frequency (remote control, see video)
Motors (see video)
Here is a small video where we directly control motors’ speed with remote control:
We now have a reliable communication between our PCB and the Gumstix, thanks to Samuel Tardieu’s help.
We mostly use ZeroMQ to share data between our processes and even between our computers !
Kalman is quite smoothly, works as well on PC as on Gumstix, and we are now optimizing each operation. The actual filter is fast enough for real-time execution (we spend a lot more time to wait after data than to actually process it), but we want the filter to use less processor working time in order to execute other heavy tasks, like Lucas and Kanade (video tracking algorithm).
We do now have a way to get by Wifi only the Kalman’s results in order to display them on a PC using OpenGL as you could have seen it on our previous videos.
We are now up to test for the first time real servomechanism PID on our Copterix !
In relation with our previous post we made some further investigations on the physic underlying the accelerometer measurements.
Theoretically, it’s really easy to extract a tilt angle from an accelerometer. We just have to take the arcsin of the right axis measurement, depending on the frame used, divide by g and we get the tilt angle.
for us : theta = arcsin(Ax/g) where :
theta = tilt angle, Ax = measurement on the x axis, g = gravitationnal constant
This solution works well while the accelerometer doesn’t translate.
If the accelerometer is translating with a non zero acceleration component, extracting a correct angle measurement becomes really tricky. As we don’t only measure the gravitationnal acceleration, but also the translating acceleration, we can’t apply directly the previous equation.
however, even without encoders, it’s mathematically possible to extract the tilt angle. As we have a 3-axis accelerometer, we can extract from the measurements of 2 well chosen axis of the accelerometers a set of 2 equations with 2 unknown variables that are the tilt angle theta and the translating acceleration x and thus it’s possible to resolve this set of equation.
In the frame of the accelerometer we have :
g*sin(theta) + x*cos(theta) = Ax (1)
-g*cos(theta) + x*sin(theta) = Az (2)
where, g is the gravitational constant, theta is the tilt angle, Ai is the acceleration measurement of the accelerometer along the i axis. We gave this set of 2 equations to matlab using the solve function.
We tried each of theses solutions, but none of them worked. We investigate some more about it and the main problem is that, experimentally, the measurement aren’t perfect and are skewed by noise. Thus, even if there is no tilt angle, Az isn’t perfectly equal to g. Furthermore, even if theoretically Ax² + Az² is supposed to be equal to g², experimentally it’s not the case, and that leads to calculate the square root of a negative number, that isn’t a great thing in the real world.
So, our conclusion is that even if this set of 2 solutions is mathematically right, it’s not relevant in the real world and we decided not to use it.
In addition, we lied, equation (1) and (2) aren’t perfectly right, we also have to add the acceleration terms from the rotational movement of the chassis around the wheels axis. That leads to :
Today, we finished and/or improved some of the micro tasks we assigned to each other at the beginning of this week.
We tested a lighter version of the Kalman Filter that will be used in RoseWheel on the testbench we made a few weeks earlier. We used a light version because the full one needed to know the state equations of the system to be accurate. As we didn’t want to loose time trying to find out the state equations of the testbench (even if they are really similar to the ones of RoseWheel), we decided to test a version that was only able to track the drift of the gyroscope and make some minor corrections on the signal from the accelerometer during x axis translation. The light Kalman filter worked well and did what we expected, he tracked the drift and thus corrected the gyroscope noise and also filtered the signal from the accelerometer thanks to the sensor fusion.
During the experiments, we could observe that the noise from both the accelerometer and the gyroscope weren’t as high as we expected, that is a good news. However, even if the drift problem is correct by the Kalman filter, the measurements from the accelerometer are still not accurate during movements because of the noise induced by the other accelerations components. These other acceleration components are only dependents on the angle and its derivatives. We thought of a way to subtract those noisy components at each step by using the estimation of this values at the previous step. We will test it tomorrow on the testbench if we have time to do it.
We also finished the implementation of the CAN drivers and we tested it on our laboratory boards as we didn’t receive yet our mainboard. The tests were satisfaying. But as at the time we performed the tests we hadn’t decided yet the final version of the CAN protocol we were going to use for the communication between our 3 board (mainboard, sensorboard, HI board ), we didn’t use any filters. Hopefully tomorrow we will be able to test the CAN protocol as we will receive our mainboard.
One of our PSSC was to have a functionnal accelerometer for last monday. Despite the fact we didn’t post that day, we did have a working implementation and we were able to compute the inclination angle using the normalized 3D acceleration vector. We did this only with interruptions and without any kind of polling. The code was actually already working at our last demo. But in the meantime we’ve made some improvements that deserve to be described here. We used to detect rising edge of the accelero’s RDY pin to update the values. The rate at which values are calculated depends on the decimation factor chosen. According to the datasheet it spans from 40 Hz to 2560 Hz (with only 4 different values) and because of the fact we need 200 values per second we chose a decimation factor of 32 that enabled us to refresh values at a rate of 400Hz (closest value). Using interruptions at rising edge of RDY pin and then requesting the values from the device using SPI bus protocol was a bit demanding for the processor at this rate. We have therefore chosen not to use the RDY pin to be able to choose more precisely our rate of update. Some changes have also been made on the number of bytes used to represent data and a global coordinate system has been defined for both accelerometer and gyroscope.
Video of our sensorboard displaying the inclination angle using a color led.
Today we also assembled the mechanics of the Zzaag project. We could even try to ride it using the board shipped with. Here is a picture of what it looks like:
Mechanics of th Zzagg project
We had the time to begin the drivers for our hypothetical encoders (we are not sure to be able to set them on the mechanics) and we will be able to finish them tomorrow to be able to validate another PSSC.
At this point we have several pieces of software that wait to be integrated in our boards softs. The integration of the sensorboard has been started. In the main task an infinite loop wakes up every 5ms (200Hz). We retrieve the values of the accelerometer and the gyroscope that are updated separately in independant tasks. Then we need to give these values to the kalman filter for the final angle to be computed getting rid of the noise. But the kalman filter needs to estimate the next state before correcting the given values. We then need to receive the current values of the commands applied to the motors sent on the CAN bus by the main board. Finally we broadcast the computed angle and angular velocity on the CAN bus.
Today, we have resolved the problem concerning the communication between the Gumstix and the STM32. Our initialization of the serial port wasn’t good because we didn’t set raw mode. Now we have a good communication at 1MHz between the two boards and we’re able to send around 300 packets per second (21 bytes per packet) . We are able to transmit all the data from sensors to the Gumstix with a precise protocol previously explained. We are also able to receive data from the Gumstix in order to command the motors later.
For the time being, we gather information from gyroscopes, accelerometers and magnetometers. These sensors are enough to use the Kalman filter. Besides, this filter has a little problem when we are doing brutal movements. A first idea to resolve this problem is to optimize our filter to be faster and to take into account big sensors variations.
After having optimized the Kalman filter (for now, we just split the 3D render process and the Kalman process, and we use ZMQ to exchange data), it became more efficient.
Tomorrow, we will add a Sharp to measure our altitude because we don’t use a pressure sensor anymore. We will also begin the communication with the motors. We will have to parameter the Kalman filter in good conditions, and finally optimize it deeply before putting it on Gumstix.
Voici un compte rendu des avancées que nous avons fait pour la démonstration de vendredi et pendant le weekend :
Côté accéléromètre, nous avons réussi à communiquer proprement avec accéléromètre en utilisant une interruption externe liée au signal RDY ; cela nous a permis plusieurs démonstrations intéressantes : contrôle d’une LED suivant l’angle, envoi des valeurs mesurées par le port série, affichage de l’angle mesurée avec Matlab.
Pour le gyroscope, nous avons eu plus de difficultés, notamment pour bien gérer l’implémentation du protocole I2C dans le STM32. Après quelques heures de debug, nous sommes arrivés à une implémentation minimale du driver pour la démonstration, les données lues étant transmises par le port série et affichées dans le même graphique que celles en provenance de l’accéléromètre ; cependant, le capteur parfois s’arrêtait sans explication.
Dans un premier temps, nous avions attribué ce problème à des micro-coupures d’alimentation. Néanmoins, nous ne sommes toujours pas satisfaits de cette explication, puisque une telle fragilité n’est pas désirable pour une partie si vitale de notre projet ; alors, nous avons travaillé sur le code pendant le weekend pour essayer de trouver la source des instabilités, mais nous n’avons pas encore trouvé la réponse. À suivre donc…
Filtre de Kalman
Nous avons implémenté en C notre filtre de Kalman dans la journée du vendredi. Afin de procéder aux tests « définitifs », nous attendons d’avoir des drivers plus fiables pour le gyroscope et la partie mécanique monté, étant donné qu’il est indispensable de tester le filtre dans les conditions prévues sur le système pour lequel il a été conçu.
Entre-temps, nous envisageons d’utiliser une version un peu modifié qui ne traquerait que la dérive du gyroscope sur notre banc de test. Pendant la démonstration, nous avons montré nos performances en simulation, dont nous avions parlé dans notre dernier post.
Banc de test
Nous avons également montré au jury le fonctionnement de notre banc de test, notamment le graphique de variation d’angle et de vitesse angulaire, que nous avons détaillé dans des posts précédents.
Nous avons aussi avancé sur le bus CAN, et nous pensons à le tester demain avec les drivers que nous avons écrit ce week-end.
Nous commençons à implémenter les des moteurs et l’utilisation de PWM pour contrôler les ponts en H.
Sur notre PCB, nous utilisons des « pilotes de demi ponts en H » (IRS2184SPBF) qui permettent (entre autres) d’éviter les court circuits.
Dans le schéma suivant, ces composants se brancheraient à droite et à gauche de façon à ne jamais avoir A et B à la même valeur :
Pour pouvoir changer le sens de rotation, on pense utiliser la broche enable disponible sur le « pilote » (IRS2184SPBF).
Le chronogramme suivant illustre ce que l’on croit nécessaire au moment de la transition d’un sens de rotation à l’autre :
De façon à éviter d’entendre le signal de contrôle, on pense utiliser une fréquence de l’ordre de 20KHz mais on ne sait pas encore si cela ne sera pas trop élevé pour « limiter les pertes lors de la commutation des transistors du pont en H ».
Un certain nombre d’incertitudes persistent donc toute suggestion serait la bienvenue !
Planning pour la semaine – « micro-tâches »
Envisageant pouvoir asservir correctement le segway avant dimanche prochain, nous nous sommes attribués les « micro-tâches » suivantes pour la suite :
Drivers carte capteurs
- I2C / Gyroscope -> João12/04
- SPI / Accéléromètre -> Clément11/04
Drivers carte principale
- Drivers Ponts en H -> Cédric13/04
- Contrôle moteurs -> Cédric14/04
- Drivers CAN -> Florian11/04
- Tests Drivers CAN -> Florian11/04
- Protocole CAN -> João13/04
- Tests protocole CAN -> João14/04
- Drivers encodeurs -> Clément14/04
Tests filtre de Kalman
- Intégration filtre TestBench -> Florian13/04
- Réglage filtre -> Florian17/04
Intégration logicielle carte capteurs
- Implémentation asservissement en C -> Clément12/04