课程 15： Lesson 5-PIDControl

最新推荐文章于 2025-03-23 10:35:49 发布

翻译最新推荐文章于 2025-03-23 10:35:49 发布 · 1.2k 阅读

标签

#Udacity课程笔记 #机器人人工智能 #PIDControl

课程笔记专栏收录该内容

19 篇文章

订阅专栏

本文探讨了机器人路径平滑算法与PID控制原理。介绍了如何通过路径平滑算法优化路径，使其更加平滑；并通过PID控制器实现机器人精确跟随平滑路径。文中详细解释了路径平滑中的梯度下降法及PID控制器中的比例(P)、积分(I)、微分(D)作用。

课程 15： Lesson 5-PIDControl

1. 练习：Robot Motion

关于PID控制器，https://baike.so.com/doc/101113-106693.html

实际生活中的车辆的路径移动，跟前面提到的一般不同，在转弯处不会进行直角转弯，一般是较平滑的曲线的转弯。

2. 练习：Smoothing Algorithm

要做到路径规划中的路径曲线是平滑的，即有以下要求，假设平滑前的路径上的诸多的点的序列是X_i,平滑后的点序列是Y_i,应使：

第一步: Y_i = X_i

第二步，优化：(Y_i – X_i)² à min

(Y_i – Y_i+1)² à min

+α

3. 练习：Smoothing Algorithm 2

只优化(Y_i – X_i)²会得到原路径，只优化 (Y_i– Y_i+1)²会使点都靠近起始点。

4. 练习：Smoothing Algorithm 3

所以在路径平滑的过程中，往往会加入α参数，给定优化(Y_i– Y_i+1)²的权重，α越小，越靠近起始点。优化过程主要针对的是起始点终点之间的点。

5. 练习：Path Smoothing

优化那两项，有一个想法是使用 梯度下降 方式，每次迭代都沿着能减小误差的方向。

给出了一个这样的优化公式，

1. 优化起点与终点

2. 按照屏幕右边提到的3个步骤，根据X_i,α，β，Y_i+1,Y_i-1,Y_i，来得到新的Y_i

Y_i ‘= Y_i+ α*（X_i - Y_i）

Y_i “= Y_i+ β*（Y_i+1 -Y_i）

Y_i = Y_i‘+ β*(Y_i+1 + Y_i-1 – 2*Y_i)

助教说屏幕上面3个式子中的三个减号要换成加号，不然测试代码的数据不能收敛。但是代码实践中，发现并未使用第二个公式。只用了第一个和第三个。

这个运算符号的差异，或许跟起点终点之间的大小变化有关，测试中终点比起点的值要大，如果是减号，越减越小，就不会到终点方向去了。这大概就是所谓梯度下降和梯度上升的区别吧。

在课程16中，有说明文字提到这里的计算错了，因为Y_i不能在中间结果这里累加，会影响后续使用时Y_i的值。所以后来，我给这前面的式子的Y_i添加了引号以示区别。它的建议是，这个计算放到一行里面算出来。

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

这一课中的视频里面说错了，包括实例的代码讲解是错的，但是说明文字那里，又写对了。如下：

y_i <- y_i + alpha (x_i - y_i) + beta (y_{i + 1} + y_{i - 1} - 2 * y_i)

这个链接对梯度下降概念进行了数学上的介绍：

https://www.cnblogs.com/pinard/p/5970503.html

3. 误差参数 tolerance

在实际代码中，使用了这个参数，然后给了一个循环，不断地使用给定的α，β，以及第二步中的那2个公式，来进行优化。使最后一次优化时优化前后的路径的积累偏差小于这个误差参数。

具体代码如下：

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

 

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

# User Instructions

#

# Define a functionsmooth that takes a path as its input

# (with optionalparameters for weight_data, weight_smooth,

# and tolerance) andreturns a smooth path. The first and

# last points shouldremain unchanged.

#

