PID control

|—平滑化算法

|—PID控制—|—P控制器编程

|—PD控制编程

|—PID控制编程

|—参数优化

|—实验P、PD、PID对减小系统误差的作用

这里讨论怎么将路径转变成行动指令(生成平滑的路径)，用到PID去控制。

从出发点到目的地，实际的路径不能像之前的path plan(蓝色)，没有car能立转90度，也不能像绿色斜转45度，红色路径最好温和地移动。

平滑化算法

将路径规划找出来的每个小格点设为x₀~x_i~ xn-1（未平滑化），并设置值与xi相等的变量yi，然后做两项优化使每项平方差最小

if只做第一项优化，结果仍会是原始路径因为步骤1已经设置的是两变量相减为0，而if只做第二项优化会使得相邻平滑点越来越近，最后除了一个原始点get nothing。

可以看出这两项式子是互相斗争的，使y i更接近xi点意味着路径趋向于块状而不光滑（如上面的蓝色路径）；使y i彼此靠近使得路径更平滑，但更不真实于原始路径（像以上绿色路径）。

那如何做到使两个式子都最小化？可以通过最小化一个线性组合使得两个式子都最小化，

c1相对c2越大就越平滑，相反就越接近一个原点，找到这两个斗争的平衡点就能得到如上面红线一样的平滑路径。

模拟情形说明两项优化后路径为什么会平滑：

当把y移动远离x(可对除起始目的点的其他所有y点)，产生y彼此之间距离越近更平滑的新的路径，同时第二个式子误差变小，但新的路径会使得第一个式子误差变大，但依靠权重c1和c2的设置能得到理想的路径，这个优化只优化中间的点，起始点和目的地点x,y一直相等。

如何做？

使用梯度下降，每一次迭代都沿着能减小误差的方向迈一小步，即每一步都对yi（除y0和yn-1）进行调整使得组合式子最小化：（这个算法红色部分原本是-，但这个模型里是+）

make code：

from math import *
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):#tolerance是误差容忍变量 

#将路径深度复制到newpath
    newpath = [[0 for col in range(len(path[0]))] for row in range(len(path))]
    for i in range(len(path)):
        for j in range(len(path[0])):
            newpath[i][j] = path[i][j]

#迭代那两条表达式应用到除0和n外的每个路径点上
#迭代不会停止直到某次更新后总体变化比容忍误差值小
change = tolerance
while (change >= tolerance):
    change = 0
    for i in range(1,len(path)­1):
        for j in range(len(path[0])):
            d1 = weight_data * (path[i][j] - ­newpath[i][j])
            d2 = weight_smooth * (newpath[i­1][j] + newpath[i+1][j] - 2 * newpath[i][j])
            change += abs(d1 + d2)
            newpath[i][j] += d1 + d2
return newpath 

PID控制

假设车有可转向的前轮和固定的后轮，以固定速率向前行驶，希望它走平滑的路径，但只能控制前轮转向角度。三种选择：1.固定转向量（这会走圆圈）2.随机转向命令，按横切误差的某个比例来设置转向角度（横切误差CTE：车辆与参考航线间的垂直距离）3.让转向与横切误差成比例

应该选第三种，误差越大就越朝着参考航线设置一个更大的转向角，随着接近航线，转向角会变小，最终挨着航线。

这种转向控制叫做P controller，应用这种控制到达航线后仍会以一个小转角继续行驶然后再，所以这种转向控制下的行驶轨迹如下图（临界稳定）：

#使用之前robot类，编写P控制器迭代100次机器人运动
#包含robot类中的__init__()、set()、set_noise()、move()、_repr_()
#转向角 steer = -tau * crosstrack_error（CTE）import random
import numpy as np
import matplotlib.pyplot as plt

import proportional

def run(param):     #控制参数param就是转向角度与CTE之比,换句话说param = tau = steer/CTE
    myrobot = robot()
    myrobot.set(0.0, 1.0, 0.0)
    speed = 1.0 # motion distance is equalt to speed (we assume time = 1)
    N = 100 

    for i in range(N):
         crosstrack_error = myrobot.y
         steer = -param * crosstrack_error
         myrobot = myroobo.move(steer,speed)
         print myrobot, steer 

#run(0.1)

下一个问题是有办法避免越界吗？用PD控制

在PD控制里，转向角度alpha不仅由控制参数tau_p和横切误差CTE决定，也会与CTE关于时间的导数有关，

这里间隔时间设为1，那么：

这个式子中，它会注意到CTE随着时间的过去变小，当减小到一定程度它会反方向使车轮向上转动，不会冲过 x轴而是平缓地接近参考航线。

所以现在不是像在P控制器里以CTE的比例控制steer，还加入了第二个常数tau_d和CTE的微分项

def run(param1=0.2, param2=3.0):
    myrobot = robot()
    myrobot.set(0.0, 1.0, 0.0)
    speed = 1.0 
    N = 100
    crosstrack_error = myrobot.y
    for i in range(N):
        diff_crosstrack_error = myrobot.y ­ crosstrack_error   #微分项
        crosstrack_error = myrobot.y
        steer = ­ param1 * crosstrack_errorparam2 * diff_crosstrack_error
        myrobot = myrobot.move(steer, speed)
        print myrobot, steer

