【树莓派小车】【pid控制+超声测距】直道竞速实验报告
电子系统导论实验
直道竞速（PID+超声）
一、实验目的：
1.通过综合运用本学期学习的内容，实现小车的直道竞速。
2.在小车不撞墙的前提下尽可能快的跑到终点。
二、实验原理：
1.PID自动控制：
此部分已在之前的实验报告中详细说明，这里对此做简单描述。
PID调节是一种闭环控制的方式，基于反馈调节实现（示意图见图2.1）。在本实验中，使用PID控制的方式，是输⼊偏差量（速度），计算出调整量（占空⽐），实现对速度的控制。在编程实现时，为了方便调整参数，将PID写成一个对象，用两个实例来控制左右轮。通过给左右轮设定相同的速度，使得左右轮速度接近，就可以跑出近似的直线。
简单来讲，PID控制就是利用三个参数 K p , K i , K d K_p,K_i,K_d Kp,Ki,Kd，来调整三个对应因素对调整量的影响。但是在之前的实验中，我们发现仅通过PID的调整难以使小车达到稳定的直行。在实际情况下，地形、风速、小车电量、车轮磨损情况等都可能对PID的参数产生影响，这使得小车直行变得更加困难，在最后的竞速中可能会遇到各种不确定因素，仅使用PID控制，将具有一定的风险。
2.超声测距
在前面的实验中，我们用到了树莓派的I2C接口，使用KS103超声测距器件进行了超声测距的初步实验。该器件功能强大，编程实现方便，最大量程达5m左右，而竞速直道宽度约为2m，该超声部件能够较好的测量墙壁的距离。
基于wiringpi库函数，可以使用树莓派python编程实现KS103超声部件的测距，测距周期最小可达33ms，因此非常适合于实时测距。
超声测距在直道竞速中具有重要意义，如果小车搭载了超声装置，那么树莓派获得的信息将不止于小车两轮的速度，可以进一步获得小车在赛道上的位置信息。考虑结合超声测距与PID控制，有望让小车在直道竞速中表现稳定。
三、实验分析与方案分析：
1.对直道竞速项目的分析：
任务：小车从赛道起点线出发，通过程序自动驾驶，到达终点线。期间不允许撞墙。
赛道示意图见图3.1.
赛道的一些特点：长度大约为一个教室（12格地砖），宽度大约为2m。中央位置有一条黑线，黑线的摩擦系数和地砖不同。
竞速的可选方案：pwm电机驱动直接跑、pwm+pid控制、pwm+pid+超声、pwm+pid+图像识别
考虑到稳定性以及反馈的及时性，我们计划使用pwm+pid+超声的方案。
2.竞速实现方案的思考与分析：
在考虑实现方案时，值得思考的是如何结合pid控制超声测距。下面是我们思考过的方案。
（1）中断调控：
使用超声实时测量小车与右侧墙壁的距离，使用PID控制让小车直行。当检测到小车距离墙壁过近 或过远时，中断PID控制，让小车进入一个事先设定好的调节模式，该模式下两轮的占空比和该模式持 续的时间都需要手动调参，该模式让小车改变偏离方向，调节结束后继续PID控制。
该方案下，小车具备了自主判断偏离情况和调节方向的能力，缺陷是会增加调参任务量，多次的模 式切换可能会降低小车速度。
与前面的PID控制相同，该方案的调参也受到地形、电量、风速等影响，该方案的稳定性有待考 究。
（2）添加基于距离的PID实例：
使用超声设备实时测量小车与右侧墙壁的距离。
从PID对速度的控制获取灵感，在程序中添加两个PID实例，通过设定目标距离，自动控制左右 轮，使得小车保持与墙壁的目标距离，最终实现直行。
详细点来说，在这两个PID实例中，目标量和当前量都换做小车与墙壁的距离，通过偏差量计算出 合适的调节值，分别调节左右电机占空比，最终达到对距离的自动控制，实现直行。
该方法是一个很好的想法，充分认识到了PID控制的本质，如果能调节出合适的参数的话或许是一 个较好的方案。但是，该方案需要进一步思考：添加了对距离的PID控制之后是否还需要保留对速度的 PID控制？若保留，当两类PID控制产生矛盾的时候该怎么处理？若不保留基于速度的PID，仅根据超声 设备的信息进行距离调控，小车是否能比原先的基于速度的PID调控跑得更好？可以发现，该方案的实 现需要进一步的试验，将带来更大的工作量，同时对试验场地的限制也比较大，需要一段足够长的墙 壁，且没有任何障碍物才能进行试验。在有限的时间内，实现此方案不太现实，但可以作为后续研究的 方向。
（3）即时修正方位：
使用超声设备实时测量小车与右侧墙壁的距离。
使用PID自动控制使得两轮速度向设定值趋近，用超声测距检测小车横向的偏移情况，若监测到小 车偏移，则立即调占空比，使得一侧轮子减速，修正小车方向。由于PID控制的存在，减速的轮子将很 快回到设定速度。
在这个方案里，PID控制是保证小车按照目标速度去跑，同时用所测距离修正小车方位。该方法是 对PID自动控制和超声测距进行一个简单的结合，代码量小，思路简单，我们最终采用了该方法，在实 验设计中，将给出该方法的全面分析。
四、实验设计与分析：
1.基于上面的（3）方案，给出算法设计：
完整代码见pid_control&ultrasound_control_NSH.py (这个链接是假的)
在我们的方案里，超声测距是pid控制的一个辅助。
首先我们要解决超声测距的问题，我们希望小车能够实时测距：
我们找到wiringpi实现超声测速的代码：
import wiringpi as wpi
address = 0x74 #i2c device address
h = wpi.wiringPiI2CSetup(address) #open device at address
wr_cmd = 0xb0 #range 0-5m, return distance(mm)
#rd_cmd = 0xb2
##range 0-5m, return flight time(us), remember divided by 2
try:
while True:
wpi.wiringPiI2CWriteReg8(h, 0x2, wr_cmd)
wpi.delay(1000) #unit:ms MIN ~ 33
HighByte = wpi.wiringPiI2CReadReg8(h, 0x2)
LowByte = wpi.wiringPiI2CReadReg8(h, 0x3)
Dist = (HighByte << 8) + LowByte
print('Distance:', Dist/10.0, 'cm')
except KeyboardInterrupt:
pass
print('Range over!')
这一部分逻辑很显然。初始化设备之后，while true里面的是对距离实时的测量，该代码的设定参数是1000ms测量一次，然后把距离获取出来，输出出来。
但是这份代码的任务是只测量距离，而且是死循环，因此不能直接植入到pid控制的代码中。为了做到实时测距，我们可以效仿老师给的pid_control.py里面实时测量轮子速度的方法——多线程！
开一个线程来反复执行这段测速代码，并且将距离测量值反馈给全局的变量Dist，这样任何需要知道距离的时候，只需要访问Dist就可以获得当前距离的测量值。也就是说，用一个线程来实时更新Dist以保证Dist是当前的距离值。
这部分代码如下：
import wiringpi as wpi
address = 0x74
h = wpi.wiringPiI2CSetup(address)
wr_cmd = 0xb0
Dist = 100.0
def get_dist():
global Dist
while True:
wpi.wiringPiI2CWriteReg8(h, 0x2, wr_cmd)
wpi.delay(100) #unit:ms MIN ~ 33（每0.1s测一次）
HighByte = wpi.wiringPiI2CReadReg8(h, 0x2)
LowByte = wpi.wiringPiI2CReadReg8(h, 0x3)
Dist = ((HighByte << 8) + LowByte)/10.0
thread2=threading.Thread(target=get_dist)#开一个线程来执行get_dist，实现实时测距
thread2.start()
至此，实时测距已经实现，并且访问这个距离值也十分简便，调用变量Dist即可。
接下来，考虑对pid控制的修正：
在原来的pid控制程序当中，每0.1s，会使用pid调控占空比。现在我们要在此基础上加入测距结果对电机占空比的调控。
首先来看pid_control.py这一部分的代码：
try:
while True:
pwma.ChangeDutyCycle(L_control.update(lspeed))#调左轮
pwmb.ChangeDutyCycle(R_control.update(rspeed))#调右轮
x.append([i])
y1.append(lspeed)
y2.append(rspeed)
time.sleep(0.1)
i+= 0.1
print ('left: %f right: %f lduty: %f rduty: %f'%(lspeed,rspeed,L_control.pre_duty,R_control.pre_duty))
except KeyboardInterrupt:
pass
可以发现，用于调控占空比的就两行，其余的内容是输出当前参数和用于绘图。
现在加入我的修正算法：
1.每隔0.1s，检测小车与墙壁的距离Dist值的变化量，得到 Δ D = D i s t − p r e _ d i s t \Delta D=Dist-pre\_dist ΔD=Dist−pre_dist。
2.给予每次变化一个状态值，如果 Δ D > 0 \Delta D>0 ΔD>0，状态值为1，否则状态值为0
3.如果连续k次及以上的状态都是1，则降低右轮占空比，若连续k次及以上的状态都是0，则降低左轮占空比。（k是一个整数经验参数）
代码实现如下：
try:
while True:
if (Dist-pre_dist > 0):#获取当前的偏移状态
nowstate = 1
else:
nowstate = 0
if (nowstate == prestate):
acc+=1#累计连续偏移的次数
else:
acc=0
if(acc > 6):#如果小车连续k次以上偏移，则修正
if (nowstate == 0):
pwma.ChangeDutyCycle(L_control.pre_duty*0.7)#减小占空比
else:
pwmb.ChangeDutyCycle(R_control.pre_duty*0.6)
else:#否则还是继续用pid调控
pwma.ChangeDutyCycle(L_control.update(lspeed))
pwmb.ChangeDutyCycle(R_control.update(rspeed))
prestate=nowstate
pre_dist=Dist
#-------------------------------上面的部分是核心
x.append([i])
y1.append(lspeed)
y2.append(rspeed)
time.sleep(0.1)
i+= 0.1
print ('left: %f right: %f lduty: %f rduty: %f distance: %f'%(lspeed,rspeed,L_control.pre_duty,R_control.pre_duty, pre_dist))
except KeyboardInterrupt:
pass
加入此算法后，神奇的事情就发生了：小车能够在行驶中自动的调整方向，感觉智能了起来。感觉添加的代码量也不是很大。
算法设计到此结束（其余部分内容和课件给的pid_control.py一样，未作修改）。
2.对上面的算法设计，给出可行性分析：
（1）该算法的意义：
在这个算法中，我们使用到了每个时刻小车与右侧墙壁的距离。实际上，我们真正使用的，是距离的变化值。考虑到，在pid的控制下，小车能够走出一个近似的直线，而不能保证前进方向平行于墙壁。那么，如果小车存在倾斜的情况，在前进过程中，小车与右侧墙壁的距离，一定是接近于线性变化且连续的（见图4.1）。
如果我们检测到了这个线性变化，根据于距离的增大与减小，就可以确定小车的方向是左倾还是右倾。因此，通过判断Dist是否在连续减少或连续增大，我们可以检测到小车方向的偏移。
这个时候，我们采取调控，如果小车向左偏，则右轮减速，小车右偏，则左轮减速，达到自动方向扳正的效果。如果扳正力度不够，Dist依然在向相同方向偏移，根据我的算法，小车会进一步修正，且两次减速的效果叠加，修正力度加大。修正后，由于pid自动控制，减速的轮子又会快速恢复原来的转速，继续以近似的直线前进。
以上便是该算法修正小车方向的原理，实质是修正小车方向，使得小车前进方向于墙壁延伸方向平行。在原来pid控制的基础上添加此修正算法，给予了小车直行的保障，提高了竞速的成功率，而原有pid控制的保留，也一定程度上保证了小车的速度，可谓一举两得。
（2）该算法的特性以及参数调整：
该算法有一个值得注意的特性：那就是不受部分障碍物的影响。比如，在走廊墙壁上突然有一个凹进去的门框（见图4.2），使得小车测距突然增大，但这不会对小车方向修正产生影响。因为突然变化的距离并不会触发修正，必须要连续的变化才会触发。因此对小车测试的时候，场地的选取会更加自由，可以自己找地方测试效果，不必依赖于严格的场地。
参数k的意义：当小车测距连续增大k次或减小k次之后，开始方向修正。因此k是一个能反馈修正灵敏度的值，如果k过小，将会出现频繁修正，甚至可能会有错误修正，如果k过大，将会出现修正不及时的情况。因此需要调整k为一个合适值。
选取适当的修正函数：小车的方向修正，依靠于减少对应电机的占空比，那么如何确定减少后的值，便是一个值得思考的问题，也就是说，需要找一个靠谱的修正函数。在实践中发现，将当前占空比乘以一个0.7左右的系数作为修正后的占空比是一个不错的选择。当然，也可以尝试其他的修正函数。
（3）算法漏洞
这个算法看起来好像很不错，但事实上有漏洞。如果要得到成功的结果，需要满足前提条件：小车在pid的控制下能够基本达到直线前进。
如果小车在pid控制下无法直线前进，而是发生转弯的话，可能会出现图4.3的情况。即当小车持续右转时，由于超声设备固定在了小车上，于是设备也会右偏出现明显倾斜。此时小车的方向是右偏，但是与墙壁距离却在增大，于是上述算法会认为小车的方向是左偏，进而抑制右轮转动，小车更加右偏，于是失控撞墙。
3.期望表现：
搭载了超声测距和修正算法的小车，调整好参数后，最理想的情况大致如下（图4.4）。
当然如果参数调的不是很好，摆动次数稍微多了一点也是可以接受的，到达目的地应该还是比较容易的。
五、竞速结果与总结思考：
最终结果：三次竞速都跑到了终点，最快15.13s
竞速视频：https://www.bilibili.com/video/bv1Pi4y1V7dp
本次实验中，我们完成了树莓派小车的直道竞速，我们使用超声测距以求取低风险的目标得到了实现。在算法的设计上也算是比较成功，用较少量的代码，较为高效的完成了任务。
不过还是有很多地方可以深入挖掘和研究。比如可以尝试使用PID控制小车与墙的距离，去具体研究各种方法的优劣势。甚至还可以尝试一些其他能提高竞速成绩的方法。竞速当天看到有个小组把小车的顶篷掀了，设备全部安装在底盘上，跑的速度非常快（据说13s左右），小车非常灵活，宛如敞篷跑车。这一点优化方式我们组是的确没想到。
总之，还有很多地方值得我们继续学习！
关于我的博客想说的：
我把一些关键性的实验报告/学习报告发在博客上，一个是便于记录整理自己做过的实验/项目/作业，同时也希望与大家学习交流，也能够为后来的同学提供一些参考价值。
如有错误和不足，欢迎大家指正批评。