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（积分）是针对于有系统偏差的机器人。

 

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