#run(0.2,3.0)

这里在初始时都是假定车轮对正前，但其实实际中车轮在开始时会有偏差角度，而这个问题P、PD都解决不了，（P会沿着一个常数震荡，PD会贴合一个常数），带来系统偏差的问题，为了解决这个问题，引入PID。

假如One people以系统误差（由前轮转向偏差带来的持续的巨大偏差）的轨迹行驶，一段时间后他发现没有接近预想轨迹，于是他开始往右打方向盘回到预想轨迹，这段时间的偏差可以用CTE基于时间的积分来衡量。然后，来编写新的控制器，它会根据CTE比例、CTE微分的某个比例、以及会受到CTE积分的比例影响，CTE积分就是历史所有观察到的CTE的和，它的累积可以矫正方向。

def run(param1, param2, param3):
     myrobot = robot()
     myrobot.set(0.0, 1.0, 0.0)
    speed = 1.0  #假定速度是1
    N = 100
    myrobot.set_steering_drift(10.0 / 180.0 * pi) # 初始转向（造成系统偏差）
    int_crosstrack_error = 0 #新变量
    for i in range(N):
        int_crosstrack_error += crosstrack_error
        diff_crosstrack_error = myrobot.y ­ crosstrack_error
        crosstrack_error = myrobot.y
        steer = ­ param1 * crosstrack_error
                      -­ param2 * diff_crosstrack_error
    ­                  - param3 * int_crosstrack_error
        myrobot = myrobot.move(steer, speed)
        print myrobot, steer
    
#run(0.2,3.0,0.004)

参数优化

使用一种称为“Twiddle”的算法，每次只调整一个参数直到找到最佳参数集合。

从一个参数向量 p = [0, 0, 0]开始，p将代表迄今为止最佳猜测参数集合，还将初始化一个向量dp = [1，1，1]，代表想尝试的潜在变化。首先，在p中选择一个参数（比如这个例子中的p [0]，尽管它可以是任何参数），并且通过dp中的对应值增加该参数（例如，如果p [0] == 1和dp [0] == 0.5，那么我们p [0]为1.5）。然后在run()中调用p，结果赋给best_error，然后遍历p，首先把p增加一个探索值，把这个值赋给error，留下更小的值更新best_error，然后把dp的值稍微增大（自乘1.1），否则减掉两个单位dp（因为之前加了一个单位），如果两个都失败了，p[i]变回原来的值，并且降低探索范围dp[i]的值（bug应该是减dp[i]，比如自乘0.9）。只要dp之和大于阀值就会一直循环以上步骤。

#这里的params是三维向量
def run(params, printflag = False):
     myrobot = robot()
     myrobot.set(0.0, 1.0, 0.0)
     speed = 1.0
     err = 0.0
     int_crosstrack_error = 0.0
     N = 100
     myrobot.set_steering_drift(10.0 / 180.0 * pi)  #设置了10度的起始转向误差
     crosstrack_error = myrobot.y

     for i in range(N * 2):  #迭代2*N次PID
         diff_crosstrack_error = myrobot.y ­ crosstrack_error
         crosstrack_error = myrobot.y
         int_crosstrack_error += crosstrack_error
         steer = ­ params[0] * crosstrack_error  \
            ­       - params[1] * diff_crosstrack_error \
            ­       - int_crosstrack_error * params[2]
         myrobot = myrobot.move(steer, speed)
         if i >= N:  #在N次以后才开始统计，想观察CTE更显著的变化
              err += (crosstrack_error ** 2)
         if printflag:
              print myrobot, steer
     return err / float(N)  #返回平均误差

def twiddle(tol = 0.2): 
     n_params = 3
     dparams = [1.0 for row in range(n_params)]
     params = [0.0 for row in range(n_params)]
     best_error = run(params)
     n = 0
     while sum(dparams) > tol:
           for i in range(len(params)):
               params[i] += dparams[i]
               err = run(params)
               if err < best_error:
                   best_error = err
                   dparams[i] *= 1.1
               else:
                   params[i] ­= 2.0 * dparams[i]
                   err = run(params)
                   if err < best_error:
                       best_error = err
                       dparams[i] *= 1.1
                   else:
                       params[i] += dparams[i]
                       dparams[i] *= 0.9
     n += 1
     print ‘Twiddle #’, n, params, ‘ ­> ‘, best_error
     return run(params)

实验

下面尝试把积分项去掉，得到的是0积分增益，但是误差比上面的那个误差大一些，说明积分项对于让误差接近与0是必要的

然后尝试一下，令dparams[1] = 0,把微分项去掉,出现了0.55的误差！即使去掉drift误差也是巨大的。

实验在只有P情况下误差是0.1，PD下接近于0（没有drift情况下），而I（积分）是针对于有系统误差的机器人。

寻路器、平滑器、控制器可以应对大部分机器人的应用了，谷歌的还往里面加了料，它考虑了转向轮自有惯性的情况，