# Smoothing should beimplemented by iteratively updating

# each entry innewpath until some desired level of accuracy

# is reached. Theupdate should be done according to the

# gradient descentequations given in the instructor's note

# below (the equationsgiven in the video are not quite

# correct).

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

 

from copy import deepcopy

 

# thank you to EnTerrfor posting this on our discussion forum

def printpaths(path,newpath):

    for old,new in zip(path,newpath):

        print('['+ ', '.join('%.3f'%x for x inold) + \

               '] -> ['+ ', '.join('%.3f'%xfor x in new) +']')

 

# Don't modify pathinside your function.

path = [[0, 0],

        [0, 1],

        [0, 2],

        [1, 2],

        [2, 2],

        [3, 2],

        [4, 2],

        [4, 3],

        [4, 4]]

 

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

 

    # Make a deep copy of path into newpath

    newpath = deepcopy(path)

 

    #######################

    ### ENTER CODE HERE ###

    #######################

    changeFactor = tolerance

    while changeFactor >= tolerance:

        changeFactor = 0

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

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

                aux = newpath[i][j]

 

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

                #temp += weight_smooth *(newpath[i+1][j] - newpath[i][j])

                newpath[i][j] += temp + weight_smooth* (newpath[i+1][j] + newpath[i-1][j] - 2.0 * newpath[i][j])

               

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

    return newpath # Leave this line for thegrader!

 

printpaths(path,smooth(path))

视频里面的演示代码是错误的。。。

6. 练习：Zero Data Weight

在平滑路径的时候，如果预定的路线附近有障碍物，有可能会使得路线碰到障碍物。这时候，可以使用之前的随机运动动态规划法(就是前面的运动时有一定的随机性，并且对碰撞有成本的练习)，或者使得路径上的点与最近的障碍物的距离最大化，以避免碰障。

7. 练习：PID Control

在汽车与参考航线发生偏离时，应该如何转向呢，可以让转向角度与横切距离（车子偏离航线的直线距离）正相关。

8. 练习：Proportional Control（比例控制）

按照上节中的转向与横切距离关联的话，实际运动时，会使车在参考航线的两侧曲线移动，这个现象被称为“临界稳定”或“稳定”。

9. Implement P Control

觉得在第10节给出的代码答案不大正确，程序没有设置噪声参数，开始的角度是0度，那么肯定是一个匀速直线运动，而且平行于y=1.0这条线，根本不会发生上下偏转。如果在我代码的基础上，初始化robot的时候，加入噪声参数，便会看到上下浮动的曲线现象。

我的代码如下：

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

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

# User Instructions

#

# Implement a P controller by running 100 iterations

# of robot motion. The desired trajectory for the

# robot is the x-axis. The steering angle should be set

# by the parameter tau so that:

#

# steering = -tau * crosstrack_error

#

# You'll only need to modify the `run` function at thebottom.

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

 

import random

import numpy as np

import matplotlib.pyplot as plt

 

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

#

# this is the Robot class

#

 

