2019FallTeam1

Team Members

(From left to right) Haoru, Yuhan, Cheyenne

• Haoru Xue - Electrical Engineering
• Yuhan Zhang - Electrical Engineering
• Cheyenne Herrera - Math/Engineering

Project Objectives

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.

Mechanical Design

Electronic Design

Components

The Lidar is mounted at the back of the car

• 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)
• Battery
• USB camera
• Arduino (for connecting ToF sensor)
• Time-of-flight sensor (ToF)
• Lidar and USB controller

Schematic

Implementation

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.

We are using YD Lidar X4. There is a python library PyLidar3 that supports this model.

To use the PyLidar3 library

Create an instance and connect

   lidar = YdLidarX4("/dev/ttyUSB0")
   lidar.Connect()

Scan

   scans = self.lidar.StartScanning()
   for scan in scans:
       for i in range(360):
           self.scan[i] = scan[i]

Stop Lidar and disconnect

   lidar.StopScanning()
   lidar.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.

Wrong Example

1. Press the button to switch state

2. Drive a very short distance and then steer

Correct Example

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

Yellow regions are used for safe lane changing and the Blue region is for 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

The red car cannot be seen in mirrors. Source

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 = [2000]
   # 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.

How Sensors are Connected

The Sensor

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".

The Code

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.

Useful Knowledge

• Donkey Parts

https://www.youtube.com/watch?v=YZ4ESrtfShs

• How Donkey works

https://www.youtube.com/watch?v=G1JjAw_NdnE

• 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

https://docs.donkeycar.com/guide/train_autopilot/#training-behavior-models

Results

Lane Change

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.

Challenges

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.

Future Work

• 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.

References

Donkey Car Framework

PyLidar3