Posted in Self-Driving Car

Trajectory Generation

Hybrid A* Pseudocode:

The pseudocode below outlines an implementation of the A* search algorithm using the bicycle model. The following variables and objects are used in the code but not defined there:

  • State(x, y, theta, g, f): An object which stores xy coordinates, direction theta, and current g and f values.
  • grid: A 2D array of 0s and 1s indicating the area to be searched. 1s correspond to obstacles, and 0s correspond to free space.
  • SPEED: The speed of the vehicle used in the bicycle model.
  • LENGTH: The length of the vehicle used in the bicycle model.
  • NUM_THETA_CELLS: The number of cells a circle is divided into. This is used in keeping track of which States we have visited already.

The bulk of the hybrid A* algorithm is contained within the search function. The expand function takes a state and goal as inputs and returns a list of possible next states for a range of steering angles. This function contains the implementation of the bicycle model and the call to the A* heuristic function.

def expand(state, goal):
    next_states = []
    for delta in range(-35, 40, 5): 
        # Create a trajectory with delta as the steering angle using 
        # the bicycle model:

        # ---Begin bicycle model---
        delta_rad = deg_to_rad(delta)
        omega = SPEED/LENGTH * tan(delta_rad)
        next_x = state.x + SPEED * cos(theta)
        next_y = state.y + SPEED * sin(theta)
        next_theta = normalize(state.theta + omega)
        # ---End bicycle model-----

        next_g = state.g + 1
        next_f = next_g + heuristic(next_x, next_y, goal)

        # Create a new State object with all of the "next" values.
        state = State(next_x, next_y, next_theta, next_g, next_f)
        next_states.append(state)

    return next_states

def search(grid, start, goal):
    # The opened array keeps track of the stack of States objects we are 
    # searching through.
    opened = []
    # 3D array of zeros with dimensions:
    # (NUM_THETA_CELLS, grid x size, grid y size).
    closed = [[[0 for x in range(grid[0])] for y in range(len(grid))] 
        for cell in range(NUM_THETA_CELLS)]
    # 3D array with same dimensions. Will be filled with State() objects 
    # to keep track of the path through the grid. 
    came_from = [[[0 for x in range(grid[0])] for y in range(len(grid))] 
        for cell in range(NUM_THETA_CELLS)]

    # Create new state object to start the search with.
    x = start.x
    y = start.y
    theta = start.theta
    g = 0
    f = heuristic(start.x, start.y, goal)
    state = State(x, y, theta, 0, f)
    opened.append(state)

    # The range from 0 to 2pi has been discretized into NUM_THETA_CELLS cells. 
    # Here, theta_to_stack_number returns the cell that theta belongs to. 
    # Smaller thetas (close to 0 when normalized  into the range from 0 to 
    # 2pi) have lower stack numbers, and larger thetas (close to 2pi when 
    # normalized) have larger stack numbers.
    stack_num = theta_to_stack_number(state.theta)
    closed[stack_num][index(state.x)][index(state.y)] = 1

    # Store our starting state. For other states, we will store the previous 
    # state in the path, but the starting state has no previous.
    came_from[stack_num][index(state.x)][index(state.y)] = state

    # While there are still states to explore:
    while opened:
        # Sort the states by f-value and start search using the state with the 
        # lowest f-value. This is crucial to the A* algorithm; the f-value 
        # improves search efficiency by indicating where to look first.
        opened.sort(key=lambda state:state.f)
        current = opened.pop(0)

        # Check if the x and y coordinates are in the same grid cell 
        # as the goal. (Note: The idx function returns the grid index for 
        # a given coordinate.)
        if (idx(current.x) == goal[0]) and (idx(current.y) == goal.y):
            # If so, the trajectory has reached the goal.
            return path

        # Otherwise, expand the current state to get a list of possible 
        # next states.
        next_states = expand(current, goal)
        for next_s in next_states:
            # If we have expanded outside the grid, skip this next_s.
            if next_s is not in the grid:
                continue
            # Otherwise, check that we haven't already visited this cell and
            # that there is not an obstacle in the grid there.
            stack_num = theta_to_stack_number(next_s.theta)
            if closed[stack_num][idx(next_s.x)][idx(next_s.y)] == 0 
                and grid[idx(next_s.x)][idx(next_s.y)] == 0:
                # The state can be added to the opened stack.
                opened.append(next_s)
                # The stack_number, idx(next_s.x), idx(next_s.y) tuple 
                # has now been visited, so it can be closed.
                closed[stack_num][idx(next_s.x)][idx(next_s.y)] = 1
                # The next_s came from the current state, and is recorded.
                came_from[stack_num][idx(next_s.x)][idx(next_s.y)] = current

