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.