课程 16:Problem Set 5

课程 16:Problem Set 5

1.   练习:Missing Parameters

这个练习向我们展示了缺失PID参数时的可能效果。

a.   优化的PID参数,理论上会让轨迹向期望的轨迹逐渐收敛

b.   如果缺失P参数,则CTE表现最大,并逐渐增大。

c.   如果缺失D参数,CTE表现没有b那样,但是也会逐渐增大。

d.   如果缺失I参数,CTE也会变小,轨迹在期望轨迹附近,呈一定的误差。

2.   练习:Cyclic Smoothing

在课程15中,做平滑的时候,起点和终点,都没有动。

这回的代码说明中只要求不动终点,起点可以动。结果演示代码答案时,循环到了所有的元素,,所以循环边界变了一下。

说明,如果是环线的平滑的时候,要包括起点和终点。

代码中还有个方法solution_check(smooth(testpath1),answer1)用来检查平滑方案的优劣,说让用这个方法测试answer1是不是正确,但是没有提到这个answer1是怎么来的。

代码:

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

 

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

# User Instructions

#

# Here you will be implementing a cyclicsmoothing

# algorithm. This algorithm should not fixthe end

# points (as you did in the unit quizzes).You

# should use the gradient descentequations that

# you used previously.

#

# Your function should return the newpaththat it

# calculates.

#

# Feel free to use the providedsolution_check function

# to test your code. You can find it atthe bottom.

#

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

# Testing Instructions

#

# To test your code, call thesolution_check function with

# two arguments. The first argument shouldbe the result of your

# smooth function. The second should bethe corresponding answer.

# For example, calling

#

# solution_check(smooth(testpath1),answer1)

#

# should return True if your answer iscorrect and False if

# it is not.

 

from math import *

 

# Do not modify path inside your function.

path=[[0, 0],

     [1, 0],

     [2, 0],

     [3, 0],

     [4, 0],

     [5, 0],

     [6, 0],

     [6, 1],

     [6, 2],

     [6, 3],

     [5, 3],

     [4, 3],

     [3, 3],

     [2, 3],

     [1, 3],

     [0, 3],

     [0, 2],

     [0, 1]]

 

############# ONLY ENTER CODE BELOW THISLINE ##########

 

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

# smooth coordinates

# If your code is timing out, make thetolerance parameter

# larger to decrease run time.

#

 

def smooth(path, weight_data = 0.1,weight_smooth = 0.1, tolerance = 0.00001):

 

   #

   # Enter code here

   #

   # deepcopy path

   newpath = []

   for i in range(len(path)):

       p1 = []

       for j in range(len(path[0])):

           p1.append(path[i][j])

       newpath.append(p1)

   

   changeFactor = tolerance

   while changeFactor >= tolerance:

       changeFactor = 0

       for i in range(len(path)):

           for j in range(len(path[0])):

                aux = newpath[i][j]

 

                newpath[i][j] += weight_data *(path[i][j] - newpath[i][j]) + \

                       weight_smooth *(newpath[(i+1)%len(path)][j] + newpath[(i-1 + len(path))%len(path)][j] - 2.0 *newpath[i][j])

               

                changeFactor += abs(aux -newpath[i][j])

               

   return newpath

 

# thank you - EnTerr - for posting this onour discussion forum

 

#newpath = smooth(path)

#for i in range(len(path)):

#   print('['+ ', '.join('%.3f'%x for x in path[i]) +'] -> ['+ ','.join('%.3f'%x for x in newpath[i]) +']')

 

 

##### TESTING ######

 

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

# check if two numbers are 'closeenough,'used in

# solution_check function.

#

def close_enough(user_answer, true_answer,epsilon = 0.001):

   if abs(user_answer - true_answer) > epsilon:

       return False

   return True

 

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

# check your solution against ourreference solution for

# a variety of test cases (given below)

#

def solution_check(newpath, answer):

   if type(newpath) != type(answer):

       print("Error. You do not return a list.")

       return False

   if len(newpath) != len(answer):

       print('Error. Your newpath is not the correct length.')

       return False

   if len(newpath[0]) != len(answer[0]):

       print('Error. Your entries do not contain an (x, y) coordinate pair.')

       return False

   for i in range(len(newpath)):

       for j in range(len(newpath[0])):

           if not close_enough(newpath[i][j], answer[i][j]):

                print('Error, at least one ofyour entries is not correct.')

                return False

   print("Test case correct!")

   return True

 

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