class Robot(object):

    def __init__(self, length=20.0):

       """

        Creates robotand initializes location/orientation to 0, 0, 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

 

    def set(self, x,y, orientation):

       """

        Sets a robotcoordinate.

       """

        self.x = x

        self.y = y

       self.orientation = orientation % (2.0 * np.pi)

 

    def set_noise(self, steering_noise, distance_noise):

       """

        Sets thenoise parameters.

       """

        # makes itpossible to change the noise parameters

        # this isoften useful in particle filters

       self.steering_noise = steering_noise

       self.distance_noise = distance_noise

 

    def set_steering_drift(self, drift):

       """

        Sets thesystematical steering drift parameter

       """

       self.steering_drift = drift

 

    def move(self,steering, distance, tolerance=0.001, max_steering_angle=np.pi / 4.0):

       """

        steering =front wheel steering angle, limited by max_steering_angle

        distance =total distance driven, most be non-negative

       """

        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

 

        # apply noise

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

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

 

        # applysteering drift

        steering2 +=self.steering_drift

 

        # Executemotion

        turn =np.tan(steering2) * distance2 / self.length

 

        if abs(turn)< tolerance:

            #approximate by straight line motion

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

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

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

        else:

            # approximate bicycle model formotion

            radius =distance2 / turn

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

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

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

            self.x =cx + (np.sin(self.orientation) * radius)

            self.y =cy - (np.cos(self.orientation) * radius)

 

    def __repr__(self):

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

 

############## ADD / MODIFY CODE BELOW ####################

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

#

# run - does a single control run

robot = Robot()

robot.set(0.0, 1.0, 0.0)

 

#robot.set_noise(0.01,0.05)

 

def run(robot, tau, n=100, speed=1.0):

    x_trajectory = []

    y_trajectory = []

    # TODO: your codehere

    crosstrack_error= 0.0

    startX = robot.x

    startY = robot.y

    startOrientation= robot.orientation

    distance = 0.0

 

    for i in range(n):

        steering =-tau * crosstrack_error

        distance =speed

 

       robot.move(steering, distance)

       x_trajectory.append(robot.x)

       y_trajectory.append(robot.y)

       

        # when in theoriginal path, x=robot.x ,caculate y.

        x1 = robot.x

        y1 = startY +(x1 - startX) * np.tan(startOrientation)

       crosstrack_error = (robot.y - y1) * np.cos(startOrientation)

       #print("{} [{}, {}] {}".format(i,robot.x, robot.y, steering))

   

    returnx_trajectory, y_trajectory

   

x_trajectory, y_trajectory = run(robot, 0.1)

n = len(x_trajectory)

 

fig, (ax1, ax2) = plt.subplots(2, 1, figsize=(8, 8))

ax1.plot(x_trajectory, y_trajectory, 'g', label='Pcontroller')

ax1.plot(x_trajectory, np.zeros(n), 'r', label='reference')

10. Implement P Control(solution)

11. 练习：Oscillations

12. PD Controller

13. PD Controller(solution)

这次实现的代码，依旧是在计算 CTE（横切距离）上，跟我自己的代码不一样，课程老是用的robot.y值，我觉得这是不正确的，要看偏移值，我假设的是起点时候的参数是正确的。路径沿着初始的orientation方向，平行于x轴，而老师假定的默认路径就是沿着x轴。

这次的代码中加入了另外一个参数，就是计算转向角的时候，加入了对CTE的导数的因素。以减小PDcontroller的振幅？

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

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

# User Instructions

#

# Implement a PD controller by running 100 iterations

# of robot motion. The steering angle should be set

# by the parameter tau_p and tau_d so that:

#

# steering = -tau_p * CTE - tau_d * diff_CTE

# where differential crosstrack error (diff_CTE)

# is given by CTE(t) - CTE(t-1)

#

#

# Only modify code at the bottom! Look for the TODO

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

 

import random

import numpy as np

import matplotlib.pyplot as plt

 

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

#

# this is the Robot class

#

 

class Robot(object):

    def __init__(self, length=20.0):

       """

        Creates robotand initializes location/orientation to 0, 0, 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

 

    def set(self, x,y, orientation):

       """

        Sets a robotcoordinate.

       """

        self.x = x

        self.y = y

       self.orientation = orientation % (2.0 * np.pi)

 

    def set_noise(self, steering_noise, distance_noise):

        """

        Sets thenoise parameters.

       """

        # makes itpossible to change the noise parameters

        # this isoften useful in particle filters

       self.steering_noise = steering_noise

       self.distance_noise = distance_noise

 

    def set_steering_drift(self, drift):

       """

        Sets thesystematical steering drift parameter

       """

       self.steering_drift = drift

 

    def move(self,steering, distance, tolerance=0.001, max_steering_angle=np.pi / 4.0):

       """

        steering =front wheel steering angle, limited by max_steering_angle

        distance =total distance driven, most be non-negative

       """

        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

 

        # apply noise

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

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

 

        # applysteering drift

        steering2 +=self.steering_drift

 

        # Executemotion

        turn =np.tan(steering2) * distance2 / self.length

 

        if abs(turn)< tolerance:

            #approximate by straight line motion

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

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

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

        else:

            #approximate bicycle model for motion

            radius =distance2 / turn

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

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

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

            self.x =cx + (np.sin(self.orientation) * radius)

            self.y =cy - (np.cos(self.orientation) * radius)

 

    def __repr__(self):

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

 

