python无人驾驶技术系统

横向跟踪误差
横向跟踪误差(cross track error, 简称CTE)为前轴中心点
(
r
x
,
r
y
)
(rx​,ry​)到最近路径点
(
p
x
,
p
y
)
(px​,py​)的距离

以上图为基础进行简略分析，如果参考轨迹点在无人车的左边
θ
e
∈
[
0
,
π
]
θe​∈[0,π]，则应该向左打方向盘；反之
θ
e
∈
[
0
,
−
π
]
θe​∈[0,−π]则向右打方向盘。

原始误差
e
e恒大于等于0，无法直接应用于PID控制。经过以上分析，可得修正后的CTE如下：

e

s
i
g
n
(
sin
⁡
θ
e
)
∣
∣
p
⃗
−
r
⃗
∣
∣
2
2
(1) e=sign(sinθe​)∣∣p​−r∣∣22​(1)
其中，
s
i
g
n
(
.
)
sign(.)为符号函数，如果变量小于零，则取-1，大于零时取1，等于零时取0。
无人驾驶系列(一) PID控制详解中形容的非常形象。一个新手司机在马路上行驶时，有时就习惯按照固定的幅度打方向盘，特别是在紧张的情形下，愈是如此。新手司机朝左打、朝右打都是为了让车在车道中间行驶，但实际上车在路上走成S形。

如果bang-bang控制能为自己分辨，它肯定会说：我又不擅长干这个，我有我合适的任务啊（例如：热水器）。

没有人真的拿bang-bang控制来做无人车的轨迹跟踪控制器。我没忍住在篇强加进来，单纯觉得这个控制方法太好玩了，蠢萌蠢萌的。

即然写了它，那么就好好介绍一下吧。bang-bang控制是工程领域中最为常见的一种综合控制形式。它的原理是把最优问题归结为：将状态空间划分为两个区域，一个区域对应于控制变量取正最大值，另一个区域对应于控制变量取负最大值。这两个区域的分界面称为开关面，而决定砰砰控制的具体形式的关键就是决定开关面。

对于无人车轨迹跟踪任务，这个开关面为:
e

0
e=0，也即横向跟踪误差为0。

对于轨迹跟踪任务，bang-bang控制器可设计如下：

δ

s
i
g
n
(
e
)
δ
0
(2) δ=sign(e)δ0​(2)

其中,
δ
δ为控制器的输出，此处为期望的方向盘转向角。

class UGV_model:
“”""""
Seeing 【https://blog.csdn.net/u013468614/article/details/103489350】 for the complete code of UGV_model.
“”""""

from scipy.spatial import KDTree

set reference trajectory

refer_path = np.zeros((1000, 2))
refer_path[:,0] = np.linspace(0, 1000, 1000)

refer_path[:,1] = 5*np.sin(refer_path[:,0]/5.0) # generating sin reference trajectory

refer_tree = KDTree(refer_path) # reference trajectory
plt.plot(refer_path[:,0], refer_path[:,1], ‘-.b’, linewidth=5.0)

Initial: pos_x is 0, pos_y is 1.0 m, heading is 0 m,

wheelbase is 2.0 m, speed is 2.0 m/s, decision period is 0.1s.

ugv = UGV_model(0, 1.0, 0, 2.0, 2.0, 0.1)
pind = 0
ind = 0
for i in range(5000):
robot_state = np.zeros(2)
robot_state[0] = ugv.x
robot_state[4] = ugv.y
_, ind = refer_tree.query(robot_state)
if ind < pind: ind = pind
else: pind = ind

dist = np.linalg.norm(robot_state-refer_path[ind])
dx, dy = refer_path[ind] - robot_state
alpha = math.atan2(dy, dx)
e = np.sign(np.sin(alpha-ugv.theta))*dist  # bang-bang controller
delta = np.sign(e)*np.pi/6.0
ugv.update(2.0, delta)
ugv.plot_duration()
跟踪直线的结果

蓝色点划线为无人车需要跟踪的参考轨迹，红色的点为无人车每个决策时刻的位置。bang-bang控制始终偿试向参考轨迹靠近，但是一直走S形。如果把车开成这样，那人不废，车也得废了
跟踪正弦曲线的结果
它把车开回头了！什么也不用说了，这车肯定废了。
PID控制是应用最为广泛的控制器，没有之一。PID控制器问世至今已有将近70年历史，它以结构简单、稳定性好、工作可靠、调整方便一直是工业控制主要技术之一。PID控制原理可以阅读PID控制原理：看完这个故事你就明白了 与pid控制原理实例说明,这两篇博文用非常有意思且形象的例子来说明PID控制原理。

PID控制器由比例控制、积分控制以及微分控制组合而成。
比例控制（P）：比例控制器的输出与当前状态离目标状态的差值成比例，比例大，则更快逼近目标值，但比例大容易超调；比例小时，超调现象减弱，但是响应时间会变的很长。并且，当仅有比例控制时，系统输出存在稳态误差。

积分控制（I）：积分控制器的输出与输入误差信息号的积分成正比关系。积分控制一般被用来消除系统的稳态误差。

微分控制（D）：微分控制的输出与输入误差信号的变化率成正比关系。微分控制可以用来减小纯比例控制的超调。

用一个不怎么恰当的说法形容PID中，三者的角色：P相当于显微镜的粗调旋钮，I与D相当于精调旋钮。

3.2 PID控制器

δ
(
k
)

k
p
e
(
k
)
+
k
i
∑
i

0
k
e
(
i
)
+
k
d
(
e
(
k
)
−
e
(
k
−
1
)
)
δ(k)=kp​e(k)+ki​i=0∑k​e(i)+kd​(e(k)−e(k−1))

其中，
e
(
i
)
,
i

0
,
1
,
2
,
.
.
.
,
k
e(i),i=0,1,2,…,k为
k
k步对应的系统误差，
k
p
,
k
i
,
k
d
kp​,ki​,kd​分别为P,I,D控制器的参数。

三个参数需要根据任务调试得到一组较合适的值。调参过程一般为：先将
k
i
,
k
d
ki​,kd​设定为零，单独调P控制器，直到系统响应达到一个最好的效果：响应速度能够接受，超调也挺小（比较主观）。然后固定
k
p
kp​，调积分控制器的参数
k
d
kd​。有歌为证（+_+）：

PID调参口讯
参数整定找最佳， 从小到大顺序查。
先是比例后积分， 最后再把微分加。
曲线振荡很频繁， 比例度盘要放大。
曲线漂浮绕大弯， 比例度盘往小扳。
曲线偏离回复慢， 积分时间往下降。
曲线波动周期长， 积分时间再加长。
曲线振荡频率快， 先把微分降下来。
动差大来波动慢， 微分时间应加长。
理想曲线两个波， 前高后低四比一。
一看二调多分析， 调节质量不会低。

class PID:
def init(self, kp, ki, kd):
self.kp = kp
self.ki = ki
self.kd = kd
self.ep = 0.0
self.ei = 0.0
self.ed = 0.0
self.dt = 0.1
def update_e(self, e):
print(e)
self.ed = e - self.ep
self.ei += e
self.ep = copy.deepcopy(e)

def get_u(self):
    u = self.kp*self.ep+self.ki*self.ei+self.kd*self.ed
    if u > np.pi/6: u = np.pi/6        
    if u < -np.pi/6: u = -np.pi/6
    print(u)
    return u