# Testing Instructions

#

# To test your code, call thesolution_check function with

# two arguments. The first argument shouldbe the result of your

# smooth function. The second should bethe corresponding answer.

# For example, calling

#

# solution_check(smooth(testpath1),answer1)

#

# should return True if your answer iscorrect and False if

# it is not.

 

testpath1 = [[0, 0],

             [1, 0],

             [2, 0],

             [3, 0],

             [4, 0],

             [5, 0],

             [6, 0],

             [6, 1],

             [6, 2],

             [6, 3],

             [5, 3],

             [4, 3],

             [3, 3],

             [2, 3],

             [1, 3],

             [0, 3],

             [0, 2],

             [0, 1]]

 

answer1 = [[0.4705860385182691,0.4235279620576893],

          [1.1764695730296597, 0.16470408411716733],

          [2.058823799247812, 0.07058633859438503],

          [3.000001503542886, 0.04705708651959327],

          [3.9411790099468273, 0.07058689299792453],

          [4.8235326678889345, 0.16470511854183797],

          [5.529415336860586, 0.4235293374365447],

          [5.76470933698621, 1.1058829941330384],

          [5.764708805535902, 1.8941189433780983],

          [5.5294138118186265, 2.5764724018811056],

          [4.823530348360371, 2.835296251305122],

          [3.941176199414957, 2.929413985845729],

          [2.9999985709076413, 2.952943245204772],

          [2.0588211310939526, 2.9294134622132018],

          [1.1764675231284938, 2.8352952720424938],

          [0.4705848811030855, 2.5764710948028178],

          [0.23529088056307781, 1.8941174802285707],

          [0.23529138316655338, 1.1058815684272394]]

 

testpath2 = [[1, 0], # Move in the shapeof a plus sign

             [2, 0],

             [2, 1],

             [3, 1],

             [3, 2],

             [2, 2],

             [2, 3],

             [1, 3],

             [1, 2],

             [0, 2],

             [0, 1],

             [1, 1]]

 

answer2 = [[1.2222234770374059,0.4444422843711052],

          [1.7777807251383388, 0.4444432993123497],

          [2.111114925633848, 0.8888894279539462],

          [2.5555592020540376, 1.2222246475393077],

          [2.5555580686154244, 1.7777817817879298],

          [2.111111849558437, 2.1111159707965514],

          [1.7777765871460525, 2.55556033483712],

          [1.2222194640861452, 2.5555593592828543],

          [0.8888853322565222, 2.111113321684573],

          [0.44444105139827167, 1.777778212019149],

          [0.44444210978390364, 1.2222211690821811],

          [0.8888882042812255, 0.8888870211766268]]

 

# solution_check(smooth(testpath1),answer1)

# solution_check(smooth(testpath2),answer2)

 

3.   练习:Constrained Smoothing

约束性/限制性 平滑

就是说平滑过程中,要求某些点固定不动。

 

这个视频下方的编程练习的说明又说视频里面的计算方式有点小问题。

还是说这个newpath[i][j] 在计算中间过程的时候不能被改变,只能用最初值计算,建议用一个公式就完成每次对于单点newpath[i][j]的调整计算。

对非固定点的单次计算公式,考虑到了前后点对 weight_smooth 部分的计算。

newpath[i][j] +=weight_smooth * (newpath[(i-1)%len(path)][j] + newpath[(i+1)%len(path)][j] - \

                                   2.0 * newpath[i][j]) + \

                                   (weight_smooth / 2.0) * (2.0* newpath[(i-1)%len(path)][j] - \

                                   newpath[(i-2)%len(path)][j] -newpath[i][j]) + \

                                   (weight_smooth / 2.0) * (2.0* newpath[(i+1)%len(path)][j] - \

                                   newpath[(i+2)%len(path)][j] -newpath[i][j])

例子中对于固定的点,给了一个fix列表,值为1,则表明是固定点,不能改变位置。

