Extended Kalman filter

 I'm trying to implement the Extended Kalman filter from the robot_localization package by clearpath, and I finally configured the launch file and the yaml file that holds all of the parameters. It turns out that all I had to do is to actually list the topics in the yaml file and voila! I have a readout of the pose estimate with covariance. However it seems that when I run the ekf noe along with all the other stuff the serial connection has a problem. At least the robot doesn't respond to the move commands anymore.

I'm honestly really tired of the serial problems as it's preventing me from working on the more interesting stuff like SLAM.

Here's a list of stuff I would like to work better:

PID. Currently I'm just using proportional control and it is very smooth. When I try to tune the PID controller better it usually ends up kinda jittery. So if this was better the error between the set velocity and the actual velocity would be less, which is also beneficial for the ekf. Because then I could use the control vel as an input. And enable that parameter to be used.

EKF. For some reason running this makes the robot irresponsive.

SLAM. I tried making a map of my house by driving the robot around, but there was an error and the map didn't work and it didn't show up in Rviz as I wanted.

Kinect: I'm having trouble with the depth camera and the stream in general. It seems that there isn't enough bandwidth on the USB bus if that makes any sense. It works fine on the pc, but the raspberry pi really struggles for some reason. Am I actually running into a resource limitation or am I just doing this in a really stupid way?

In any case, I finally finished my course on Data Structures and Algorithms and can now get back into working on this. uggghhhhhhhh. just do the thing!

On a happier note, I want to try loading my urdf model into Gazeebo. I think that could be fun, because I could assume that in a perfect world I wouldn't have hardware problems and could just drive the robot around.

Another thing I could start doing is contributing to open source communities.

Added the kinect

I finally soldered and installed the kinect to the front of the robot. 

The kinect boasts an impressive array of sensors including an depth camera, RGB camera and microphones. My intention is to use the kinect for SLAM and maybe teleoperation. The microphone would be able to listen to commands such as "move forward". Technically it can also be used for a bunch of fancy stuff like gesture recognition, since its original purpose was to be a gaming console that tracks player's movements.

Naturally I've had to elevate the LIDAR so that the kinect isn't blocking the laser. This is not ideal because now the lidar can't detect objects that are below a height of 25 cm. So its going to have a hard time detecting the small stuff I might have on my floor. This limitation makes it worthwhile for me to learn computer vision and use the kinect camera for object detection.

Adding URDF

So now that I have a whole bunch of sensors on the robot it's time to talk about transforms. 

Transforms are the relationships between different frames of reference on the robot. For example the center of the robot could be 5 cm off the ground, but the laser scanner is 10 cm off the ground. 

So your transform would be ( 0 0 5) to indicate that it is 5 cm up in the z direction (the convention is x y z).

However as you can imagine it gets messy trying to keep track of all of the transforms as the robot grows in complexity. What can end up happening is that you have transforms tucked away in different files, and its not obvious where to find them if you need to change one. 

Enter URDF!

URDF stands for Unified Robot Description Format and it is used to create a visual model of the robot as well as describe the relationships between links. Essentially it is an XML file with some dimensions and other values. The great thing about it is that you can use macros to make it really efficient to specify the details and have one central spot to edit the robot characteristics.

Recently I created my own URDF file to describe my robot. What I need to do next is to connect it with a robot state publisher that will publish the transforms for me.

Current state of the robot

 So currently the robot can be teleoperated via keyboard and scan the room using the lidar. I also connected the IMU and 4 ultrasound sensors. 

Driving the robot around is really fun, but because the wheels are all controlled individually it doesn't go straight. Surprisingly the strafing mode works a lot better than just driving forward. 

I did some digging online and found out that one way to solve this problem is to control the linear and angular velocities individually. 

Below is a graph of what I currently have as a setup on the robot and what I want to have.

At first I didn't want to do this, because I didn't want the kinematics calculations to be on the Arduino. I'm going to have to find a linear algebra library or figure something else out. 