Now we go to next step:

Implementing Hybrid A*

In this exercise, you will be provided a working implementation of a breadth first search algorithm which does not use any heuristics to improve its efficiency. Your goal is to try to make the appropriate modifications to the algorithm so that it takes advantage of heuristic functions (possibly the ones mentioned in the previous paper) to reduce the number of grid cell expansions required.

Instructions:

  1. Modify the code in ‘hybrid_breadth_first.cpp’ and hit Test Run to check your results.
  2. Note the number of expansions required to solve an empty 15×15 grid (it should be about 18,000!). Modify the code to try to reduce that number. How small can you get it?

Solution:

#include <iostream>
#include <vector>
#include "hybrid_breadth_first.h"

using std::cout;
using std::endl;

// Sets up maze grid
int X = 1;
int _ = 0;

/**
 * TODO: You can change up the grid maze to test different expansions.
 */
vector<vector<int>> GRID = {
  {_,X,X,_,_,_,_,_,_,_,X,X,_,_,_,_,},
  {_,X,X,_,_,_,_,_,_,X,X,_,_,_,_,_,},
  {_,X,X,_,_,_,_,_,X,X,_,_,_,_,_,_,},
  {_,X,X,_,_,_,_,X,X,_,_,_,X,X,X,_,},
  {_,X,X,_,_,_,X,X,_,_,_,X,X,X,_,_,},
  {_,X,X,_,_,X,X,_,_,_,X,X,X,_,_,_,},
  {_,X,X,_,X,X,_,_,_,X,X,X,_,_,_,_,},
  {_,X,X,X,X,_,_,_,X,X,X,_,_,_,_,_,},
  {_,X,X,X,_,_,_,X,X,X,_,_,_,_,_,_,},
  {_,X,X,_,_,_,X,X,X,_,_,X,X,X,X,X,},
  {_,X,_,_,_,X,X,X,_,_,X,X,X,X,X,X,},
  {_,_,_,_,X,X,X,_,_,X,X,X,X,X,X,X,},
  {_,_,_,X,X,X,_,_,X,X,X,X,X,X,X,X,},
  {_,_,X,X,X,_,_,X,X,X,X,X,X,X,X,X,},
  {_,X,X,X,_,_,_,_,_,_,_,_,_,_,_,_,},
  {X,X,X,_,_,_,_,_,_,_,_,_,_,_,_,_,}};

vector<double> START = {0.0,0.0,0.0};
vector<int> GOAL = {(int)GRID.size()-1, (int)GRID[0].size()-1};

int main() {
  cout << "Finding path through grid:" << endl;
  
  // Creates an Empty Maze and for testing the number of expansions with it
  for(int i = 0; i < GRID.size(); ++i) {
    cout << GRID[i][0];
    for(int j = 1; j < GRID[0].size(); ++j) {
      cout << "," << GRID[i][j];
    }
    cout << endl;
  }

  HBF hbf = HBF();

  HBF::maze_path get_path = hbf.search(GRID,START,GOAL);

  vector<HBF::maze_s> show_path = hbf.reconstruct_path(get_path.came_from, 
                                                       START, get_path.final);

  cout << "show path from start to finish" << endl;
  for(int i = show_path.size()-1; i >= 0; --i) {
      HBF::maze_s step = show_path[i];
      cout << "##### step " << step.g << " #####" << endl;
      cout << "x " << step.x << endl;
      cout << "y " << step.y << endl;
      cout << "theta " << step.theta << endl;
  }
  
  return 0;
}
Posted in Self-Driving Car