例子代码如下:

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

"""

Created on Tue May 22 18:41:08 2018

 

@author: Administrator

"""

 

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

# User Instructions

#

# Now you will be incorporating fixedpoints into

# your smoother.

#

# You will need to use the equations fromgradient

# descent AND the new equations presentedin the

# previous lecture to implement smoothingwith

# fixed points.

#

# Your function should return the newpaththat it

# calculates.

#

# Feel free to use the providedsolution_check function

# to test your code. You can find it atthe bottom.

#

 

######################## ENTER CODE BELOWHERE #########################

 

def smooth(path, fix, weight_data = 0.0,weight_smooth = 0.1, tolerance = 0.00001):

   #

   # Enter code here.

   # The weight for each of the two new equations should be 0.5 *weight_smooth

   #

   newpath = [[path[row][col] for col in range(len(path[0]))] for row inrange(len(path))]

 

   changeFactor = tolerance

   while changeFactor >= tolerance:

       changeFactor = 0

       

       for i in range(len(path)):

           if fix[i] == 0:

                for j in range(len(path[0])):

                    aux = newpath[i][j]

                   

                    newpath[i][j] +=weight_smooth * (newpath[(i-1)%len(path)][j] + \

                           newpath[(i+1)%len(path)][j] - 2.0 * newpath[i][j]) + \

                            (weight_smooth /2.0) * (2.0 * newpath[(i-1)%len(path)][j] - \

                           newpath[(i-2)%len(path)][j] - newpath[i][j]) + \

                            (weight_smooth /2.0) * (2.0 * newpath[(i+1)%len(path)][j] - \

                           newpath[(i+2)%len(path)][j] - newpath[i][j])

 

                    changeFactor += abs(aux -newpath[i][j])

 

   

   return newpath

 

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

# Testing Instructions

#

# To test your code, call thesolution_check function with the argument smooth:

# solution_check(smooth)

#

 