############## ADD / MODIFY CODE BELOW ####################

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

#

# run - does a single control run

 

# previous P controller

def run_p(robot, tau, n=100, speed=1.0):

    x_trajectory = []

    y_trajectory = []

    for i inrange(n):

        cte = robot.y

        steer = -tau* cte

       robot.move(steer, speed)

       x_trajectory.append(robot.x)

       y_trajectory.append(robot.y)

    returnx_trajectory, y_trajectory

   

robot = Robot()

robot.set(0, 1, 0)

 

robot.set_noise(0.1,0.05)

 

def run(robot, tau_p, tau_d, n=100, speed=1.0):

    x_trajectory = []

    y_trajectory = []

    # TODO: your codehere

    #steering =-tau_p * CTE - tau_d * diff_CTE

    crosstrack_error= []

   crosstrack_error.append(0.0)

    diff_CTE = 0.0

    startX = robot.x

    startY = robot.y

    startOrientation= robot.orientation

    distance = 0.0

 

    for i inrange(n):

        steering =-tau_p * crosstrack_error[i] - tau_d * diff_CTE

        distance =speed

 

       robot.move(steering, distance)

       x_trajectory.append(robot.x)

       y_trajectory.append(robot.y)

       

        # when in theoriginal path, x=robot.x ,caculate y.

        x1 = robot.x

        y1 = startY +(x1 - startX) * np.tan(startOrientation)

        crosstrack =(robot.y - y1) * np.cos(startOrientation)

       crosstrack_error.append(crosstrack)

        diff_CTE =crosstrack_error[i+1] - crosstrack_error[i]

       print("{} [{}, {}] {}, {}".format(i,robot.x, robot.y,steering, crosstrack))

       

    returnx_trajectory, y_trajectory

   

x_trajectory, y_trajectory = run(robot, 0.2, 3.0)

n = len(x_trajectory)

 

fig, (ax1, ax2) = plt.subplots(2, 1, figsize=(8, 8))

ax1.plot(x_trajectory, y_trajectory, 'g', label='PDcontroller')

ax1.plot(x_trajectory, np.zeros(n), 'r', label='reference')

代码中的run_p() 是老师提供的上次的代码实现，我没有按照我自己的理解去修改它。

14. 练习：Systematic Bias

15. 练习：Is PD Enough

使用微分项 diff_CTE,还是不能完全解决问题。

看了网上一部分文章，说到PID的使用有很多限制，一般适用于线性非时变系统，和一阶系统？好处是不需要知道系统的确切模型。然后有的系统可能用PI或者PD就能解决问题，未必要用全PID。

16. PID Implementation

所以，加入了CTE的积分项。三项组合起来，就叫做PID

P代表比例项，D代表微分项，I代表积分项。

17. PID Implementation(solution)

steering = -tau_p * CTE - tau_d * diff_CTE - tau_i *int_CTE

代码例子：

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

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

# User Instructions

#

# Implement a P controller by running 100 iterations

# of robot motion. The steering angle should be set

# by the parameter tau so that:

#

# steering = -tau_p * CTE - tau_d * diff_CTE - tau_i *int_CTE

#

# where the integrated crosstrack error (int_CTE) is

# the sum of all the previous crosstrack errors.

# This term works to cancel out steering drift.

#