Program an Autonomous Vehicle

Thiết lập dự án Dự án sẽ yêu cầu sử dụng Ubuntu Linux (hệ điều hành của Carla) và một trình mô phỏng mới. Để giảm bớt khó khăn khi cài đặt, chúng tôi đã cung cấp Không gian làm việc trong trình duyệt để bạn làm việc. Bạn có thể tìm thấy hướng dẫn cho Workspace và chính Workspace sau trong bài học này. Nếu bạn không muốn sử dụng Không gian làm việc, hãy làm theo các bước bên dưới để thiết lập: Bởi vì ROS được sử dụng, bạn sẽ cần sử dụng Ubuntu để phát triển và kiểm tra mã dự án của mình. Bạn có thể sử dụng Ubuntu 14.04 với ROS Indigo Ubuntu 16.04 với ROS Kinetic Bạn có thể sử dụng cài đặt Ubuntu hoặc máy ảo của riêng mình (không được hỗ trợ) hoặc bạn có thể sử dụng VM được cung cấp trong Máy ảo của bạn trong bài học “Giới thiệu về ROS”. Máy ảo được cung cấp đã cài đặt sẵn ROS và Dataspeed DBW. Người dùng Windows 10 – những sinh viên đồng nghiệp của bạn đã gợi ý rằng lựa chọn cục bộ tốt nhất là sử dụng VM cho ROS, trong khi chạy trình mô phỏng nguyên bản (và đảm bảo mở các cổng giữa hai để giao tiếp). Bạn có thể tìm thấy repo của dự án tại đây. Sao chép hoặc tải xuống mã dự án trước các phần tiếp theo để bạn có thể theo dõi cùng với các mô tả mã! Trong README, bạn sẽ có thể tìm thấy bất kỳ phụ thuộc bổ sung nào cần thiết cho dự án. Dự án tích hợp hệ thống sử dụng trình mô phỏng của riêng nó sẽ giao diện với mã ROS của bạn và có tính năng phát hiện đèn giao thông. Bạn có thể tải xuống trình mô phỏng tại đây. Để cải thiện hiệu suất khi sử dụng máy ảo, chúng tôi khuyên bạn nên tải xuống trình mô phỏng cho hệ điều hành máy chủ của bạn và sử dụng trình mô phỏng này bên ngoài máy ảo. Bạn sẽ có thể chạy mã dự án trong máy ảo trong khi chạy trình mô phỏng nguyên bản trong máy chủ sử dụng chuyển tiếp cổng trên cổng 4567. Để biết thêm thông tin về cách thiết lập chuyển tiếp cổng, hãy xem phần cuối của khái niệm lớp học tại đây. Điểm đánh giá cho dự án này khá đơn giản – chiếc xe có điều hướng thành công đường đua không? Nếu bạn đang sử dụng phiên bản ba kỳ hạn, hãy kiểm tra phiếu tự đánh giá tại đây hoặc đối với phiên bản hai kỳ hạn, hãy xem phiếu đánh giá tại đây.

Posted in Self-Driving Car

Implement Practical Filter

Particle Filter Algorithm Steps and Inputs

The flowchart below represents the steps of the particle filter algorithm as well as its inputs.

Particle Filter Algorithm Flowchart

Psuedo Code

This is an outline of steps you will need to take with your code in order to implement a particle filter for localizing an autonomous vehicle. The pseudo code steps correspond to the steps in the algorithm flow chart, initialization, prediction, particle weight updates, and resampling. Python implementation of these steps was covered in the previous lesson.

Initialization

At the initialization step we estimate our position from GPS input. The subsequent steps in the process will refine this estimate to localize our vehicle.

Prediction

During the prediction step we add the control input (yaw rate & velocity) for all particles

Update

During the update step, we update our particle weights using map landmark positions and feature measurements.

Resampling

During resampling we will resample M times (M is range of 0 to length_of_particleArray) drawing a particle i (i is the particle index) proportional to its weight . Sebastian covered one implementation of this in his discussion and implementation of a resampling wheel.

Return New Particle Set

The new set of particles represents the Bayes filter posterior probability. We now have a refined estimate of the vehicles position based on input evidence.

