- Haoru Xue - Electrical Engineering
- Yuhan Zhang - Electrical Engineering
- Cheyenne Herrera - Math/Engineering
The goal of our project is to increase the safety of the self-driving car by implementing rear-end collision prevention as well as apply the lane change safety. The Donkey RoboCar will the car will speed up if a vehicle/object is approaching it from behind and will not change lane when there is another car in that lane. Additionally, the car will stop itself when approaching an object in the front using a TOF sensor mounted to it. Furthermore, the RoboCar will implement lane change on command.
- Jetson Nano with fan and wirless card installed
- PCA9685 PWM (control servo and ESC)
- Steering Servo (control steering)
- Electronic speed controller (ESC) (control throttle)
- Relay (provide emergency stop)
- LED (show emergency stop status)
- 5V regulator (powering Jetson)
- USB camera
- Arduino (for connecting ToF sensor)
- Time-of-flight sensor (ToF)
- Lidar and USB controller
Donkey and parts
We use the Donkey Car framework for car control. With the framework, we can easily train deep learning autonomous driving models by recording manual driving. The frameworks use modularized "parts" to manage all the components in a car. When the car runs, it loops through all parts that have been added to it. Not only sensors can be Donkey parts, controllers and actuators are also Donkey parts.
Our project involves new sensors, ToF and Lidar, that have not been included in Donkey. They should be added to Donkey in the form of parts.
To use the PyLidar3 library
Create an instance and connect
lidar = YdLidarX4("/dev/ttyUSB0") lidar.Connect()
scans = self.lidar.StartScanning() for scan in scans: for i in range(360): self.scan[i] = scan[i]
Stop Lidar and disconnect
We use Adruino to connect to the ToF sensor.
myArduino = serial.Serial('/dev/ttyACM0', 115200)
Get sensor reads
myArduino.flushInput() ser_bytes = myArduino.readline() try: distance = float(ser_bytes[0:len(ser_bytes) - 2].decode("utf-8"))
Donkey uses a dictionary to save data that flow through each part. The data used as input or output of parts should be defined when adding parts.
Example: adding Lidar to Vehicle in manage.py
V.add(lidar, outputs=['lidar/scan', 'lidar/time'], threaded=True)
Lane changing behavior training
Donkey framework supports training different behavior states. We can use that to train a model that is able to perform lane changing.
Basically, when the feature is turned on, each data is tagged with a state, which the user is able to switch with a press on the controller. by default, there are two states: Left_lane and Right_lane. First drive a couple of laps in the left state, then another few in the right state, and finally, train the transition. This is the trickiest part.
1. Press the button to switch state
2. Drive a very short distance and then steer
1. Make the car come to a stop
2. Press the button to switch state
3. Steer to maximum, but no throttle
4. Apply throttle and complete the lane change
The reason why the first method would produce an undesired result is that a little portion of driving straight on the wrong track is recorded. Since data is collected only when there is throttle applied, it is important to steer first before applying the throttle, so that the computer leans that the only right thing to do when driving on the wrong lane is to switch immediately.
Rear collision prevention
Our first goal is to provide rear collision prevention. When another car is going to crash into the back of our car and there is no obstacle in front of our car. We can accelerate forward to prevent a collision. To use Lidar to detect objects in the back. The car is 250 millimeters in width so if we want to detect something that wide 200 millimeters away from the back, we need at least about 60 degrees of Lidar measurement.
We set a detection range of 60 degrees, 150 to 210, and from 200 millimeters to 2000 millimeters. We use cosine to calculate the real distance from lidar readings which include angles and distances. Then the median of all 60 distances is selected to be the distance between our car and the object in the back.
First, we just set a threshold, when the distance is lower than 200 millimeters we add throttle by 0.1. It works as intended preventing rear collision but there are some lags and it cannot go faster if the object in the back does not stop.
To mitigate the problem above, we look further and act earlier. Relative speed is calculated using two adjacent distance measure and time between. If the speed of the car is lower than the speed of back object in the same direction, we will add a value proportional to relative speed and inverse proportional to distance. If the distance goes inside 200 millimeters the car will run at its full speed.
Here is part of the code that implements the system described above.
# when lidar is not updated, keep last change if time == self.lasttime: return self.gett(throttle) distance = self.get_d(scan) if distance == 0 or distance > self.maxdis: # out of range is often 0 self.lasttime = time self.lastdis = distance return self.gett(throttle) if distance <= self.mindis self.lasttime = time self.lastdis = distance return 1 if self.lastdis != 0: rspeed = (distance - self.lastdis) / (time - self.lasttime) if rspeed >= 0: self.lasttime = time self.lastdis = distance return self.gett(throttle) self.throttle_change = rspeed/(self.mindis - distance)*50 self.lasttime = time self.lastdis = distance return self.gett(throttle)
gett() here is adding the change to throttle and capped by 1.
Safe lane changing
When changing lane, it is not safe to just look at mirrors. Using Lidar we can detect cars in the blind spot and override steering control to prevent unsafe lane changing.
More specifically, use the yellow regions of the Lidar scan to do this. The black region is blocked by the car itself. It is tested to be under 45 degrees on both sides. Similar to the last part we set a virtual "wall" on both sides of the car: we calculate the distance using sine and set a threshold 300 millimeters. If some object is on one side of our car, we block the ability to turn further in that direction. The measurement range is very large, larger than other cars. Also, sometimes only part of the object can be detected in the range. So we assume there is an object when at least 20% of the distances are under the threshold.
dis =  # turn right if angle - self.last_angle > 0: dis = self.distances(scan, True) # turn left if angle - self.last_angle < 0: dis = self.distances(scan, False) if angle== 0: self.last_angle = angle return angle disnp = np.array(dis) if sum(disnp<self.mindis)/len(disnp) > self.threshold: return self.last_angle # locked else: self.last_angle = angle return angle
This rule doesn't work well when there is an object on one side and the road itself is turning to that direction. Therefore the action of this system is better to be a warning rather than locking the steering. Another way would be using lane change detection rather than steering angle increment as the condition to activate the detection.
Front collision prevention
The front collision prevention is similar to the rear one. The Lidar is blocked by the car itself so we cannot get distance measurements in front of the car. Instead, a Time of Flight (ToF) sensor is installed to do that job. The sensor communicates via i2C protocol, but the i2C pins of the Jetson Nano have already been occupied by the connections to the PWM board. Although it is possible to assign a different address to the sensor and talk to it directly via Jetson's i2C, it takes extra work to implement. Instead, we connect the ToF sensor to an Arduino UNO and talk to the Arduino via serial from Jetson.
Our VL53L1X Time of Flight Sensor (Make sure you don't mess it up with VL53L0X) works by shooting a laser beam, recording the time for a round-trip, and calculate the distance. Its maximum range is 4 meters (while VL53L0X only has 2 meters), although we mount it a little downward to let the beam hit the ground at a 2-meter point. Because we are not interested in seeing too far, and we want to make sure that it always gets valid readings.
Depending on how you want to test the collision prevention ability, you can mount it at any height of your choice in the front. If you are using an actual car as the frontal vehicle, mount it low enough to be able to detect the other car. If you have a human being mocking the frontal vehicle, mount wherever you like.
The sensor has four connections to make with the Arduino: GND, VCC, SDA, and SCL. make sure the connections are firm, and your soldering work is good. Download the sample code Continuous.ino from Pololulu or Github. Make sure it is for VL53L1X, not VL53L0X -- we got tipped over on that. change the sampling frequency. The datasheet recommends a maximum of 20Hz (50ms), and that is what we use.
Troubleshoot: if the serial prints "Fail to initialize sensor", make sure you are using the library and code for the specific sensor, and try testing with known good working sensors.
After making sure the serial output is as desired, plug the Arduino into the Jetson. Then give permission to access the port.
sudo chmod 777 /dev/ttyACM0 # depending on the devices you have connected, it could be ttyACM1 or others
It must be done every time you reboot. Workarounds could be found in "Useful Knowledge".
we did not write a separate part for the sensor but instead embedded the logics into the controller part. It is because we would like to utilize the emergency brake method, and also invalidate user input when an emergency response is in action.
In class JoystickController(object):
myArduino = None prevTOFReading = 200 Estop = False
def TOF(self): self.myArduino.flushInput() # Always flush so that old data does not pile up ser_bytes = self.myArduino.readline() # Read in the original, undecoded bytes try: decoded_bytes = float(ser_bytes[0:len(ser_bytes)-2].decode("utf-8")) # Decode the bytes except: decoded_bytes = self.prevTOFReading # On error use the previous reading #print(decoded_bytes) if decoded_bytes > 1500: # Not interested in things happening too far decoded_bytes = 1500 if decoded_bytes < 250 and self.prevTOFReading >= 250: # The physical position of the car with the frontal object is too close print("Too Close!") self.emergency_stop() elif self.prevTOFReading <= 1500 and (self.prevTOFReading-decoded_bytes) >= (decoded_bytes/12) and (self.angle < 0.6 and self.angle > -0.6): # The car has a tendency to crash and it is not turning to avoid print("Gonna Crash!") self.emergency_stop() self.prevTOFReading = decoded_bytes tofSampling = threading.Timer(0.05, self.TOF) #In 50ms, wake up to retrive the next reading tofSampling.start()
In def __init__():
import serial if self.myArduino == None: self.myArduino = serial.Serial('/dev/ttyACM0',115200) self.TOF()
A list of known issues:
1. Error in reading and decoding happen from time to time.
2. The user needs to hit Ctrl-C again after shutting down the car to kill the sampling thread.
3. Lag is obvious under high velocity.
4. Need open space for test driving (walls, lockers, and humans right next to the track that's in the way of the sensor would cause misdetection.
Protential side collision prevention
In this task, we use a combination of techniques in rear collision prevention and safe lane change. For side collision prevention we don't need that many measurements as in safe lane change. Because for lane changing we need to make sure the car behind in the other lane is far enough such that they can be aware of our lane changing. But for side collision prevention we just want to keep objects outside our virtual "walls" and minimize false alarms. The wall behind is 200 millimeters and the walls on the sides are 200 millimeters from the Lidar. So the angles for more than 90 degrees should be arctan(200/200) which is 45 degrees. The maximum distance to start detection is set to 800 millimeters.
After finding the region to look at, we use the technique we used in the safe lane changing task, to detect an object when at least 25% of the distances are under the threshold. Then the average of distances under the threshold is used for calculating the speed using the technique in the rear collision prevention task.
rthrottle = 0 lthrottle = 0 if time == self.lasttime: return self.gett(throttle) rdiss = self.distances(lidar_scan, True) ldiss = self.distances(lidar_scan, False) rdisnp = np.array(rdiss) ldisnp = np.array(ldiss) rdis = 0 ldis = 0 # get right distance and left distance if sum(rdisnp < self.maxdis) / len(rdisnp) > self.threshold: up = rdisnp < self.maxdis lo = rdisnp > 0 if len(rdisnp[up & lo]) == 0: rdis = 0 else: rdis = np.mean(rdisnp[up & lo]) if sum(ldisnp < self.maxdis) / len(ldisnp) > self.threshold: up = ldisnp < self.maxdis lo = ldisnp > 0 if len(ldisnp[up & lo]) == 0: ldis = 0 else: ldis = np.mean(ldisnp[up & lo]) if rdis == 0 or rdis >= self.maxdis: # out of range is often 0 self.rlastdis = rdis if ldis == 0 or ldis >= self.maxdis: # out of range is often 0 self.llastdis = ldis if rdis <= self.mindis and rdis > 0: # run fast when the object is too close self.lasttime = time self.llastdis = ldis self.rlastdis = rdis return 1 if ldis <= self.mindis and ldis > 0: # run fast when the object is too close self.lasttime = time self.llastdis = ldis self.rlastdis = rdis return 1 rrspeed = (rdis - self.rlastdis) / (time - self.lasttime) lrspeed = (ldis - self.llastdis) / (time - self.lasttime) self.rthrottle_change = rrspeed / (self.mindis - rdis) * 110 self.lthrottle_change = lrspeed / (self.mindis - ldis) * 110 self.lasttime = time self.rlastdis = rdis self.llastdis = ldis # compare throttle change from two sides and select the larger one if self.rthrottle_change >= self.lthrottle_change and self.rthrottle_change > 0: if self.rthrottle_change > 0.1: self.throttle_change = self.rthrottle_change*1.5 return self.gett(throttle) elif self.lthrottle_change > self.rthrottle_change and self.lthrottle_change > 0: if self.lthrottle_change > 0.1: self.throttle_change = self.lthrottle_change*1.5 return self.gett(throttle) else: self.rthrottle_change = 0 self.lthrottle_change = 0 self.throttle_change = 0 return self.gett(throttle)
gett() here is still adding the change to throttle and capped by 1.
- Donkey Parts
- How Donkey works
- We connect the Lidar and ToF(Arduino) using serial ports. We need to add our user to a group that has access to serial ports.
sudo usermod -a -G dialout jetson
- Behavior training
Race Track Autonomous
Front End Collision
Rear End Collision
After receiving the lane change command from the joystick, the car changes its state to the right lane and drives into the right lane.
Lane Change Protection
When the car detects an object close to its side, it will not turn further in that direction.
Side Collision Prevention
The car detects an object approaching from the right side then accelerates to avoid collision. We can still see the lag caused by Lidar here.
Over the course of the quarter, we faced many challenges. One of the biggest challenges that we faced during this project was adding the LiDAR and TOF sensor to the donkey framework. This is due to the fact that the donkey library is very long and complex; however, we were able to accomplish this after getting help from TA's/outside resources. Likewise, there were some challenges that we could unfortunately not figure out. This includes the latency for the LiDAR to transfer data to the jetson via serial. What this means is that when we were testing the rear-end collision, there was a significant lag time between unsafe approach and the response of the car. The sampling was around 8Hz and the data transfer was around 2Hz. Furthermore, our lane changing method lacked the fine-tuning it needed. We had pushed off training, and in the final days of our project, the weather was rainy and we were not able to receive the results we wanted.
- Currently, we are using the Lidar mounted at the back of the car to detect objects on both sides of the car. As the shape of the car is long, the detection range can only cover part of the sides of the car. Adding more than one ToF sensor or add another Lidar in the front of the car can provide better all-direction collision prevention.
- We select to use simple control systems to provide the collision prevention in this project. We believe collision prevention can also be achieved by using machine learning methods. How to design the models and how to train it should be interesting to work on.
- Our ultimate goal of this project is to prevent potential collisions from all directions given more complicated situations. For example, when there is an obstacle in the front and there is a car approaching fast from behind, our car is able to change to a safe lane to avoid the collision.