def solution_check(smooth, eps = 0.0001):

   

   def test_case_str(path, fix):

       assert( len(path) == len(fix) )

 

       if len(path) == 0:

           return '[]'

       if len(path) == 1:

           s = '[' + str(path[0]) + ']'

           if fix[0]: s += ' #fix'

           return s

 

       s = '[' + str(path[0]) + ','

        if fix[0]: s += ' #fix'

       for pt,f in zip(path[1:-1],fix[1:-1]):

           s += '\n ' + str(pt) + ','

           if f: s += ' #fix'

       s += '\n ' + str(path[-1]) + ']'

       if fix[-1]: s += ' #fix'

       return s

   

   testpaths = [[[0, 0],[1, 0],[2, 0],[3, 0],[4, 0],[5, 0],[6, 0],[6,1],[6, 2],[6, 3],[5, 3],[4, 3],[3, 3],[2, 3],[1, 3],[0, 3],[0, 2],[0, 1]],

                 [[0, 0],[2, 0],[4, 0],[4,2],[4, 4],[2, 4],[0, 4],[0, 2]]]

   testfixpts = [[1, 0, 0, 0, 0, 0, 1, 0, 0, 1, 0, 0, 0, 0, 0, 1, 0, 0],

                  [1, 0, 1, 0, 1, 0, 1, 0]]

   pseudo_answers = [[[0, 0],[0.7938620981547201,-0.8311168821106101],[1.8579052986461084,-1.3834788165869276],[3.053905318597796, -1.5745863173084],[4.23141390533387,-1.3784271816058231],[5.250184859723701, -0.8264215958231558],[6,0],[6.415150091996651, 0.9836951698796843],[6.41942442687092,2.019512290770163],[6, 3],[5.206131365604606,3.831104483245191],[4.142082497497067, 4.383455704596517],[2.9460804122779813,4.5745592975708105],[1.768574219397359, 4.378404668718541],[0.7498089205417316,3.826409771585794],[0, 3],[-0.4151464728194156,2.016311854977891],[-0.4194207879552198, 0.9804948340550833]],

                      [[0,0],[2.0116767115496095, -0.7015439080661671],[4, 0],[4.701543905420104,2.0116768147460418],[4, 4],[1.9883231877640861, 4.701543807525115],[0,4],[-0.7015438099112995, 1.9883232808252207]]]

   true_answers = [[[0, 0],[0.7826068175979299,-0.6922616156406778],[1.826083356960912,-1.107599209206985],[2.999995745732953,-1.2460426422963626],[4.173909508264126, -1.1076018591282746],[5.217389489606966,-0.6922642758483151],[6, 0],[6.391305105067843,0.969228211275216],[6.391305001845138, 2.0307762911524616],[6,3],[5.217390488523538, 3.6922567975830876],[4.17391158149052,4.107590195596796],[2.9999982969959467, 4.246032043344827],[1.8260854997325473,4.107592961155283],[0.7826078838205919, 3.692259569132191],[0,3],[-0.3913036785959153, 2.030774470796648], [-0.3913035729270973,0.9692264531461132]],

                    [[0,0],[1.9999953708444873, -0.6666702980585777],[4, 0],[4.666670298058577,2.000005101453379],[4, 4],[1.9999948985466212, 4.6666612524128],[0,4],[-0.6666612524127998, 2.000003692691148]]]

   newpaths = map(lambda p: smooth(*p), zip(testpaths,testfixpts))

   

   correct = True

   

   for path,fix,p_answer,t_answer,newpath in zip(testpaths,testfixpts,

                                                  pseudo_answers,true_answers,

                                                  newpaths):

       if type(newpath) != list:

           print("Error: smooth did not return a list for the path:")

           print(test_case_str(path,fix) + '\n')

           correct = False

           continue

       if len(newpath) != len(path):

           print("Error: smooth did not return a list of the correct lengthfor the path:")

           print(test_case_str(path,fix) + '\n')

           correct = False

           continue

 

       good_pairs = True

       for newpt,pt in zip(newpath,path):

           if len(newpt) != len(pt):

                good_pairs = False

                break

       if not good_pairs:

           print("Error: smooth did not return a list of coordinate pairs forthe path:")

           print(test_case_str(path,fix) + '\n')

           correct = False

           continue

       assert( good_pairs )

       

       # check whether to check against true or pseudo answers

       answer = None

       if abs(newpath[1][0] - t_answer[1][0]) <= eps:

           answer = t_answer

       elif abs(newpath[1][0] - p_answer[1][0]) <= eps:

           answer = p_answer

       else:

           print('smooth returned an incorrect answer for the path:')

           print(test_case_str(path,fix) + '\n')

           continue

       assert( answer is not None )

 

       entries_match = True

       for p,q in zip(newpath,answer):

           for pi,qi in zip(p,q):

                if abs(pi - qi) > eps:

                    entries_match = False

                    break

           if not entries_match: break

       if not entries_match:

           print('smooth returned an incorrect answer for the path:')

           print(test_case_str(path,fix) + '\n')

           continue

           

       assert( entries_match )

       if answer == t_answer:

           print('smooth returned the correct answer for the path:')

           print(test_case_str(path,fix) + '\n')

       elif answer == p_answer:

           print('smooth returned a correct* answer for the path:')

           print(test_case_str(path,fix))

           print('''*However, your answer uses the "nonsimultaneous"update method, which

is not technically correct. You shouldmodify your code so that newpath[i][j] is only

updated once per iteration, or else theintermediate updates made to newpath[i][j]

will affect the final answer.\n''')

 

solution_check(smooth)

 

4.   练习:Racetrack Control

赛马场控制,有个要注意的点是,视频下方提示了:

Make sure CTE is negative inside the track and positive outside the track.

我之前没看到,在计算跟x轴重合的那条路线时,计算得到的cte值的符号跟其他地方相反。导致运行结果 err 比较大,然后改正后,err值收敛,符合要求了。

但是为何有这个要求呢??看了后面一节课的Help,我想这主要是因为,类比等高线图来说,环形线就是一个等高线,进入环内和远离环内,意味着沿着梯度的上升或者下降,所以得方向相反,而在环外的是同一个方向。跟坐标系无关。另外一方面,这样就使前进方向的同一侧的cte值是同一种符号。

代码:

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

 

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

# User Instructions

#

# Define a function cte in the robot class thatwill