# Only modify code at the bottom! Look for the TODO.

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

 

import random

import numpy as np

import matplotlib.pyplot as plt

 

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

#

# this is the Robot class

#

 

class Robot(object):

    def __init__(self, length=20.0):

       """

        Creates robotand initializes location/orientation to 0, 0, 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

 

    def set(self, x,y, orientation):

       """

        Sets a robotcoordinate.

       """

        self.x = x

        self.y = y

       self.orientation = orientation % (2.0 * np.pi)

 

    def set_noise(self, steering_noise, distance_noise):

       """

        Sets thenoise parameters.

       """

        # makes itpossible to change the noise parameters

        # this isoften useful in particle filters

       self.steering_noise = steering_noise

       self.distance_noise = distance_noise

 

    def set_steering_drift(self, drift):

        """

        Sets thesystematical steering drift parameter

       """

       self.steering_drift = drift

 

    def move(self,steering, distance, tolerance=0.001, max_steering_angle=np.pi / 4.0):

       """

        steering =front wheel steering angle, limited by max_steering_angle

        distance =total distance driven, most be non-negative

       """

        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

 

        # apply noise

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

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

 

        # applysteering drift

        steering2 +=self.steering_drift

 

        # Executemotion

        turn =np.tan(steering2) * distance2 / self.length

 

        if abs(turn)< tolerance:

            #approximate by straight line motion

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

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

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

        else:

            #approximate bicycle model for motion

            radius =distance2 / turn

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

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

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

            self.x =cx + (np.sin(self.orientation) * radius)

            self.y =cy - (np.cos(self.orientation) * radius)

 

    def __repr__(self):

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

 

############## ADD / MODIFY CODE BELOW ####################

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

#

# run - does a single control run

 

robot = Robot()

robot.set(0, 1, 0)

 

# 下面一行是自己加的

robot.set_noise(0.05,0.05)

 

def run(robot, tau_p, tau_d, tau_i, n=100, speed=1.0):

    x_trajectory = []

    y_trajectory = []

    # TODO: your codehere

    #steering =-tau_p * CTE - tau_d * diff_CTE - tau_i * int_CTE

    crosstrack_error= []

   crosstrack_error.append(0.0)

    diff_CTE = 0.0

    startX = robot.x

    startY = robot.y

    startOrientation= robot.orientation

    distance = 0.0

 

    for i inrange(n):

        steering =-tau_p * crosstrack_error[i] - tau_d * diff_CTE - tau_i * sum(crosstrack_error)

        distance =speed

 

       robot.move(steering, distance)

       x_trajectory.append(robot.x)

       y_trajectory.append(robot.y)

       

        # when in theoriginal path, x=robot.x ,caculate y.

        x1 = robot.x

        y1 = startY +(x1 - startX) * np.tan(startOrientation)

        crosstrack =(robot.y - y1) * np.cos(startOrientation)

       crosstrack_error.append(crosstrack)

        diff_CTE =crosstrack_error[i+1] - crosstrack_error[i]

    returnx_trajectory, y_trajectory

 

 

x_trajectory, y_trajectory = run(robot, 0.2, 3.0, 0.004)

n = len(x_trajectory)

 

fig, (ax1, ax2) = plt.subplots(2, 1, figsize=(8,8))

ax1.plot(x_trajectory, y_trajectory, 'g', label='PIDcontroller')

ax1.plot(x_trajectory, np.zeros(n), 'r', label='reference')

上面的run()是按照我的想法写的，没有像视频中那样使crosstrack = robot.y

当给初始的orientation一个值时，如果只用了CTE，就是P控制器的话，后期会有波浪线增大的趋势，使用PD控制器或者PID控制器之后，斜线会几乎按照预期的变成接近直线。

18. Twiddle

那么，在实现PID控制器时，如何确定更优的 tau_p,tau_d,tau_i呢？

这里使用一个方法，叫做Twiddle算法。

里面讲了下Twiddle算法的过程，说它属于一种局部爬山算法，但很有效。

19. Parameter Optimization

这节是个实现Twiddle调PID参数的例子，run()中有个for循环，循环长度为2N，但是只是在N-2N之间才开始调参。返回的best_err是N-2N之间的CTE的平方和的平均数。

20. Parameter Optimization(solution)

里面提到说 PID 的积分项，主要用于降低系统误差。微分项很重要，显著地降低了误差。事实上在我自己改动的代码里面，对于匀速直线行驶的这个例子来说，加入P项（比例项），微分项D后，结果已经几乎是一条直线了。

绿线就是一次代码测试中仅用了PD项的情况。robot参数设置是：

robot.set(0.0,1.0, 0.2)

robot.set_steering_drift(10.0/ 180.0 * np.pi)

robot.set_noise(0.1,0.1)

其期望值就是沿着一条斜线走。run()里面的n我设置成了1000.实际测试时，去掉dparams中的p项或者d项参数都不行，仅去掉i项参数，效果还是比较好的。

练习代码如下：

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

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

# User Instructions

#

# Implement twiddle as shown inthe previous two videos.

# Your accumulated error should bevery small!

#

# You don't have to use the exactvalues as shown in the video

# play around with differentvalues! This quiz isn't graded just see

# how low of an error you can get.

#

# Try to get your error below1.0e-10 with as few iterations

# as possible (too many iterationswill cause a timeout).

#

# No cheating!

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

 

import random

import numpy as np

import matplotlib.pyplot as plt

 

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

#

# this is the Robot class

#

 

class Robot(object):

   def __init__(self, length=20.0):

        """

        Creates robot and initializeslocation/orientation to 0, 0, 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

 

   def set(self, x, y, orientation):

        """

        Sets a robot coordinate.

        """

        self.x = x

        self.y = y

        self.orientation = orientation % (2.0 *np.pi)

 

   def set_noise(self, steering_noise, distance_noise):

        """

        Sets the noise parameters.

        """

        # makes it possible to change the noiseparameters

        # this is often useful in particlefilters

        self.steering_noise = steering_noise

        self.distance_noise = distance_noise

 

   def set_steering_drift(self, drift):

        """

        Sets the systematical steering driftparameter

        """

        self.steering_drift = drift

 

   def move(self, steering, distance, tolerance=0.001,max_steering_angle=np.pi / 4.0):

        """

        steering = front wheel steering angle,limited by max_steering_angle

        distance = total distance driven, mostbe non-negative

        """

        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

 

        # 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 = np.tan(steering2) *distance2 / self.length

 

        if abs(turn) < tolerance:

            # approximate by straight linemotion

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

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

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

        else:

            # approximate bicycle model formotion

            radius = distance2 / turn

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

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

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

            self.x = cx +(np.sin(self.orientation) * radius)

            self.y = cy -(np.cos(self.orientation) * radius)

 

   def __repr__(self):

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

 

