Lines of Code, Miles of Autonomy: The Autonomous car Project - Phase I
Problem statement
To build a car fully autonomous able to take two locations within our university (eg. Engineering Block and Law Block) and drive the passengers to the location without needing a driver -making decisions with full autonomy such as path planning and obstacle avoidance.
The Car
We started out by building an EV completely from scratch with out-of-the-garage tools. Another duo of members from JUMakerspace named Jeshurun Solomon and Vedant Kaothekar handled the mechanics and chassis building of this operation.
A collage of our Chassis
The Autonomy
We began by planning on various approaches by which to make the car autonomous. The first problem was to look out for where the road is and where it isn't (lane detection that is). For this, Tilak bhaiya conceived of this logic- there are sidewalks on both sides of the roads in our university, we can use a 2D LiDAR to detect the edges of these sidewalks and form a map of these roads. I then planned to use a technique called SLAM as an overall approach. It stands for- Simultaneous Localization And Mapping- which means, quite literally to Simultaneously map the environment and estimate the robot's position within the map.
To go through with this, we first needed a map of the roads, and for that, we need to move the LiDAR around on the roads- and the easiest SLAM algorithm(not involving the usage of much odometry and data that is) is Hector SLAM. We used the Nvidia Jetson Nano development board as our processor and installed ROS 1 - melodic on it. We used the RPLidar A1 as our 1D LiDAR sensor. We created a workspace named Saxxophone_500 as our ROS workspace. You can find this workspace and all the codes on this gitHub Link: Saxxophone_500.
Nvidia Jetson Nano Dev Kit
RPLiDAR A1 scanning illustrated
The First Mapping Attempt
On my first attempt to map the college roads, we encountered some problems. The following points elucidate upon my experience:
1. The Rviz visualization of the LiDAR data showed that the scans were good and captured the obstacles perfectly.
First time I read the LiDAR data into the Jetson Nano
I was quite fascinated by the visuals that we played around a little
2. The Rviz visualization of the hector-slam mapping and the map generated did reflect the correct data being captured, but there was an issue in the movement detection of the robot (i.e. our car).
First time I launched the Hector SLAM mapping inside the room
(Was simply happy it was showing something)
Here, We were sitting on a road and took a scan
Some videos of us driving around mapping with hector SLAM
3. The Robot represented by a 3-axis model on the map, did not seem to reflect any of the correct movement, instead, it would either just stand there or jitter away sideways. This resulted in the map data being captured added in the wrong location, and ultimately in a completely unrecognizable map being formed.
Some issues I could think of:
How Hector SLAM captures movement- Due to the lack of odometry data(like wheel encoders, IMU etc.), it relies on a technique called scan matching- it compares subsequent LiDAR scans to detect changes and hence, estimate movement.
Some possible causes I can think of-
1. Since all that LiDAR will ever see is an edge of a sidewalk- here, a little ahead, even a mile ahead, it 'sees' little to no change - just parallel lines and when the car starts to get closer to one of the sidewalks, the LiDAR's distance from one of those parallel lines decrease and the other increase and hence, it detects a sideways movement(as it detects a change).
2. The vibrations of the car are too much for hector-SLAM to estimate the movement/pose correctly- it somehow manages to affect the pose estimation while the scan data is being visualized perfectly.
I ran a test to diagnose the issue-
1. I put the LiDAR on a practically hand-pulled small wooden plank cart and put the Jetson Nano alongside- we pulled this baby slowly throughout the entire hallway which has walls running parallel, distanced at no more than 2-2.5 meters, also, there was a bridge in the corridor- where, there were no walls on the sides, plus, there were some door openings that disrupted the parallel wall pattern. Meaning- a context-rich environment. The map was perfect. The movement detection was perfect.
This is a video we took on a separate occasion, after the completion of phase one.
But this demonstrates exactly our activities in the case of the car.
2. We took the same LiDAR cart down to the actual road and tried to carry out the same process. The data we captured was all over the place- in some instances, we encountered an error is some invalid data being captured. We couldn't get any meaningful result, but this time, because the roads were the same, this pointed to a very heavy sensitivity to vibrations too.
My final explanations were:
1. Because of the placement and being of the sidewalks and the whole environments of the roads, hector SLAM is not able to catch enough context for scan-matching to work.
2. Although the vibrational issue had a strong effect in the case of the hand-pulled cart, the existence of relatively clean scans when LiDAR was attached to the car- suggest that the vibrational issue did not play a major role in movement perception.
Conclusion- Because of lack of contextual data, hector SLAM failed to perceive movement in a more rough outdoor environment.
Google Cartographer
My next choice was to move onto a more robust algo which does take in more than scan-matching to figure movement out. I landed on Google Cartographer eventually- an open source library developed by folks at Google for real-time SLAM in 2-D and 3-D.
This does include more contextual data to form the maps leading me to think of it as more accurate.
This also includes:
1. Odometry Data
2. IMU data (optional)
3. GPS (optional)
This also detects loop closure when a robot has returned to its starting location- improves map estimation.
I started out with planning what sensory data to include. It was obvious to include Laser scan (LiDAR sensor). Apart from that, I chose to include two quantities:
1. IMU (Inertial Measurement Unit) data
2. Odometry data (like velocity, acceleration etc.)
My first choice of sensors were ICM - 20948 (an MPU9250 upgrade) as IMU and an E3856G5-600B-G24N incremental rotary encoder for odometry data.
I had to drop ICM-20948 and later cancel its order because of a significant delay and eventually unavailability of stock on the seller's end.
I, then, downgraded to MPU9250. I started by downloading a GitHub repo of a ROS package to integrate MPU9250 and publish a ROS topic for the IMU data.
The only problem was-
1. This repo created a driver class (abstract class) which implemented the i2c interaction with the MPU9250 IC and implemented the GPIO communication (callback function to call when the data is read) and an initialize_i2c() function which are both platform specific.
2. So, the repo included a file called rpi_driver.h and rpi_driver.cpp which implemented both of these functions for raspberry pi platform. These made use of pigpiod.h which enables the use of pigpio daemon for using the gpio functionality on raspberry pi.
3. But, it did not include a driver implementation for Jetson Nano platform. Plus, I observed a general paucity of material on this domain online. So, I decided to extend the driver class and implement this functionality for Jetson Nano on my own.
4. I successfully established communication with the MPU9250 IC but there was a catch- I failed to communicate with another IC which is embedded in the same chip- the ak8963. I put the MPU9250 in bypass mode (IMT_PIN_CFG = 0x22) to establish an I2C communication with the ak8963 IC but was unable to do that. After multiple different approaches, I concluded that the particular module I have had either a faulty or missing ak8963, either way, I dropped the MPU9250.
5. I learned that Cartographer only requires accelerometer and gyroscope as IMU data and not magnetometer - so I downgraded even further to MPU6050.
Luckily I found a complete article based on using MPU6050 with ROS in Jetson Nano! I was able to use the information in it to publish the /IMU/data topic. I did face some minor issues which I resolved and then created a launch file to launch it rather than having to rosrun the scripts every time I wanted to use them.
I then went ahead and created a robot description file- named Saxx.urdf and defined the transformation and the difference frames in it. Now, I moved onto the rotary encoders and the odometry data. Because of the difference in logic level voltage between jetson nano and the encoders, I had to use an arduino UNO as a middleman. I first connected the encoders to the arduino board and then uploaded a sketch to read data- I used Paul Stofferger's library- encoder.h (this is the GitHub repository's link). I then took in the data serially from the arduino to the jetson nano via a usb cable. Then I wrote out an entire package for it and named it encoder_odom with a package.xml, src and a launch file.
I then created a unified launch file for all of the 3 sensors-
- MPU6050
- rplidar_a1 (used the usual rplidar_ros package)
- E3856G5-600B-G24N
This launch file also included the launch for google cartographer- so this launch file would launch the mapping software of cartographer including the 3 sensory data nodes.
When I launched the file and began to visualize the map being created, the laser scan as usual, were good- as I could see by the /scan topic being visualized.
All else, was a nightmare- odometry was all over the place (but we couldn't even get a good enough visualization for it because the robot (tf viz.) was jerking around everywhere on the damn map)- where I visualized the /IMU/data in isolation, I saw the problem bare- the IMU symbol (a big orange arrow) - was in-fact reflecting the actual motion from the MPU6050, but there was just- so much noise- which was essentially causing it to jump around.
These were the initial maps I got with the MPU6050 as my IMU
After a consultation with Tilak Bhaiya, I learning that these commercial grade IMUs (such as MPU6050) are not meant to be used for the purposes of inertial navigation. For those purposes, we will have to use IMUs which are specifically manufactured for purposes such as this (more expensive ones).
So, for the time being, I decided to go with odometry data alone to track the robot's movement- Now there were some very critical issues with the current odometry setup as well. See, I had two 600 ppr rotary encoders planted on both of the rear wheels. I had an arduino UNO as a middleman between the encoders A and B channels were on pin 2 and 4 and the other one on 3 and 5 - Now, the data that was reaching the Jetson Nano was great- but the foundational concept I was using was flawed.
You see, the model of odometry I was using was a differential drive model- which is a simplistic model for movement of small two-wheeled robots- but our car (as any other car), worked on the Ackermann model and kinematics. This meant, we had to implant another encoder on the steering and also alter the logic for the movement computation- So I deleted the current package for odometry and created a new one and wrote a new cpp code specially with the ackermann logic in mind.
This was all set to work- but then another issue showed up- you see, an arduino UNO only has two hardware interrupt pins- 2 and 3- which means, even with a reduced resolution, it can only accommodate two encoders at max- so, we decided to use two arduino UNOs- 2 on one and 1 on the other. Now this, should've worked fine - but, it did not- this time, the issue was getting the data from all the encoders to the Jetson Nano- the first approach I took was to connect both of the arduinos via a USB cable each - but the issue that presented with this approach was that it drew too much current- thus forcing Jetson to shut down and restart consistently whenever the node was launched and a serial read was attempted. The second approach was- using only one arduino to connect to the Jetson Nano and connecting the 2nd Arduino to the 1st one via UART connection- this method failed too because of the current issue - there were still two arduinos goddamit - I then decided on a 3rd approach after a consultation with Tilak bhaiya- connecting the first UNO to the jetson nano using UART- this approach failed because, according to various forums, Jetson has a shitty UART pin header.
this image shows the odometry visualization red arrow not only
rotating erratically but teleporting from place to place
here we can see the corruption in the map
happening because of the corrupted odom data
It was also an issue that the first arduino could serially read and write data simultaneously without corruption. I, then, finally decided to switch to arduino Mega. which had 6 dedicated interrupt pins- which handled everything perfectly and we connected it via a USB cable with no power issue bugging us this time- So, we were able to get perfect data this time to Jetson Nano from the rotary encoders. I then tested the ackermann node and- what the visualization showed us was that, it was essentially reacting to the actual encoder data. and there were no signs of the earlier issues pertaining to odometry jumping around and essentially teleporting from one location to other (this was created due to noisy data which issue was resolved by now).
The remaining issues was- oversensitivity to the steering rotation data- which essentially meant that even I'd do much as rotate the steering wheel a little bit to maneuver it slightly, it would essentially form circles in the visualization.
this is the odometry reacting strongly to even a
slight nudge to the steering
I solved this by multiplying the angle calculation line by a steering gain ratio which was essentially from 0 to 1 and adjusted it to match the actual car's movement after repeated driving and observation. So, the odometry visualization, now, was just right.
Here, we were just excited to see the car's actual
odom reflected on the screen better
After this, I set out to attempt mapping by launching cartographer once again- this time utilizing- /odom and /scan only. I encountered the following issues-
1. Learned that during daytime the time of direct sunlight- the LiDAR scans got corrupted due, in part, to the NIR light that interfere with the scan.
2. Due to cartographer's reliance on scan-matching during turns and a lack of IMU data, whenever a scan showed a curved surface, cartographer would essentially assume it as an indicator of the robot taking a turn even if it physically isn't.
3. When a map gets large, to a considerable scale, the computational cost gets essentially too high and the Jetson nano hangs.
So, the next steps would be to keep attempting mapping and keep tweaking the parameters from the configuration .lua file until we get the map just right.
After tuning the .lua parameters a few times, we did get a pretty decent map, at a specific loop. There were some issues though- all related, in one way or other, to the detection of rotation of the car. During turns, our software has not the slightest idea of how the turn was actually taking place.
We figured we would require an actual industrial grade IMU for this purpose. But to make it easier to procure budget for the same, we had to first display some results.
Some decent maps we created
The best one I got 😭
The Full Stack on a Straight Road
So, I decided to implement the full stack on a straight road. for this, we created a map of a straight road and implemented localization using AMCL(Adaptive Monte Carlo Localization) + map_server (to serve the already saved map on the /map topic) setup.
Straight line Mapping and Localization
I created a unified launch file to launch this complete stack, the issue was that it publishes twist values which are movement values associated with a differential drive robot. But I had a car- which follows the Ackermann kinematics model. I, therefore, had to write a separate node named twist_to_ackermann, that converts twist /cmd_vel values to /ackermann_cmd values like steering angle and speed.
When we launched the unified navigation.launch, it ran fine- until we set a nav_2d goal. When we do that, the planners call lookup_transform there was a delay in the timestamp whose transform was looked up and the latest timestamp transform. This caused an- extrapolation into the future error.
I solved this by first learning who published the map -> odom transform - it was /amcl node and then introducing a tolerance parameter of 0.3 seconds in the unified launch file.
Now, everything was perfect. When we set a nav goal, the ackermann_cmd spins up with glorious steering and speed values. The last step which remained was to introduce an interface layer between these values and the physical actuators for the steering and the acceleration.
For this purpose, instead of creating another ROS node within Jetson Nano, I chose to outsource this layer to another microcontroller- My first choice was Arduino UNO. The first difficulty I faced after writing the code was the ros.h library. The thing is- you can neither download it from the arduino ide's library manager, nor can you download this online anywhere. You need to generate it natively within a ROS environment. So, I generated in the Jetson Nano, compressed it to form a .zip file to chatGPT in the same account as my Mac, and then downloaded it in my Mac.
Once ros.h ceased to be an issue, the second issue I faced was the lack of SRAM space in Arduino UNO, which forced me to choose arduino Mega as an alternative. And with the Mega, we connected the linear actuator(for the throttle that passes onto an accelerator pedal) and a motor (used to actuate the steering wheel) via an IBT-2/BTS 7960 H-bridge motor drivers.
Ran a rosserial script on the jetson side- launched the navigation launch file- et Voila- we had a self-driving car. As soon as we set a nav_2d goal on Rviz on the map, the actuators come into action and as soon as the linear actuator comes into contact with the pedal, we saw the magic happen for the very first time. The car shot off and the steering wheel was turning midway.
I took 'em from the project reel we uploaded on the JUMakerspce insta page
There was, however, still some issues- like the linear actuator, once, jumping into action -> taking forever to reach the pedal and also, it takes forever to retract, which led the car to move way past the set goal. Also, I was, although ecstatic at the moment, still in doubt regarding the actual movement and control of the car. So, I tried to observe, tweak, and configure stuff till I make it.
Phase I over
Great job👍👍
ReplyDelete