课程 19：Lesson 6 – SLAM

Lesson 6 – SLAM

1.   Putting It All Together

2.   练习：Localization

3.   练习：Planning

算法名	是否在连续空间	是否会取得最优解（找到目标）	是否是通用性算法	是否仅是局部性算法
广度优先搜索	否	是	否	否
A*	否	是	否	否
动态规划	否	是	是	否
平滑(Smoothing)	是	否	否	是

 

4.   练习：PID

下表是对PID控制器中的子项及其作用的描述：

 

 

QUIZ	子项	避免超调	最小化误差	补偿漂移
PID	P	否	是	否
	I	否	否	是
	D	是	否	否

 

5.   Your Robot Car

 

6.   练习：Segmented CTE

一个编程的例子，主要是写控制部分，如何根据粒子提供的预测路径（由线段组成），来计算出横切距离（CTE）。

视频提供了个相关的计算公式：

u大于1时，表明已经进入了下一个线段的计算范围。这上面的图片是视频讲解图片，遗憾的是，又写错了。。。。汗

在网页的视频的下方写道：Note: theformula for CTE should actually take the square root of the denominator, i.e.(Ry dx - Rx dy) /sqrt(dx^2 + dy^2)

就是说 CTE = (Ry* dx – Rx* dy) /sqrt(dx^2 + dy^2)

视频代码的答案中，给出的计算cte，没有做sqrt操作。

对于u和cte的计算公式怎么来的，这里我没有去深究。

代码：

# -*- coding: utf-8 -*-

 

# -----------

# User Instructions

#

# Familiarize yourself with the code below. Most of it

# reproduces results that you have obtained at some

# point in this class. Once you understand the code,

# write a function, cte, in the run class that

# computes the crosstrack

# error for the case of a segmented path. You will

# need to include the equations shown in the video.

#

 

from math import *

import random

 

 

# don't change the noise paameters

 

steering_noise    = 0.1

distance_noise    = 0.03

measurement_noise = 0.3

 

 

class plan:

 

    # --------

    # init:

    #    creates an empty plan

    #

 

    def __init__(self, grid,init, goal, cost = 1):

        self.cost = cost

        self.grid = grid

        self.init = init

        self.goal = goal

       self.make_heuristic(grid, goal, self.cost)

        self.path = []

        self.spath = []

 

    # --------

    #

    # make heuristic functionfor a grid

 

    def make_heuristic(self,grid, goal, cost):

        self.heuristic = [[0for row in range(len(grid[0]))]

                          forcol in range(len(grid))]

        for i inrange(len(self.grid)):

            for j inrange(len(self.grid[0])):

               self.heuristic[i][j] = abs(i - self.goal[0]) + \

                    abs(j -self.goal[1])

 

 

 

    #------------------------------------------------

    #

    # A* for searching a pathto the goal

    #

    #

 

    def astar(self):

 

 

        if self.heuristic ==[]:

            raise(ValueError,"Heuristic must be defined to run A*")

 

        # internal motionparameters

        delta = [[-1,  0], # go up

                 [ 0,  -1], # go left

                 [ 1,  0], # go down

                 [ 0,  1]] # do right

 

 

        # open list elementsare of the type: [f, g, h, x, y]

 

        closed = [[0 for rowin range(len(self.grid[0]))]

                  for col inrange(len(self.grid))]

        action = [[0 for rowin range(len(self.grid[0]))]

                  for col inrange(len(self.grid))]

 

       closed[self.init[0]][self.init[1]] = 1

 

 

        x = self.init[0]

        y = self.init[1]

        h =self.heuristic[x][y]

        g = 0

        f = g + h

 

        open = [[f, g, h, x,y]]

 

        found  = False # flag that is set when searchcomplete

        resign = False # flagset if we can't find expand

        count  = 0

 

        while not found andnot resign:

 

            # check if westill have elements on the open list

            if len(open) == 0:

                resign = True

                print('######Search terminated without success')

 

            else:

                # remove nodefrom list

                open.sort()

                open.reverse()

                next =open.pop()

                x = next[3]

                y = next[4]

                g = next[1]

 

            # check if we aredone

 

            if x == goal[0]and y == goal[1]:

                found = True

                # print'###### A* search successful'

 

            else:

                # expandwinning element and add to new open list

                for i inrange(len(delta)):

                    x2 = x +delta[i][0]

                    y2 = y +delta[i][1]

                    if x2>= 0 and x2 < len(self.grid) and y2 >= 0 \

                           and y2 < len(self.grid[0]):

                        ifclosed[x2][y2] == 0 and self.grid[x2][y2] == 0:

                            g2= g + self.cost

                            h2= self.heuristic[x2][y2]

                            f2= g2 + h2

                           open.append([f2, g2, h2, x2, y2])

                           closed[x2][y2] = 1

                           action[x2][y2] = i

 

            count += 1

 

        # extract the path

 

 

 

        invpath = []

        x = self.goal[0]

        y = self.goal[1]

        invpath.append([x, y])

        while x !=self.init[0] or y != self.init[1]:

            x2 = x -delta[action[x][y]][0]

            y2 = y -delta[action[x][y]][1]

            x = x2

            y = y2

            invpath.append([x,y])

 

        self.path = []

        for i inrange(len(invpath)):

           self.path.append(invpath[len(invpath) - 1 - i])

 

 

 

 

    #------------------------------------------------

    #

    # this is the smoothingfunction

    #

 

 

 

 

    def smooth(self,weight_data = 0.1, weight_smooth = 0.1,

               tolerance =0.000001):

 

        if self.path == []:

            raise(ValueError,"Run A* first before smoothing path")

 

        self.spath = [[0 forrow in range(len(self.path[0]))] \

                           forcol in range(len(self.path))]

        for i inrange(len(self.path)):

            for j inrange(len(self.path[0])):

               self.spath[i][j] = self.path[i][j]

 

        change = tolerance

        while change >=tolerance:

            change = 0.0

            for i in range(1,len(self.path)-1):

                for j inrange(len(self.path[0])):

                    aux =self.spath[i][j]

 

                   self.spath[i][j] += weight_data * \

                       (self.path[i][j] - self.spath[i][j])

 

                   self.spath[i][j] += weight_smooth * \

                       (self.spath[i-1][j] + self.spath[i+1][j]

                         - (2.0 * self.spath[i][j]))

                    if i >=2:

                       self.spath[i][j] += 0.5 * weight_smooth * \

                           (2.0 * self.spath[i-1][j] - self.spath[i-2][j]

                             -self.spath[i][j])

                    if i <=len(self.path) - 3:

                       self.spath[i][j] += 0.5 * weight_smooth * \

                           (2.0 * self.spath[i+1][j] - self.spath[i+2][j]

                             -self.spath[i][j])

 

            change += abs(aux - self.spath[i][j])

 

 

 

 

 

 

 