One thing that's kinf of weird is that Twist commands are in Float 64 format, but that doesn't work with ros-serial. It automatically converts it to Float 32 and a loss of precision occurs. 

One thing I'm really excited for is to implement an Extended Kalman filter. I learnt about it in school during my Bachelor's degree and it seems like an advanced concept with a lot of cool applications.

LIDAR experiements

  

 Working on the lidar I discovered that you first need to create the build directory before you cd into it.

$ git clone https://github.com/YDLIDAR/YDLidar-SDK.git
$ cd YDLidar-SDK/build
$ cmake ..
$ make
$ sudo make install

 

lidar is -60mm in the x dimension. 0 in the y. 1/2 height of robot in the z.

It can be launched with 

roslaunch ydlidar_ros_driver X4.launch

roslaunch ydlidar_ros_driver lidar_view.launch

Then you can view the results in rviz.

The next step is for me to establish the transforms from the lidar to the center of the robot. I can do that by defining a static tf broadcast or by specifying that in the URDF (
Unified Robotics Description Format) file. Then I can make the visualization better.

odometry implemented!

 Hey guys, I haven't been keeping up with the blog, because I've been progressing too fast to write down what I've done. I have finally completed the odometry node on the robot and implemented a tf broad cast on it.

 I know that odometry isn't a reliable method for localization, so I've also bought an IMU. After some test I was able to connect it to the Arduino Mega using the SDA pins. I haven't calibrated it yet so I don't know how accurate it will be, but I can combine the readings from the IMU and Odometry to implement an Extended Kalman filter, which should make the pose estimation a lot better.

I'm very worried about the Serial communication between the Mega and the Pi. There are a lot of parameters here. It seems to be working so far though. 

ALSO! I finally bought a lidar, which sits on the robot like a little hat! This will let me map the environment and navigate obstacles. I'm really excited to use it!

New Enclosure!

 

Hey it's been a while! I've had a lot more time to work on the robot recently, so I've been making so much progress that I haven't been able to write it all down.

Last time I left off, I was having problems getting the robot to respond quickly over serial communication. It turns out that the 10 AA batteries that I was using as my main power source were draining to quickly. 

I decided to fix the issue by finally getting a lipo battery. I chose a 3 cell battery, which is rated at 11.1V.

Lipo batteries are dangerous and should not be discharged below a certain rate. With the addition of the kinect I also realized that I need something that would provide a stable 12V. I'm sure the kinect has an internal voltage regulator, but I still want to be able to provide a steady voltage. This would also make sure the motors are operating at their rated voltage.

In order to solve the problems above I bought a buck-boost converter and a low voltage cut-off circuit. The battery came with really large wires so I also had to buy an xt60 connector. I'm also going to add a more robust power switch. Eventually the circuit will look like this

Battery >> connector >> power switch >> low voltage protection circuit >> buck boost converter.

Currently I just have the low voltage protection circuit in there with the connector and the switch.

Since cost is a driving factor for me, I chose the cheapest low voltage protection circuit I could find, which relied on a potentiometer to adjust the cut offs. This proved to be really annoying to set and very unreliable. In the future I'm going to pick something that provides me with more information even if it costs extra.

Because I don't have a power supply, I had to put some resistors between the battery and the circuit, measure the output with my voltmeter and then twist the adjustment screw on the potentiometer. The process was excruciating, but I did manage to set the cutoff point at about 11 V.  Now that I have a few extra pieces of gear it's time to start thinking how I'm going to fit all of that on the robot.

Enclosure:

In the past I made a second platform for the robot and elevated it with columns. Going forward I wanted to make the robot more presentable and take advantage of the space more. I plan to put the kinect and lidar on the second level, so I needed another approach.

Here are the objectives of the enclosure

1. Make the robot look more professional
   
2. Hold the lipo battery securely
3. Provide space for the low voltage circuit
4. Provide space for the buck boost converter
5. House the arduino and the motor shield
6. House the raspberry pi
7. House the raspberry pi battery
8. Provide access to the raspberry pi battery power button
9. Provide access to the arduino usb port
10. Mount at least 4 ultra sound distance senors
11. Provide room on the second level for the kinect and lidar.

