|—平滑化算法
|—PID控制—|—P控制器编程
|—PD控制编程
|—PID控制编程
|—参数优化
|—实验P、PD、PID对减小系统误差的作用
这里讨论怎么将路径转变成行动指令(生成平滑的路径)，用到PID去控制。
从出发点到目的地，实际的路径不能像之前的path plan(蓝色)，没有car能立转90度，也不能像绿色斜转45度，红色路径最好温和地移动。
平滑化算法
将路径规划找出来的每个小格点设为x~x~ 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[i1][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（积分）是针对于有系统误差的机器人。
寻路器、平滑器、控制器可以应对大部分机器人的应用了，谷歌的还往里面加了料，它考虑了转向轮自有惯性的情况，