############## ADD / MODIFY CODEBELOW ####################

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

#

# run - does a single control run

 

 

def make_robot():

   """

   Resets the robot back to the initial position and drift.

   You'll want to call this after you call `run`.

   """

   robot = Robot()

   robot.set(0.0, 1.0, 0.2)#原代码值0.0

   robot.set_steering_drift(10.0 / 180.0 * np.pi)

   #robot.set_noise(0.1,0.1) # 这行自己加了测试用的，加入噪声以后，轨迹会发生偏移，仅加入上面那行，也会发生偏移。

   return robot

 

 

# NOTE: We use params instead oftau_p, tau_d, tau_i

'''作业中的源码，我先注释掉，因为我对这CTE的计算有疑问，用我自己的CTE计算方法，所以在下面重写了个run()

def run(robot, params, n=100,speed=1.0):

   x_trajectory = []

   y_trajectory = []

   err = 0

   prev_cte = robot.y

   int_cte = 0

   for i in range(2 * n):

        cte = robot.y

        diff_cte = cte - prev_cte

        int_cte += cte

        prev_cte = cte

        steer = -params[0] * cte - params[1] *diff_cte - params[2] * int_cte

        robot.move(steer, speed)

        x_trajectory.append(robot.x)

        y_trajectory.append(robot.y)

        if i >= n:

            err += cte ** 2

   return x_trajectory, y_trajectory, err / n

'''