# compute the crosstrack error for a robot on a

# racetrack with a shape as described in thevideo.

#

# You will need to base your error calculationon

# the robot's location on the track. Rememberthat

# the robot will be traveling to the right onthe

# upper straight segment and to the left on thelower

# straight segment.

#

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

# Grading Notes

#

# We will be testing your cte function directlyby

# calling it with different robot locations andmaking

# sure that it returns the correct crosstrackerror. 

 

from math import *

import random

 

 

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

#

# this is the robot class

#

 

class robot:

 

    #--------

    #init:

   #    creates robot and initializeslocation/orientation to 0, 0, 0

    #

 

    def __init__(self, length = 20.0):

        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.steering_drift = 0.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):

        #makes it possible to change the noise parameters

        #this is often useful in particle filters

       self.steering_noise = float(new_s_noise)

       self.distance_noise = float(new_d_noise)

 

    #--------

    #set_steering_drift:

    #     sets the systematical steering driftparameter

    #

 

    def set_steering_drift(self, drift):

       self.steering_drift = drift

       

    #--------

    #move:

   #    steering = front wheelsteering angle, limited by max_steering_angle

   #    distance = total distancedriven, most be non-negative

 

    def move(self, steering, distance,

            tolerance = 0.001, max_steering_angle = pi / 4.0):

 

        ifsteering > 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.steering_drift = self.steering_drift

 

        #apply noise

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

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

 

        #apply steering drift

       steering2 += self.steering_drift

 

        #Execute motion

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

 

       if abs(turn) < tolerance:

 

           # approximate by straight 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:

 

           # approximate bicycle 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)

 

       return res

 

 

 

 

    def __repr__(self):

       return '[x=%.5f y=%.5f orient=%.5f]' % (self.x, self.y, self.orientation)

 

 

############## ONLY ADD / MODIFY CODE BELOWTHIS LINE ####################

  

    def cte(self, radius):

        #

        #

        #Add code here

        #

        #           

        #find out the two circle center point.

       CxLeft = radius

       CyLeft = radius

       CxRight = radius + 2 * radius

       CyRight = radius

       

       cte = float('Inf')

 

       if self.x < radius:#robot.x >= 0

            cte = sqrt((self.x - CxLeft)**2 + (self.y- CyLeft)**2) - radius

       elif self.x >= CxLeft and self.x < CxRight:

           cte1 = self.y - 2 * radius

           cte2 = self.y

           if abs(cte1) < abs(cte2):

               cte = cte1

           else:

               # 因为编程前视频下方说明:Make sure CTE is negative inside the track andpositive outside the track.

               #cte = cte2

               cte = -cte2

       else:# self.x >= CxRight and self.x < CxRight + radius

           cte = sqrt((self.x - CxRight)**2 + (self.y - CyRight)**2) - radius

       return cte

   

############## ONLY ADD / MODIFY CODE ABOVETHIS LINE ####################

 

 

 

 

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

#

# run - does a single control run.

 

 

def run(params, radius, printflag = False):

   myrobot = robot()

   myrobot.set(0.0, radius, pi / 2.0)

    speed= 1.0 # motion distance is equal to speed (we assume time = 1)

    err =0.0

   int_crosstrack_error = 0.0

    N =200

 

   crosstrack_error = myrobot.cte(radius) # You need to define the ctefunction!

 

    for iin range(N*2):

       diff_crosstrack_error = - crosstrack_error

       crosstrack_error = myrobot.cte(radius)

       diff_crosstrack_error += crosstrack_error

       int_crosstrack_error += crosstrack_error

       steer = - params[0] * crosstrack_error \

               - params[1] * diff_crosstrack_error \

               - params[2] * int_crosstrack_error

        myrobot = myrobot.move(steer, speed)

       if i >= N:

           err += crosstrack_error ** 2

       if printflag:

           print(myrobot)

   return err / float(N)

 

radius = 25.0

params = [10.0, 15.0, 0]

err = run(params, radius, True)

print('\nFinal parameters: ', params, '\n->', err)

 

  • 0
    点赞
  • 0
    收藏
    觉得还不错? 一键收藏
  • 0
    评论
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值