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