Remember my plan to make a ball balancing robot? Last year I set myself the goal to make a ball balancing robot. I even build the robot. Since then I wondered off my original goal and made a guardbot, Koios, from this platform. Now I am having another shot at making a balancing robot.
Programming a balancing robot is easy in theory. You just need a sensor that tells you how much the robot is tilted, most often people use a gyro for this. I use my IMU for this, so that I do not suffer from gyro drift. The tilt angle is then feeded to a PID-controller that transformes tilt to motor speed. The hard part is to tune the PID controller, it has to translate tilt into just the right amount of motor speed, too little and the robot falls of the ball, too much and the robot goes over the top and falls of on the other side of the ball. Falling of the ball damages the robot. So I had a problem, how to tune the PID controller without damaging the robot?
To be able to tune the PID-controller without damaging the robot I made a special platform. It is a large disk with a small pole in the middle pointing down Due to the pole the disk will always be tilted when lying on the ground, only when it balances perfectly on the pole it is level. Therefore this disk can be used to tune the controller. The robot can ride off the disk, but it doesn’t fall then, it just comes on the floor with one or two wheels.
When I tested this setup I discovered that the disk whas too smooth, the wheels didn’t have enough grip and slipped. To increase the friction I coated the surface of the disk with sillicon rubber, It is the light blue surface you see in the picture. Now I have a very “slick” surface.I only hope it lasts under the forces the NXT motors generate.But for the moment this problem is solved.
But there are other problems. One is the fact that these holonomic wheels make the robot vibrate, this affects the IMU filter, there is still some drift although it stays within certain limits. I do have prototype rotacaster wheels. The manufacturer told me that the production wheels are more round and generate less vibrations. If you are ever going to by these wheels, and they are a lot of fun, I advice you to take the black ones. They have the best grip. Anyway, I will have to tune the IMU as well.
Tuning PID controllers is difficult and very, very time consuming. There is some theory around tuning PID controllers but in the end it is mostly trial and error. Everytime I want to try a new set of parameters I’ll have to modify the program, download it to the brick, run the program and evaluate the results by watching the robot. It is hard to understand what goes wrong when you see the robot ride of the disk and make a run for the door to the staircase.
But not anymore. Kirk, one of the developers of Lejos made a very nice program that allows you to tune a running PID controller during over bluetooth. The tool is still under development so you won’t find it in Lejos 0.9.1 yet. This program is an add-on to the charting logger I often use to evaluate internals of the robot on the PC. So basicly, this program shows me what is going on in my robot and allows me to modify PID parameters on the fly. I think this is a great tool. Below is a screen shot of it.
So, now I have the robot, a test platform and a efficient tuning tool. That must mean immediate succes! Well, to be honest I don´t think so. I´m still not sure if I can get this robot to work as there are problem with weight and inertia as well. The robot weigths 998 grams. This is quite heavy, even for three powerful NXT motors. The robot is quite stiff, but there it still bends a bit under weight. This affects the IMU sensor. And I´m working on other projects as well. So in the end I think there is a bigger chance to fail than to succeed.
I have included a compass into my INS filter. It now is drift-free and accurate over all three axis. It also is very robust to magnetic disturbances. The compass itself is not. It gives me great pleasure to see the first results.
Tuning the filter is difficult though. I have used the NXJChartingLogger that will be part of Lejos 0.9.1 a lot. The filter provides all kinds of output that is displayed real time on the pc. Different parameters are logically grouped into different graphs. The graphs provide information on the state of the filter and thus help in tuning.
I have also included a method to introduce errors in the sensor data. This enables me to examine the performance of the filter.
As a last thing I have made the filter calculate the initial state of the filter. With this the filter settles accurately (<.5degrees) within half a second.
The performance dropped to 97 Hertz with three sensors attached. But I haven't optimized the compass code yet.
In later posts I'll demonstrate the performance of the filter.
By request I will publish the Lejos software for the dIMU sensor.
The software contains of drivers for the Dexter Industries IMU sensor, programs to calibrate the sensor and the IMU filter that I wrote about on this blog before. The filter can be used for other IMU sensors as well. Also included is the sample program that utilizes this filter. It is the very same program I used in the video’s published in a previous post. There is no formal support and no warranty. If you are going to use it you will have to rely on your own wit, the javadoc and on this blog.
Download and install
You can download the software from this link. After downloading you will have to extract it keeping the directory structure intact. Make sure yor compiler can find the code.
Using the IMU drivers
The dIMU consists of two sensors, a gyroscope and a accelerometer, each has it’s own driver. The gyroscope driver is called L3G4200D and the driver for the accelerometer is MMA7455L. (I use the technical names of the sensors a driver names.) On top of these drivers I made user interfaces that allowes you to examine, configure and calibrate the sensors. The user interfaces are called L3G4200D_E and MMA7455L_E respectively. You can use these programs as driver as well. It gives your programs access to the additional functionality but it needs the NXT screen and might result in larger programs. There is a sample program included that gives access to the user interfaces, it is called testDIMU.
This is how you enable the drivers in your code,
SensorPort.S4.i2cEnable(I2CPort.HIGH_SPEED);
MMA7455L accel = new MMA7455L(SensorPort.S4);
L3G4200D gyro = new L3G4200D(SensorPort.S4);
The first line instructs Lejos to use high speed I2C. The sensors support this.
This is how you get data (rate, acceleration and tilt) from the sensors.
float[] rate = new float[3];
gyro.fetchAllRate(rate);
float[] accel = new float[3];
accel.fetchAllAccel(accel);
float[] tilt = new float[3];
accel.fetchAllTilt(tilt);
As you can see the drivers return data from all three axis is one call. If you just need data from one axis you can get it from the arrays. The order is X, Y, Z. The gyro returns degrees per second by default. The accelerometer returns Milli-G for acceleration and Degrees for tilt, also by default. If you want to use other units in your programs you can change the default values like this.
This changes the default units to radians per second, meter per second^2 and radians respectively. Other units are available, check the javaDoc. The default unit can be overridden per data request by specifying the desired unit as second parameter in the FetchAll… methods.
Configuring the sensors
The gyro can be configured for dynamic range and sample rate using the setRange and setSampleRate methods. As a rule one should select the lowest value that your application allows for. This gives data of best possible quality.
The accelerometer cannot be configured. I found this of little use.
Calibrating the sensors
The gyro of my dIMU doesn’t really need calibrating. however there is the possibility to do so. Calibration is started b calling gyro.calculateOffset(). During calibration the sensor should remain motionless. Calibration settings of the gyro are not stored, so they are lost when your program terminates. (Storing calibration settings of gyro sensors is of no use as the calibration values depend on environmental factors and are not stable over time.)
The accelerometer needs calibration. The user interface driver provides functionality to calibrate the sensor and to store the calibration settings. The (base) driver looks for stored calibration settings upon initialization and loads these automatically when they are available. Calibration settings of the accelerometer are stable over time so you’ll need to do this just once. Each of the three axes has to be calibrated separately. To calibrate an axis one has to point it straight up first and hold still while the calibration routine collects data samples. Then the axis has to be pointed straight down and held still for some time. Follow the on screen instructions and do not forget to save the settings after calibration.
Using the IMU filter
The IMU filter can be used with any three-axis gyro and any three-axis accelerometer as long as their drivers implement the RataData and TiltData interfaces. This is how you initialise the filter
NLCFilter attitude = new NLCFilter(gyro, accel);
attitude.start();
The two parameters to the filter constructor are the gyro driver and accelerometer driver. One could leave out the accelerometer, the filter will work but values will drift over time. The second line of code starts the filter. The filter needs 2 to 5 seconds to settle at start up, therefore you need to keep the sensor motionless and more or less level for a few seconds. You can find out if the filter is ready settling with the Initializing() method.
The IMU filter keeps track of the attitude, or pose, of your sensor/robot. You can query the roll, pitch and yaw angles this way
float[] tilt = new float[3];
attitude.fetchAllTilt(tilt);
By default these methods return angles in radians. You can change this by setting a different unit with the setTiltUnit() method.
You can also use the filter to transform data from a body frame to a world frame. This is useful if another sensor returns data that needs to be corrected for the robots current attitude. the next example transforms a distance returned by a UltraSonic sensor to world coordinates. The example assumes the us and IMU sensors are aligned and that the US sensor measures along the X-axis.
Matrix usBody=new Matrix(1,3);
Matrix usWorld=null;
us = new UltrasonicSensor(SensorPort.S1);
usBody.set(0,0,us.getDistance());
usWorld=attitude.transformToWorld(usBody);
The matrix usWorld now contains the distance from sensor to the detected object over the X, Y and Z axis.
Configuring and tuning the IMU filter
By default the IMU filter updates as often as possible. It’s update frequency is over 100 Hertz. To free computer resources one can lower the update frequency by using the setFrequency() method. The filter will try to match the specified frequency. A parameter value of 0 will run the filter at maximum speed.
The filter can be tuned to increase the quality of its output. I advise you not to tune the filter until you are familiar with it and understand its inner workings. Tuning consists of altering the P and I values of it’s internal PI controller. The I value takes care of gyro drift cancellation and the P value controls how fast attitude is corrected by use of tilt data from the accelerometer. The values can be modified by using the setKp() and setKi() methods.
There are two ways the filter can assist you in tuning. It keeps track of the integral of absolute errors, or absI. This is a measure of the total error of the filter over time. The smaller the error (given a fixed period) the better the filter performs. /* The filter also allows you to send data over bluetooth to the pc for examination. For this one has to use the NXTChartingLogger on the pc that is part of the Lejos distribution. You instruct the filter to send its state to the pc by supplying a NXTDataLogger object with the setDataLogger() method. */
Running the demo program
The demo program is called testIMU. At startup of this program the sensor must be horizontal and motionless. The sensor is assumed to be aligned ith the NXT brick with the sensor plug facing to the same direction as the sensor ports. Once you see the wire frame you can start moving the sensor.The demo has four display modes:
Wire frame. Here it shows a wire frame of the sensor on the NXT screen
Rotation matrix. The screen will show the content of the rotation matrix. In this matrix the current attitude is stored. The matrix is also used to convert body coordinates to world coordinates by matrix multiplication..
Roll, Pitch, Yaw. The screen shows the Roll, Pitch, Yaw angles of the sensor.
Update speed. The screen shows the actual update speed of the filter.
You can browse through the display modes by using the arrow keys. The enter key resets the filter. The clip below shows the demo program in action in wire frame mode.
In this post I will discuss my genetic algorithm to tune a PID controller.
PID controllers are often used in robotics, especially to drive motors. In my case a want a PID controller that turns Sidbot (see previous posts for this robot) to desired headingd in the quickest possible way. The job of the PID controller is to specify the amount of motor power that is suited to turn the robot. One can imagine that the amount of power will be large when the robot is way off the desired heading. If the robot is exactly on the desired heading the motor must remain motionless, the amount of power must be 0. When the robot is close to the desired heading then the motors must receive little power, otherwise the robot could overshoot the desired heading without stopping. If the robot overshoots the power must be negative.
The PID controller works like this. It takes the difference between current heading and desired heading and calles this the error. The error is the basis for calculating the amount of power that needs to got to the motors. It not only takes the current error (Proportional), it also takes into account how big the error used to be (Integral) and how big it is likely to be (Derivative). It weights these three inputs with PID values and sums them. This is the amount of power that is send to the motors.
Finding good values for P, I and D can be a difficult thing. You might check wikipedia to read more about some of the methods that are used. I wanted to use a smart kind of trial and error method using the same principle that also drive evolution: Survival of the fittest. I took ten randomly chosen values for PID and tested them all. The best sets survived, the worst 50%were discarded. As there were only 5 sets left I copied these these to have one copy each. By chance these copies were a little bit different from their parent. Then I tested these ten sets again. This process was repeated over and over until a satisfactory set of parameters was found.
Below is a graph that’s shows the results of my experiment. It actually shows the PID values for the best set each time I tested the 10 sets. It also shows how much time it took to make a turn of 60 degrees. Remember my goal was to go to the end heading as quick as possible, so the lower this light blue line, the better the parameters.
So what does this graph tell you? In the last test the robot reached it’s goal in about 0.4 seconds. I don’t think it can do much faster due to physical limitations. The graph also shows that a fairly good set of parameters was found quickly, just after a few tests the turn was made within 4.2 seconds. (I must admit that I had some idea for a suitable range for the random generator that selected the initial PID parameters). It then took some time to find a set that was even better. After that changes to the PID values got smaller and the gain was also smaller. The optimum PID values were reached.
I might conclude that my experiment was successful. But the task of the PID controller was a simple one.
For those still here, these are the resulting values P=210.91 I=56.75 D=2 hI=0.65. hI is the fading speed of the Integral.
As you might know from a previous post I want to put Sidbot on a ball. It must be able to balance on top of it without falling of. If it knows how to do that it must also learn to wander around. There are a number of things I must accomplish to reach this ultimate goal. Here I’ll describe what has been done and what must be done. In later posts I might zoom in on some of the tasks.
The platform
Sidbot has been built. You can see photo’s of it on previous posts. I am able to change the angle of the wheels, this enables me to adapt Sidbot to different ball sizes. I also made a spreadsheet that calculates the exact ball size needed for a given setup.
The ball
I bought a fitness ball with a diameter of 65 cm. My guess is that the bigger the ball the easier it is to balance it. Bigger balls will also be heavier and more difficult to get in motion, having more inertia. The ball also has a flat sticky surface that give grip to the wheels. It is a bit bouncy though.
The sensor
To balance on a ball Sidbot must know what is up and when it is tilting over. A lot of NXT based balancing bots measure the distance to the ground for tilt information. This technique cannot be used for Sidbot, it is a long way from the ground and the ball is in the way when looking at the ground. Therefore I will use gyro sensors to get tilt information.
I need at least two gyro’s, one to detect tilt in the y direction
(rear to front) and one to detect tilt in the x direction (left to
right). I also want to measure the rate of turn around the z axis (down to up) . This is not needed for balancing but for keeping a fixed heading when it is moving. Currently I just have the Hitechnic gyro sensor. This measures rate of turn in just one direction. Instead of buying two more of these I will built my own sensor that takes care of all three axes. This occupies less space and sensor ports, has a higher sampling rate, might be more accurate and also includes a 3 axis accelerometer. It is the IMU digital combo board from Sparkfun.
By chance this sensor fits exactly into the slide on the inside of the lego sensors. I will sacrifice my (never used) sound sensor to house it. Thus far my attempts to use this sensor with the NXT have been unsuccessful. The problem is the different voltage level of the sensor (3.3V) and the odd pull ups required by the NXT. I also lack experience with electronics. But I won’t give up that easy.
The PID controller
To balance on the ball Sidbot needs to adjust its position constantly. To do so sensory information from the gyro’s has to be translated into motor commands. For this I will use a PID controller. PID controllers are
very easy to program (that goes for number two and later) but they are hard to tune. And every time the controller fails Sidbot will hit the deck quite hard. So I want a quick way to find the optimal settings for the PID controller. My idea is to use a genetic algorithm to find the best settings. The algorithm works like this:
Generate 10 random PID parameters.
Try each of the PID parameters and measure how long
Sidbot stays on the ball.
Throw away the 5 least successful parameters.
Duplicate the other 5 parameters but change them a little bit when duplicating.
Start over at point 2.
This algorithm should run until a successful set of parameters is found. During Christmas holidays I developed as small function library that implement the steps mentioned above. I tested this library to find PID values for another controller that Sidbot uses. This PID is used to keep and adjust the heading of Sidbot. In the end this worked surprisingly well. But I also found out that it is not easy to quantify the success of a PID controller. Does it have to be fast, can it overshoot the desired position or not, etc?
I expect some difficulties with this in the future. However, now I got myself a good set of PID parameters for maintaining/adjusting the heading of Sidbot.
My new utility NXT2excel proves to be very useful to examine the NXT closely. Here is an example of the robotC PID speed control in action.
One of the advantages of robotC is that one has very good regulated speed for the NXT motors. Here is a graph I made of the speed control in action . I used NXT2excel to fetch the data in excel.
The graph shows 2 seconds of action, updated every 0,2 seconds. The dark blue line represents the regulated speed level. It is set to 50 right at the beginning. The dotted line is the actual speed of the motor . The motor runs at about 500 encoder ticks per second when it is stable. The light blue line is the Pulse Width Modulation value. This is a measure of he amount of power sent to the motor. When stable its value is about 65, meaning that the motor runs at about 65% of its maximum.
One can see that the PID control sends a bit more power to the motor when it is still gaining speed. Just before the intended speed is reached the PID control starts to power down the motor. This is to minimise overshoot. The motor speed is stable after 0,8 seconds and stays very stable. The power send to the motor changes a bit over time to keep the speed stable. At the moment the motor has to stop the PID control applies negative power to break the motor (I set the motor rest state to halt instead of coast). The motor halts within 0,4 seconds.
I love regulated speed!
(Battery level was about 7,1 Volt at the time. The motor was driving a NXT wheel with 20:12 gear ratio.)
Here is another graph, this one based on readings every 50 ms.
To balance on a ball Sidbot must know what is up and when it is tilting over. A lot of NXT based balancing bots measure the distance to the ground for tilt information. This technique cannot be used for Sidbot, it is a long way from the ground and the ball is in the way when looking at the ground. Therefore I will use gyro sensors to get tilt information.
NXT time 