【树莓派小车】【pid控制+超声测距】直道竞速实验报告

电子系统导论实验

直道竞速（PID+超声）

---

一、实验目的：

​ 1.通过综合运用本学期学习的内容，实现小车的直道竞速。

​ 2.在小车不撞墙的前提下尽可能快的跑到终点。

---

二、实验原理：

​ 1.PID自动控制：

​ 此部分已在之前的实验报告中详细说明，这里对此做简单描述。

​ PID调节是一种闭环控制的方式，基于反馈调节实现（示意图见图2.1）。在本实验中，使用PID控制的方式，是输⼊偏差量（速度），计算出调整量（占空⽐），实现对速度的控制。在编程实现时，为了方便调整参数，将PID写成一个对象，用两个实例来控制左右轮。通过给左右轮设定相同的速度，使得左右轮速度接近，就可以跑出近似的直线。

图2.1 PID控制原理示意图

​ 简单来讲，PID控制就是利用三个参数$K_p,K_i,K_d$，来调整三个对应因素对调整量的影响。但是在之前的实验中，我们发现仅通过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+超声的方案。

图3.1 赛道示意图

​ 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值的变化量，得到$\Delta D=Dist-pre\_dist$。

2.给予每次变化一个状态值，如果$\Delta 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）。

图4.1 小车方向偏离的例子

​ 如果我们检测到了这个线性变化，根据于距离的增大与减小，就可以确定小车的方向是左倾还是右倾。因此，通过判断Dist是否在连续减少或连续增大，我们可以检测到小车方向的偏移。

​ 这个时候，我们采取调控，如果小车向左偏，则右轮减速，小车右偏，则左轮减速，达到自动方向扳正的效果。如果扳正力度不够，Dist依然在向相同方向偏移，根据我的算法，小车会进一步修正，且两次减速的效果叠加，修正力度加大。修正后，由于pid自动控制，减速的轮子又会快速恢复原来的转速，继续以近似的直线前进。

​ 以上便是该算法修正小车方向的原理，实质是修正小车方向，使得小车前进方向于墙壁延伸方向平行。在原来pid控制的基础上添加此修正算法，给予了小车直行的保障，提高了竞速的成功率，而原有pid控制的保留，也一定程度上保证了小车的速度，可谓一举两得。

​

​ （2）该算法的特性以及参数调整：

​ 该算法有一个值得注意的特性：那就是不受部分障碍物的影响。比如，在走廊墙壁上突然有一个凹进去的门框（见图4.2），使得小车测距突然增大，但这不会对小车方向修正产生影响。因为突然变化的距离并不会触发修正，必须要连续的变化才会触发。因此对小车测试的时候，场地的选取会更加自由，可以自己找地方测试效果，不必依赖于严格的场地。

图4.2

​ 参数k的意义：当小车测距连续增大k次或减小k次之后，开始方向修正。因此k是一个能反馈修正灵敏度的值，如果k过小，将会出现频繁修正，甚至可能会有错误修正，如果k过大，将会出现修正不及时的情况。因此需要调整k为一个合适值。

​ 选取适当的修正函数：小车的方向修正，依靠于减少对应电机的占空比，那么如何确定减少后的值，便是一个值得思考的问题，也就是说，需要找一个靠谱的修正函数。在实践中发现，将当前占空比乘以一个0.7左右的系数作为修正后的占空比是一个不错的选择。当然，也可以尝试其他的修正函数。

​ （3）算法漏洞

​ 这个算法看起来好像很不错，但事实上有漏洞。如果要得到成功的结果，需要满足前提条件：小车在pid的控制下能够基本达到直线前进。

​ 如果小车在pid控制下无法直线前进，而是发生转弯的话，可能会出现图4.3的情况。即当小车持续右转时，由于超声设备固定在了小车上，于是设备也会右偏出现明显倾斜。此时小车的方向是右偏，但是与墙壁距离却在增大，于是上述算法会认为小车的方向是左偏，进而抑制右轮转动，小车更加右偏，于是失控撞墙。

图4.3 该算法下可能产生的失控的例子

​ 3.期望表现：

​ 搭载了超声测距和修正算法的小车，调整好参数后，最理想的情况大致如下（图4.4）。

图4.4 理想路径

​ 当然如果参数调的不是很好，摆动次数稍微多了一点也是可以接受的，到达目的地应该还是比较容易的。

---

五、竞速结果与总结思考：

​ 最终结果：三次竞速都跑到了终点，最快15.13s

​ 竞速视频：https://www.bilibili.com/video/bv1Pi4y1V7dp

​ 本次实验中，我们完成了树莓派小车的直道竞速，我们使用超声测距以求取低风险的目标得到了实现。在算法的设计上也算是比较成功，用较少量的代码，较为高效的完成了任务。

​ 不过还是有很多地方可以深入挖掘和研究。比如可以尝试使用PID控制小车与墙的距离，去具体研究各种方法的优劣势。甚至还可以尝试一些其他能提高竞速成绩的方法。竞速当天看到有个小组把小车的顶篷掀了，设备全部安装在底盘上，跑的速度非常快（据说13s左右），小车非常灵活，宛如敞篷跑车。这一点优化方式我们组是的确没想到。

​ 总之，还有很多地方值得我们继续学习！

---

关于我的博客想说的：
我把一些关键性的实验报告/学习报告发在博客上，一个是便于记录整理自己做过的实验/项目/作业，同时也希望与大家学习交流，也能够为后来的同学提供一些参考价值。

如有错误和不足，欢迎大家指正批评。