Posted in Self-Driving Car

Transfer Learning

The Four Main Cases When Using Transfer Learning

Transfer learning involves taking a pre-trained neural network and adapting the neural network to a new, different data set.

Depending on both:

  • the size of the new data set, and
  • the similarity of the new data set to the original data set

the approach for using transfer learning will be different. There are four main cases:

  1. new data set is small, new data is similar to original training data
  2. new data set is small, new data is different from original training data
  3. new data set is large, new data is similar to original training data
  4. new data set is large, new data is different from original training data

Four Cases When Using Transfer Learning

A large data set might have one million images. A small data could have two-thousand images. The dividing line between a large data set and small data set is somewhat subjective. Overfitting is a concern when using transfer learning with a small data set.

Images of dogs and images of wolves would be considered similar; the images would share common characteristics. A data set of flower images would be different from a data set of dog images.

Each of the four transfer learning cases has its own approach. In the following sections, we will look at each case one by one.

Demonstration Network

To explain how each situation works, we will start with a generic pre-trained convolutional neural network and explain how to adjust the network for each case. Our example network contains three convolutional layers and three fully connected layers:

General Overview of a Neural Network

Here is an generalized overview of what the convolutional neural network does:

  • the first layer will detect edges in the image
  • the second layer will detect shapes
  • the third convolutional layer detects higher level features

Each transfer learning case will use the pre-trained convolutional neural network in a different way.

Case 1: Small Data Set, Similar Data

Case 1: Small Data Set with Similar Data

If the new data set is small and similar to the original training data:

  • slice off the end of the neural network
  • add a new fully connected layer that matches the number of classes in the new data set
  • randomize the weights of the new fully connected layer; freeze all the weights from the pre-trained network
  • train the network to update the weights of the new fully connected layer

To avoid overfitting on the small data set, the weights of the original network will be held constant rather than re-training the weights.

Since the data sets are similar, images from each data set will have similar higher level features. Therefore most or all of the pre-trained neural network layers already contain relevant information about the new data set and should be kept.

Here’s how to visualize this approach:

Neural Network with Small Data Set, Similar Data

Case 2: Small Data Set, Different Data

Case 2: Small Data Set, Different Data

If the new data set is small and different from the original training data:

  • slice off most of the pre-trained layers near the beginning of the network
  • add to the remaining pre-trained layers a new fully connected layer that matches the number of classes in the new data set
  • randomize the weights of the new fully connected layer; freeze all the weights from the pre-trained network
  • train the network to update the weights of the new fully connected layer

Because the data set is small, overfitting is still a concern. To combat overfitting, the weights of the original neural network will be held constant, like in the first case.

But the original training set and the new data set do not share higher level features. In this case, the new network will only use the layers containing lower level features.

Here is how to visualize this approach:

Neural Network with Small Data Set, Different Data

Case 3: Large Data Set, Similar Data

Case 3: Large Data Set, Similar Data

If the new data set is large and similar to the original training data:

  • remove the last fully connected layer and replace with a layer matching the number of classes in the new data set
  • randomly initialize the weights in the new fully connected layer
  • initialize the rest of the weights using the pre-trained weights
  • re-train the entire neural network

Overfitting is not as much of a concern when training on a large data set; therefore, you can re-train all of the weights.

Because the original training set and the new data set share higher level features, the entire neural network is used as well.

Here is how to visualize this approach:

Neural Network with Large Data Set, Similar Data

Case 4: Large Data Set, Different Data

Case 4: Large Data Set, Different Data

If the new data set is large and different from the original training data:

  • remove the last fully connected layer and replace with a layer matching the number of classes in the new data set
  • retrain the network from scratch with randomly initialized weights
  • alternatively, you could just use the same strategy as the “large and similar” data case

Even though the data set is different from the training data, initializing the weights from the pre-trained network might make training faster. So this case is exactly the same as the case with a large, similar data set.

If using the pre-trained network as a starting point does not produce a successful model, another option is to randomly initialize the convolutional neural network weights and train the network from scratch.

Here is how to visualize this approach:

Neural Network with Large Data Set, Different Data