# ------------------------------------------------

#

# this is the robot class

#

 

class robot:

 

    # --------

    # init:

    #    creates robot and initializes location/orientation to 0, 0, 0

    #

 

    def __init__(self, length= 0.5):

        self.x = 0.0

        self.y = 0.0

        self.orientation = 0.0

        self.length = length

       self.steering_noise    = 0.0

       self.distance_noise    = 0.0

        self.measurement_noise= 0.0

        self.num_collisions    = 0

        self.num_steps         = 0

 

    # --------

    # set:

    #    sets a robot coordinate

    #

 

    def set(self, new_x,new_y, new_orientation):

 

        self.x = float(new_x)

        self.y = float(new_y)

        self.orientation =float(new_orientation) % (2.0 * pi)

 

 

    # --------

    # set_noise:

    #    sets the noise parameters

    #

 

    def set_noise(self,new_s_noise, new_d_noise, new_m_noise):

        # makes it possible tochange the noise parameters

        # this is often usefulin particle filters

       self.steering_noise     =float(new_s_noise)

       self.distance_noise    =float(new_d_noise)

        self.measurement_noise= float(new_m_noise)

 

    # --------

    # check:

    #    checks of the robot pose collides with anobstacle, or

    # is too far outside theplane

 

    def check_collision(self,grid):

        for i inrange(len(grid)):

            for j inrange(len(grid[0])):

                if grid[i][j]== 1:

                    dist =sqrt((self.x - float(i)) ** 2 +

                               (self.y - float(j)) ** 2)

                    if dist< 0.5:

                       self.num_collisions += 1

                        returnFalse

        return True

 

    def check_goal(self, goal,threshold = 1.0):

        dist =  sqrt((float(goal[0]) - self.x) ** 2 +(float(goal[1]) - self.y) ** 2)

        return dist <threshold

 

    # --------

    # move:

    #    steering = front wheel steering angle,limited by max_steering_angle

    #    distance = total distance driven, most benon-negative

 

    def move(self, grid,steering, distance,

             tolerance =0.001, max_steering_angle = pi / 4.0):

 

        if steering >max_steering_angle:

            steering =max_steering_angle

        if steering <-max_steering_angle:

            steering =-max_steering_angle

        if distance < 0.0:

            distance = 0.0

 

 

        # make a new copy

        res = robot()

        res.length            = self.length

       res.steering_noise    =self.steering_noise

       res.distance_noise    =self.distance_noise

        res.measurement_noise= self.measurement_noise

       res.num_collisions    =self.num_collisions

        res.num_steps         = self.num_steps + 1

 

        # apply noise

        steering2 =random.gauss(steering, self.steering_noise)

        distance2 =random.gauss(distance, self.distance_noise)

 

 

        # Execute motion

        turn = tan(steering2)* distance2 / res.length

 

        if abs(turn) <tolerance:

 

            # approximate bystraight line motion

 

            res.x = self.x +(distance2 * cos(self.orientation))

            res.y = self.y +(distance2 * sin(self.orientation))

            res.orientation =(self.orientation + turn) % (2.0 * pi)

 

        else:

 

            # approximatebicycle model for motion

 

            radius = distance2/ turn

            cx = self.x -(sin(self.orientation) * radius)

            cy = self.y +(cos(self.orientation) * radius)

            res.orientation =(self.orientation + turn) % (2.0 * pi)

            res.x = cx +(sin(res.orientation) * radius)

            res.y = cy -(cos(res.orientation) * radius)

 

        # check for collision

        #res.check_collision(grid)

 

        return res

 

    # --------

    # sense: