Artificial Intelligence for Robotics

Here is some of my notes and thoughts from the following course: Artificial Intelligence for Robotics – Programming a robotic car – Learn how to program self-driving cars. The reason I am writing about the subject is to get a better understanding of the things I learn in the course. I also think it’s great to be able to go back and read up the concepts later on if there’s something I forgot or do not fully understand.

Lesson 1 – Localization

The first lesson is about localization of self-driving cars and how to solve that problem. A self-driving car needs to know where it is in the environment, so it can drive safely without harming itself and the environment. We can take a car driving on a highway as an example. The car wants to know where it is on the highway. Is it inside the lane or is it crossing lane markers on the highway? A standard way to solve this problem is using satellites that emit signals to the car. That is know as GPS(global positioning system). The problem with GPS is that it is not so accurate. In GPS there can be from 2-10 meters of localization error and that can cause big problems for the car. The car will eventually drive of the lane and crash into something. It would be much better if the car has something with 2-10 centimeters of error. Then the car would have a much better chance staying on the highway. The google self driving car solves the localization problem with use of images of the road surface and techniques that includes some math to predict where it is on the road. That way the car can stay on to road with few centimeters of error even when to line markers are missing. Thats pretty amazing right.

probability

Probability
Another big part of self-driving cars is probability which is the measure of the likeliness that an event will occur. The course gives an example of Total probability and the example starts with that there is a robot, who have no clue where it is. At the top you see a function with probability for any location in this world on the y axis and all the locations in this world on the x axis. The robots current belief where it might be is confusion that assigns equal weight to every location. That is the red line on the picture also called Maximum confusion. On the top of the picture there is 3 doors. The measurement of a door transforms the belief function defined over possible locations to a new function. That is the posterior belief. Posterior means after a measurement have been taken. The 3 reds bumps you see is the current best belief of where the robot are. Then the robot moves to the right a certain distance. Then the Posterior beliefs shifts right according to the motion. That is called convolution. Then the robot senses a door again and it’s here the magic happens. The belief gets multiplied with the prior function and then the belief gets higher. That is the last function you see on the picture with a high belief peak. At this point the robot has localized itself. I recommend you to watch this about Total Probability if you want the learn more.

My answers for some of the python Programming exercises in lesson 1 – Localization

Lesson 2 – Kalman Filters

When a self-driving car is driving on a road it needs to detect other cars so that it does not drive into them. Radar and laser sensors are great at tracking things and are therefore used on self-driving cars to detect distance and velocity of other cars on the road. These two sensors gives a lot of data to the self-driving car about the environment and here are Kalman Filters used to make sense of all this data. It estimates a continuous state and gives a uni-modal distribution. Gaussians are a big part of Kalman Filters because the distribution is given in Gaussains. A Gaussian is a continuous function over space of locations. There a lot of exercises with Gaussians in Lesson 2.

Kalman-Filters

Lesson 3 – Particle Filters

Particle Filters estimates a continuous state and gives a multimodal distribution and is really easy to program compared to Histogram Filters, Kalman Filters. This video shows a good example of Particle Filters and explains how it works.

[youtube id=”H0G1yslM5rc” width=”781″ height=”350″ autoplay=”no” api_params=”” class=””]

Lesson 4 – Search

Lesson 4 is about motion planning for self-driving cars. It’s about how self-driving cars plan it’s motions to get from A to B in the most optimal way. An example could a self-driving car driving from San Francisco to Los Angeles. There are a lot of different roads to take, and the motion planning helps the self-driving car to find a optimal route. A* search algorithm and Dynamic Programming are used in Lesson 4 to fix the route planning problem. A* search algorithm calculates the shortest path and uses a heuristic function which is some extra information that helps the algorithm search in the right direction. Dynamic programming in route planning is explained very well in this video.

[youtube id=”r2bPY2s9wII” width=”781″ height=”350″ autoplay=”no” api_params=”” class=””]

Lesson 5 – PID Control

Lesson 5 is about Robot motion and it goes in dept with generating a smooth path and PID control.

Lesson 6 SLAM

Lesson 6 is about Simultaneous localization and mapping and that goes in dept with building maps of the environment.

Programming exercises in lesson 1 – Localization

#Modify the empty list, p, so that it becomes a UNIFORM probability
#distribution over five grid cells, as expressed in a list of 
#five probabilities.

p=[0.2, 0.2, 0.2, 0.2, 0.2]

print p

Output: [0.2, 0.2, 0.2, 0.2, 0.2]
#Modify your code to create probability vectors, p, of arbitrary 
#size, n. Use n=5 to verify that your new solution matches 
#the previous one.

p=[]
n=5
for i in range(n):
    p.append(1./n)
print p

Output: [0.2, 0.2, 0.2, 0.2, 0.2]
#Write code that outputs p after multiplying each entry 
#by pHit or pMiss at the appropriate places. Remember that
#the red cells 1 and 2 are hits and the other green cells
#are misses.

p=[0.2,0.2,0.2,0.2,0.2]
pHit = 0.6
pMiss = 0.2

for i in xrange(len(p)):
    if i==1 or i==2:
        p[i] = p[i] *pHit
    else:
        p[i] = p[i] * pMiss

print p

Output: [0.04000000000000001, 0.12, 0.12, 0.04000000000000001, 0.04000000000000001]
#Modify the code below so that the function sense, which 
#takes p and Z as inputs, will output the NON-normalized 
#probability distribution, q, after multiplying the entries 
#in p by pHit or pMiss according to the color in the 
#corresponding cell in world.

p=[0.2, 0.2, 0.2, 0.2, 0.2]
world=['green', 'red', 'red', 'green', 'green']
Z = 'red'
pHit = 0.6
pMiss = 0.2

def sense(p, Z):
    q=[]
    for i in range(len(p)):
        if Z==world[i]:
           q.append(pHit*p[i])
        else:
            q.append(pMiss*p[i])
    return q

print sense(p,Z)

Output: [0.04000000000000001, 0.12, 0.12, 0.04000000000000001, 0.04000000000000001]
#Modify your code so that it normalizes the output for 
#the function sense. This means that the entries in q 
#should sum to one.

p=[0.2, 0.2, 0.2, 0.2, 0.2]
world=['green', 'red', 'red', 'green', 'green']
Z = 'red'
pHit = 0.6
pMiss = 0.2

def sense(p, Z):
    q=[]
    for i in range(len(p)):
        if Z==world[i]:
           q.append(pHit*p[i])
        else:
            q.append(pMiss*p[i])
    s = sum(q)
    for i in range(len(p)):
        q[i] = q[i] / s
    return q
print sense(p,Z)

Output: [0.1111111111111111, 0.3333333333333332, 0.3333333333333332, 0.1111111111111111, 0.1111111111111111]
#Modify the code so that it updates the probability twice
#and gives the posterior distribution after both 
#measurements are incorporated. Make sure that your code 
#allows for any sequence of measurement of any length.

p=[0.2, 0.2, 0.2, 0.2, 0.2]
world=['green', 'red', 'red', 'green', 'green']
measurements = ['red', 'green']
pHit = 0.6
pMiss = 0.2

def sense(p, Z):
    q=[]
    for i in range(len(p)):
        if Z==world[i]:
           q.append(pHit*p[i])
        else:
            q.append(pMiss*p[i])
    s = sum(q)
    for i in range(len(q)):
        q[i] = q[i] / s
    return q
for i in measurements:
    p=sense(p,i)
print p

Output: [0.20000000000000004, 0.19999999999999996, 0.19999999999999996, 0.20000000000000004, 0.20000000000000004]
#Program a function that returns a new distribution 
#q, shifted to the right by U units. If U=0, q should 
#be the same as p.

p=[0, 1, 0, 0, 0]
world=['green', 'red', 'red', 'green', 'green']
measurements = ['red', 'green']
pHit = 0.6
pMiss = 0.2

def sense(p, Z):
    q=[]
    for i in range(len(p)):
        if Z==world[i]:
           q.append(pHit*p[i])
        else:
            q.append(pMiss*p[i])
    s = sum(q)
    for i in range(len(q)):
        q[i] = q[i] / s
    return q

def move(p, U):
    q = []
    for i in range(len(p)):
        q.append(p[(i-U) % len(p)])
    return q

print move(p, 0)
print move(p, 1)
print move(p, 2)
print move(p, -1)

Output: 
[0, 1, 0, 0, 0]
[0, 0, 1, 0, 0]
[0, 0, 0, 1, 0]
[1, 0, 0, 0, 0]
#Modify the move function to accommodate the added 
#probabilities of overshooting or undershooting 
#the intended destination.

p=[0, 1, 0, 0, 0]
world=['green', 'red', 'red', 'green', 'green']
measurements = ['red', 'green']
pHit = 0.6
pMiss = 0.2
pExact = 0.8
pOvershoot = 0.1
pUndershoot = 0.1

def sense(p, Z):
    q=[]
    for i in range(len(p)):
        if Z==world[i]:
           q.append(pHit*p[i])
        else:
            q.append(pMiss*p[i])
    s = sum(q)
    for i in range(len(q)):
        q[i] = q[i] / s
    return q

def move(p, U):
    q = []
    for i in range(len(p)):
        s = pExact * p[(i-U)%len(p)]
        s = s + pOvershoot * p[(i-U-1) % len(p)]
        s = s + pUndershoot * p[(i-U+1) % len(p)]
        q.append(s)
    return q
    
print move(p, 1)

Output: [0.0, 0.1, 0.8, 0.1, 0.0]
#Modify the previous code so that the robot senses red twice.
#This code is the essence of google's self driving car localization approach

p=[0.2, 0.2, 0.2, 0.2, 0.2]
world=['green', 'red', 'red', 'green', 'green']
measurements = ['red', 'red']
motions = [1,1]
pHit = 0.6
pMiss = 0.2
pExact = 0.8
pOvershoot = 0.1
pUndershoot = 0.1

def sense(p, Z):
    q=[]
    for i in range(len(p)):
        if Z==world[i]:
           q.append(pHit*p[i])
        else:
            q.append(pMiss*p[i])
    s = sum(q)
    for i in range(len(q)):
        q[i] = q[i] / s
    return q

def move(p, U):
    q = []
    for i in range(len(p)):
        s = pExact * p[(i-U) % len(p)]
        s = s + pOvershoot * p[(i-U-1) % len(p)]
        s = s + pUndershoot * p[(i-U+1) % len(p)]
        q.append(s)
    return q

for k in range(len(measurements)):
    p = sense(p, measurements[k])
    p = move(p, motions[k])
    
print p         

Output: [0.07882352941176471, 0.07529411764705884, 0.22470588235294123, 0.4329411764705882, 0.18823529411764706]

Leave a Reply