# 重写的自己的CTE的计算方法的run()

def run(robot, params, n=100,speed=1.0):

   x_trajectory = []

   y_trajectory = []

   err = 0

   prev_cte = 0

   int_cte = 0

   

   cte = 0

   startX = robot.x

   startY = robot.y

   startOrientation = robot.orientation

   

   for i in range(2 * n):

        diff_cte = cte - prev_cte

        int_cte += cte

        prev_cte = cte

        steer = -params[0] * cte - params[1] *diff_cte - params[2] * int_cte

        robot.move(steer, speed)

        x_trajectory.append(robot.x)

        y_trajectory.append(robot.y)

       

        x1 = robot.x

        y1 = startY + (x1 - startX) *np.tan(startOrientation)

        cte = (robot.y - y1) * np.cos(startOrientation)

       

        if i >= n:

            err += cte ** 2

   return x_trajectory, y_trajectory, err / n

 

 

# Make this tolerance bigger ifyou are timing out!

def twiddle(tol=0.2):

   # Don't forget to call `make_robot` before every call of `run`!

   p = [0.0, 0.0, 0.0] # params

   dp = [1.0, 1.0, 1.0] # dparams

   #dp[2] = 0 # for test, do not use I item params.

   robot = make_robot()

   x_trajectory, y_trajectory, best_err = run(robot, p)

   # TODO: twiddle loop here

 

   # Begin: From the video

   # 要调节的 PID 参数的个数 P I D

   #n_params = 3

   #dparams = [1.0 for row in range(n_params)]

   #params = [0.0 for row in range(n_params)]

   

   params = p   # params 里面存的是最优的各个PID参数。

   dparams = dp # dparams 里面存的应该是调节params各参数对应的步长

   n = 0

   while sum(dparams) > tol:

        for i in range(len(params)):

            params[i] += dparams[i]

            robot = make_robot() # 这行是自己加的，因为视频上使用的是run(params)，可能它里面已经重新初始化了robot.

            x_trajectory, y_trajectory, b_err =run(robot, params)

            if b_err < best_err:

                best_err = b_err

                dparams[i] *= 1.1

            else:

                params[i] -= 2.0 * dparams[i]

                robot = make_robot() # 这行是自己加的

                x_trajectory, y_trajectory,b_err = run(robot, params)

                if b_err < best_err:

                    best_err = b_err

                    dparams[i] *= 1.1

                else:

                    params[i] += dparams[i]

                    dparams[i] *= 0.9

        n += 1

        print('Twiddle #', n, params, ' ->', best_err)

   print(' ')

   p = params # 这行自己加的，以对应下方的返回值。

   # End: From the video

   return p, best_err

 

 

params, err = twiddle()

print("Final twiddle error ={}".format(err))

robot = make_robot()

x_trajectory, y_trajectory, err =run(robot, params)

n = len(x_trajectory)

 

fig, (ax1, ax2) = plt.subplots(2,1, figsize=(8, 8))

ax1.plot(x_trajectory,y_trajectory, 'g', label='Twiddle PID controller')

ax1.plot(x_trajectory,np.zeros(n), 'r', label='reference')

21. Summary

这节课讨论了：1. 如何从离散路径转换成连续的平滑路径。

2. 如何用PID控制来让机器人跟着平滑路径走。

多数的机器人，只要用上寻路器（Planner），平滑器（Smoother），控制器（Controller），它就能工作得很好了。

这些就是机器人驾驶的要点。