As you can see it's a tall order. When I added the adafruit motor shield I had to screw down the arduino. The existing holes in the enclosure weren't easy to line up against. As a result the board is positioned in such a way that when you plug the USB cable in, it sticks out past the edges of the platform. This isn't ideal as I hoped the whole thing could be completely enclosed. However this was actually convenient for development, because I could access the cable from the side.

The enclosure took me more iterations that I would have liked. My biggest issue was lining up holes in the 3d model with holes on the actual chassis, since the chassis is from china, I didn't get a drawing of where the holes were and had to rely on my imperfect measurements made with a caliper. 

But eventually I was able to print the enclosure! Here is what the end product looks like!

 

I had to create a cut out for the arduino cable and profile cutouts for the distance sensors. The lipo battery is fixed with a ushaped bracket, same with the raspberry pi battery. The pi is mounted on a plate that inserts into the battery bracket. This enables it to be removed if need be. 

I also had to get a usb-C cable with a 90-degree bend so that it wouldn't interfere with the lipo battery.

Overall, it fits really nice! 

To attach the roof of the enclosure to the walls, I added a lip to the walls. The lips houses a nut inserted into a cutout. This nut allows the roof to the secured with screws. The roof also has a large opening in the middle to feed wires through from future additions such as the lidar or the kinect. 

Another part of this issue was getting the enclosure to print in a way that would look beatiful. Since the walls are mostly flat it made sense to lay them down, in the print configuration, but that resulted in some ugly layers. What I ended up doing instead is printing all the pieces upright. This took wayy more time, but the result was phenomenal! I originally intended for the enclosure to be a mint colour, but I ran out and had to use purple silk PLA. This gives the robot a distinct shine!

Overall, I'm really happy with the progress so far. All of the wire mess can be hidden now and protected from my cats.

    

PID:

After making the enclosure I started driving the robot around again, and I was disappointed with the accuracy of the control. I drove the robot forward and the told it to stop. The robot overcorrected by driving backwards. This told me that there is still work that needs to be done on the PID tuning.

This stuff is the bane of my existence. I realized I may be a bit out of my depth as I've previously tried a lot of different tuning approaches, and I still couldn't get a reliable result. I decided to find a PID library and found the Arduino PID library by Brett. 

This library is intended to replicate what is being used in industry and has numerous improvements over the regular PID algorithm. I replaced my code with the PID from the library and was still having trouble.

The first thing that I did right was to correct the low pass filter I was using on my velocity measurements. As they say garbage in garbage out. I adjusted the cut off frequency and the sampling rate of the filter, which provided significantly more accurate results. That got me thinking that I also had to do the same for the PID controller. One of the improvements that the library implements is that the controller runs at a set interval to increase the performance. After playing around with the PID constants I was finally able to get the controller to follow a sine wave! 

I was excited to try it with the rest of the code, however when I tried to setup the serial node on the pi to connect to the arduino I got the error

wrong checksum or topic id

I looked online and one of the solutions is to increase the delay in the arduino loop. Yesterday I was able to fix this problem by increasing the time between evaluations of the PID loop. I think it has some interrupts in there that must be screwing up my message reception. I still get the error occasionally, but at least it's able to connect. 

I tried to drive the robot around, but there were HUGE spikes in output! The thing will lurch forward like a monster that is out for blood! This is disappointing, because I'm not worse than when I started in terms of PID control. The PID library that I'm using wasn't designed for robots, but rather fridges or cruise control in a car. So, I'm going to investigate if there's something I can do to get smoother performance. The library offers quite a few sophisticated tools that I can take advantage of, such as adjusting constants on the fly. 

If I can't figure it out, I might try another library or just go back to my code. 

 

 Here's a video of the robot startling my cat:

https://www.youtube.com/shorts/RhPg2kjIFW0