Preface
During my time at my first internship at Omnipresent robotics in 2016, apart from the dare for making a quadcopter, I was also given the dare of creating an autonomous ground vehicle using GPS and IMU sensors (and ultrasonic sensors for obstacle avoidance). I have seen a fair number of tutorials that make these autonomous vehicles using a single-board computer with ready to go code (ROS packages) or using a pixhawk(or compatible) board with ardupilot/px4 running the rover code. However, I felt that I would learn virtually nothing if I did this, and hence decided to build my system from the ground up on a microcontroller. This blog is a short summary of the first two versions.
Version 1.0 (2016-2017)
I started with a fairly basic platform; a slightly modified HPI sprint 2, a simple GPS (ublox NEO-6M), a magnetometer, and an IMU. I created a 3d printed harness for the sensors to hold them in place. The algorithms for position estimation were fairly simple; a complementary filter with the gain dependent on GPS's reported accuracy combined with a complementary filter for the AHRS. The control approach was relatively simple as well, a PID controller on the steering to point the nose of the car towards the next waypoint and an on/off throttle control based on distance from the waypoint. The control and state estimation were performed in-sync at a rate of 50 Hz.
After a few rounds of testing, I realized that the cheap GPS was useless for low speeds as it’s horizontal accuracy would be worse than 5 meters (even px4 and ardupilot stop using the GPS once the horizontal accuracy becomes worse than 2.5 meters). Thus, I decided to add an optical flow sensor to the setup, like the one found in an optical mouse. The advantage of an optical flow sensor was that it could track slow movements in 2 (orthogonal) directions and it wasn’t affected by wheel slippage. However, I wasn’t able to fully utilize it as it needed a lot of light to operate. This version was concluded in the summer of 2017.
Flaws in version 1.0:
The harness designed to hold the sensors was flimsy.
The code was all in one single file, which made it a nightmare to manage.
State estimator was using a modified complementary filter which “worked” but wasn’t ideal
Did not have enough light to support optical flow navigation.
Navigation was based on a point and shoot approach using a PID controller (which worked fine, but I needed something better)
The GPS module was pretty much useless.
Low control and estimation frequency: For a small vehicle, the inertia is usually low and thus the refresh rate needs to be higher.
The wiring was all over the place and would get lose at times, which eventually led to a crash.
Updates:
With this version, I decided to redesign the hardware and the software. I wanted to make a setup that would be capable of racing. Thus the focus of this version was on making the car reliable at speed. For this, the following improvements were made:
The sensor harness was improved: I used a square tube design to reduce flex.
Use of a smaller breakout board for the Atmega328P.
The hmc5883l and mpu6050 were ditched and replaced with a single mpu9250 to reduce wiring.
The optical flow was placed inside a shroud situated behind the car (which made the car look like a beetle to some extent).
In terms of the software, the position estimator now used a simplified extended Kalman filter; the simplifications were based on the assumption of non-holonomic constraints and motion on a 2-D plane. This would run at an update rate of 400 Hz. The AHRS was still using a complementary filter with no tilt compensation for the compass to keep the computation time low.
Direct Curvature Control
The first version was trying to mimic the Ardurover project, which used an L1 controller. While this is better than a PID based point-shoot approach, this controller could not control for the destination heading, i.e, it only controls the position of the car and not the pose. If the heading is also to be controlled, a smooth trajectory connecting the car's current position to the end goal is required such that the tangent at the end-point is along the direction of the required heading. This would further require calculation and storage of discrete intermediate waypoints leading to an increase in the computational and memory requirements for the purpose of control.
This is when I came up with the idea of using direct curvature control. The idea here is to have the trajectory defined by a parametric C2 continuous curve, and find the steering angle and speed by finding the curvature along the trajectory where the car would be one control cycle into the future (the control frequency should be less than or equal to the steering actuator bandwidth). I chose 3rd order Bezier curves for this as they allow for a point of inflection in the trajectory, which is required for lane changing-like maneuvers. The advantage of a bezier curve over a B-spline is that it isn't defined recursively which makes it convenient to use.
The advantages of this approach are:
The trajectories can be generated with little computational expense as we only need to find the control points to find the entire curve; the initial and final points are the current position and the target position while the intermediate points are found by extending a line along the direction of the heading at the initial and final points).
The actual control does not require discrete trajectory points, the curvature, and thus, steering angle as well as maximum velocity, required at the next control time step can be calculated directly from the control points. Thus, it has lower computational and memory requirements.
The trajectory is generated at every control cycle and adjusts to the over/undershoot on its own.
It requires absolutely no tuning to start working (for reasonable maneuvers).
If information about the location of obstacles is available, it is fairly simple to define an intermediate point which allows the vehicle to go around the obstacle.
These reasons make the bezier based approach great for computationally constrained systems that need to “just work”.
While this approach is computationally inexpensive, due to the state estimation taking up most of the computation time (2.2-2.3 out of 2.5 milliseconds), the control code could not be executed in one fell swoop as that would take roughly 800 microseconds on an Atmega328P. I decided to break down the control into smaller “sub-functions” which would be executed across multiple cycles, resulting in a control frequency of 50 Hz. This was an Adhoc RTOS implementation.
Here is a gif showing the car in action, following the trajectory shown in the image below it (red line):
The red line shows the trajectory, the white lines show the lines connecting the waypoints, and the yellow curves show the steering angles at those points (notice that the steering angle graph is discontinuous at the ends. This happens because the 3rd order Bezier curves can't control the curvature at the final point, and thus C2 continuity does not hold between two sections).
Flaws in version 2.0
Now, it’s not all hunky-dory in the land of bezier curves; the approach so far is somewhat naive and has the following disadvantages:
The generated trajectory does not guarantee any sort of optimality.
It requires the user to define the exact waypoints through which the car must pass. I would prefer to only define the center of a region through which it must pass and let it optimize the trajectory for me.
No support for preemptive braking (as this requires finding the point of maximum curvature which wasn’t done in this version). Preemptive braking is essentially applying the brakes before you reach the turn rather than slowing down as you turn.
Besides this, there were a bunch of other issues with this version, such as :
No telemetry feedback from the car; I used to pull my hair out trying to understand what went wrong during debugging.
The way-points were hardcoded into the code.
Not enough light for the optical flow to work at higher speeds (beyond 3 m/s).
I would often get no GPS lock (which forces a speed limit on the car as the optical flow sensor did not support speeds higher than 3 m/s).
The GPS’s reported speed would only be useful beyond 7-8 m/s, however, the optical flow would provide incorrect speed measurements beyond 3-4 m/s (due to insufficient lighting), thus creating a valley in the middle where I had no reliable speed measurements.
The optical flow sensor would hang behind the car, which made it look like a beetle. The aesthetic aspect aside, this makes the setup vulnerable (to impact) as some of the components are outside of the vehicle boundaries.
The AHRS was running a simple complementary filter with no tilt compensation for the heading.
The control board was being powered by the electronic speed controller, which would sometimes have a voltage sag under high load. This caused the microcontroller to reset, resulting in random PWM signals being sent to the ESC.
No fallback controller: There should always be a fallback controller that allows regaining control of the car, either automatically (by detecting a failure) or by force (flipping a switch on the radio controller). I learned this the hard way when this version ended up crashing into a wall at close to 7 m/s and damaging the front of the car. The cause is still unknown (see disadvantage 1) ) but I speculate that this happened due to a voltage dip (see disadvantage 8) ).
No support for a companion computer (for computer vision or high processing low rate tasks).
This concludes part one of my journey in building a mini autonomous (race)car. Part two of this blog will cover the current (long term support) version. Stay tuned!
Comments