摘 要
本文研究了一种基于单片机自动控制的电动模型汽车。主要论述了自动循迹、避障、测距跟随等自动驾驶相关技术在模型车上的应用。
模型汽车以STM32为主控芯片，采用了多种传感器、驱动电机、控制舵机等检测与控制模块，实现了路径循迹行驶和检测避障的功能。本文研究了基于图像处理和PID控制技术的自动识别与避障跟随的解决方案，探索了智能驾驶的关键技术。基于机器视觉OpenMV模块，借助于OpenMV IDE软件和OpenCV库，运用Python语言设计验证了相关目标识别和追踪的算法，并实现了自动循迹、避障跟随的功能。模型汽车同时配备了蓝牙通信模块，具备遥控操作功能。经过实验测试，模型汽车能够在模拟路线上自动行驶，自动循迹、跟随、蓝牙遥控等功能效果良好。
关键词
STM32单片机；OpenMV；数字图像处理；循迹；跟随
目 录
第1章 绪论
1.1 课题研究背景
1.2 无人驾驶汽车的技术现状
1.3 课题主要研究内容
第2章 控制系统的总体设计方案
第3章 智能车系统硬件设计
3.1小车车身的选择
3.2 STM32单片机主控模块的选择
3.3 图像采集与处理模块的选择与接口设置
3.4电源模块的选择
3.5电机驱动模块的选择
3.6无线通信模块的选择
3.7超声波模块
第4章 系统软件设计
4.1系统软件总体流程设计
4.2图像处理软件设计
4.3电机PID调速软件的设计
第5章 实验结果与分析
5.1实验步骤与结果
5.2数据记录
5.3缺点分析与改进
第6章 总结
致谢
参考文献
附录一：原理图
附录二：程序
第1章 绪论
1.1 课题研究背景
随着科技的不断进步，尤其是高新技术产业的飞速发展，出现了许多智能化程度越来越高的产品，具有辅助无人驾驶功能的智能化汽车便是其中技术发展非常迅速的产品之一。
无人驾驶汽车的研究最早开始于20世纪。1925年，美国电子技术专家Francis P.Houdina展示了基于无线电控制的名为“American Wonder”的无人驾驶车辆，可以说它是世界上第一辆无人驾驶汽车。1956年，美国通用汽车研发了全球第一辆应用自动导航和安全驾驶系统的无人驾驶汽车。但是由于早期的无人驾驶汽车技术不成熟，而且由于成本价格昂贵，所以无人驾驶技术的研究一度陷入瓶颈，停滞不前。
20世纪90年代初英国也开始了无人驾驶汽车的研发计划，直到2010年才由英国技术先进的交通系统公司研发出了名为“Ultra”的无人驾驶汽车，并且希望以后可以投放到希斯罗机场作为出租车来运送旅客。法国著名的无人驾驶汽车当属命名为Cycad的无人驾驶汽车，它配备了世界上最为先进的激光传感器和GPS导航系统，可以准确的定位行驶目标，及时躲避各种障碍物，已经在法国试用区域内得到了比较成功的应用与成绩。此外，德国在无人驾驶汽车领域也颇有建树，其建议在现有的智能汽车上通过应用集成智能感知技术和决策装置把有人驾驶汽车改装成人机协同操作的无人驾驶汽车，大大改善了行驶安全性。
1.2 无人驾驶汽车的技术现状
近年来，特斯拉和谷歌等行业巨头在无人驾驶汽车的研发和应用中处于领先地位。特斯拉截至目前为止已经积累了超过10亿英里路程的研究数据，谷歌凭借其强大的人工智能处理和大数据处理能力很有希望在无人驾驶领域青出于蓝。
我国的无人驾驶技术研发可以追溯到上个世纪80年代末，经过了几十年的不懈努力，已经在部分领域居于世界先进水平。在无人驾驶的理论知识和科技研发上，上海交通大学、国防科技大学和清华大学居于国内无人驾驶技术研发第一梯队，研发了多款无人驾驶汽车控制系统。无人驾驶技术研究也是智能交通系统的关键技术。在商业领域中，阿里、百度和一汽等行业巨头也纷纷推出了无人驾驶汽车测试运营计划。2018年11月，百度和一汽共同发布L4级别无人驾驶试验车，并于2020年开始逐步量产。
现如今的无人驾驶汽车的研究方向主要有两种，一种是利用GPS、北斗等高精度导航以及激光雷达，全称“光检测与测距”，其原理是激光雷达设备向周围散射强烈的脉冲激光，测算出光反射回来所用的时间，然后根据激光测距原理计算出周围环境的三维数字模型。然后计算机通过对数据的分析并发出指令，从而使无人驾驶汽车安全稳定的运行,但激光雷达价格昂贵，代表公司：谷歌；另一种是基于机器视觉，利用摄像头等获取周围环境，并能精确分辨人，车，红绿灯，这样最大限度的利用了便宜资源，并且大大降低了成本价格，代表公司：特斯拉。
1.3 课题主要研究内容
为了学习与了解无人驾驶汽车的关键技术，结合单片机智能控制系统的特点，在基本的自动行驶电动模型小车基础上，重点研究基于图像处理的物体识别与规避技术，提高模型车的自动控制水平。该智能小车具有自主性、适应性、智能化程度高的特点。它应用了自动控制、机器视觉、数字图像处理、人工智能、机械设计制造以及信息的传输等多项各个领域先进的科技成果。由于技术与材料有限，目前此小车只能实现循迹跟踪小球的功能，但可以简单验证基于机器视觉的无人驾驶汽车巡线跟车行驶的可行性。同时小车具有蓝牙遥控的功能，可以实现一定距离内的远程操控。
第2章 控制系统的总体设计方案
小车控制系统以STM32F103C8T6单片机作为主控芯片，配置了各种功能模块，主要有：OpenMV机器视觉模块、电机与舵机模块、L298N电机驱动模块、HC05 主从一体蓝牙串口模块、12V电源模块、LM2596S直流可调降压稳压12V转5V模块等。电机驱动模块由12V电源模块直接供电。LM2596可调降压稳压模块将电源模块的电压由12V转为5V，然后向STM32最小单元和其他电路供电。
本项目重点研究了基于图像处理的物体识别技术，并在智能小车上验证基于机器视觉的循迹和追踪行驶功能。控制系统总体结构框图如图2-1所示。
图2.1 控制系统总体结构框图
利用OpenMV机器视觉模块识别目标物品的技术方法为：首先，要先利用OpenMV视觉模块采集目标物体（本项目以设计验证技术为主，以小球作为标识物）的图像，并将图像信息传给单片机。单片机根据收到的位置信息确定是否为目标物体。
OpenMV视觉模块识别目标物体，并实时测量智能车与标识物之间的距离，并将数据传回给单片机。单片机收到数据后及时处理，并通过PID控制算法来实时调节电机的转动速度，以此来控制小车的速度，保持与标识物的距离，以此来实现对小车的实时追随动作。由于技术原因，只能做到在空旷平坦的环境下实现对标记物的跟踪，希望能做到当小车与标记物之间有障碍物时能够避开障碍物，继续实现对标记物的实时追踪。
数字摄像头将采集的图像信息传送给OpenMV视觉模块进行图像信息的处理。摄像头的最高图像传输速率为60帧/s，完全能够保证图像的采集速率，协助控制小车的速度保持在一个合理的范围内。单片机收到采集的图像信息并经过一系列的算法运算，向舵机输出舵机应转角度，同时控制电机的转动速度，以此来实现循迹行驶的功能。图像处理应用了图像预处理、图像灰度化、图像滤波、图像二值化膨胀和腐蚀等技术。本设计采用直线检测的方法，最终实现了单线路循迹识别与追踪。以后，准备验证双线路循迹识别方法，进一步改进循迹效果。
第3章 智能车系统硬件设计
3.1小车车身的选择
智能小车为了适用于各种场合，应该注重车体的平衡性，使其重量分布均衡，重心最好居中，避免在崎岖的道路上发生侧翻，因此选用轮式+履带的行走地盘。4个车轮分别采用独立电机驱动，能够在作业环境中更好的移动，完成前进后退、左转右转、加速减速的行驶功能。车身如下图所示：
图3.1 小车车身设计
3.2 STM32单片机主控模块的选择
本设计选择了STM32单片机作为主控模块。该单片机具有ARM公司的高性能“Cortex-M3”内核和一流的外设：1微秒的双12位ADC，4兆位/秒的UART，18兆位/秒的SPI，18MHz的I/O翻转速度；同时功耗较低：在72MHz时消耗36mA（所有外设处于工作状态），待机时下降到2微安；还集成了复位电路、调压器、精确的RC振荡器等组成单元。
STM32单片机相对于51单片机来说运算速度是其几十倍且外围端口功能更多，有更加强大的开发功能。由于该单片机价格便宜，对于我们要做的非大型系统设计来说功能足够用，故此小车选择STM32F103C8T6作为主控芯片。
图3.2 STM32单片机主控模块
3.3 图像采集与处理模块的选择与接口设置
图像采集与处理模块采用机器视觉开发组件OpenMV。在进行开发时，可直接使用Python语言在OpenMV IDE上编程，并调用图像处理相关算法和Python库。OpenMV最高像素可达30万。
图3.3 视觉模块
图3.4 收发器模块
该模块通过UART2-TXD-232管脚与收发器对应管脚相连，收发器再通过UART1-TXD管脚与单片机PA9管脚相连，以此来完成单片机与OpenMV模块的信息交互。其中收发器起到协调单片机与外设，实现数据传送的同步。数据传输采用半双工传输。
该模块采集信息后将信息传给单片机，单片机再按上述方式传回该模块，UART2-TXD-232接收为0时为循迹模式，接收为1时为跟踪模式。
3.4电源模块的选择
电源模块采用了12V电池和12 V转5 V的降压和稳压模块。电池示电动模型车常用的驱动电源，具有供电电压稳定、使用方便的特点。智能车的电机驱动模块由12V电池直接供电。电源降压与稳压模块实现电池电压由12 V转到5 V的降压作用，向单片机和其他功能模块供电，满足4.25V到6V左右的电压要求。
图3.5 电源模块
3.5电机驱动模块的选择
L298N-stepper-motor是一种具有大电流、高电压特性的电机驱动芯片，其采用的是15脚的封装。它的主要特点是：额定功率为25W；输出电流足够大，最高可达3A；工作电压高，最高工作电压可以达到46V。其含有两个H桥的高电压大电流全桥式驱动器，用来驱动继电器线圈和步进电动机、直流电动机等负载；采用的是标准逻辑电平信号控制；有一个逻辑电源输入端，使其内部的逻辑电路部分在低电压下工作；具有两个使能控制端，在不受输入信号影响的情况下允许或禁止器件工作；可以外接用以检测电阻，将电阻的变化量反回给控制电路。电路图如下图所示：
。
图3.6 电机驱动模块
四个管脚分别与单片机PC0、PC1、PC2、PC3管脚相连，采用半双工传输方式。
对电机转速的控制采用脉宽调制。脉宽调制(PWM)是利用微处理器的数字输出来对模拟电路进行控制的一种非常有效的技术，广泛应用在从测量、通信到功率控制与变换的许多领域中。
由摄像头模块返回至主控制器的小车偏转角数据与设定值做差可以得到PID调节的输入偏差值, 然后将PID控制器的输出值转化为控制电机运行的PWM信号，完成对小车速度与方向的控制。
3.6无线通信模块的选择
无线通信模块选用HC05 主从一体蓝牙串口模块，实现与单片机之间的双向无线通信，完成远程遥控智能车的功能。
电路图如下图所示：
图3.7 无线通信模块
该模块通过UART1-TXD-232、UART1-RXD-232管脚与收发器对应管脚相连，收发器UART1-TXD、UART1-RXD管脚与单片机PA9、PA10管脚相连，并完成交互。
HC-05 蓝牙串口模块具有两种工作模式：命令响应模式和自动连接模式，在自动连接工作模式下模块又可分为主 、从和回环三种工作模式。
3.7超声波模块
由于本实验只考虑在简单环境进行测试，故采用HC系列超声波测距模块，其使用方便，便于调试，实现了小车测距跟随的功能。其跟随安全距离设定为20cm，若小车与小球距离改变，则由主控器控制电机速度，完成跟随操作。
电路图如下图所示：
图3.8 超声波模块
2管脚作为输出口与单片机PA6管脚相连，3管脚作为接收echo返回数据的输入口与单片机PA5管脚相连，采用IO口trig触发测距，模块可以发出8个40khz的方波，并实时检测有无信号返回，若有信号返回，则通过IO口echo输出一个高电平，高电平持续的时间就是超声波发出和返回的时间。
测试距离=（高电平持续时间/2）*声速（340米/秒）.
第4章 系统软件设计
4.1系统软件总体流程设计
首先进行所有设备的初始化，并分配资源。视觉模块OpenMV开始收集环境中的目标物体（本次使用了小球）的图像信息，并将信息做数字图像处理。如果判定为目标，则将信息传回给单片机，单片机接收到数据并完成目标的锁定，然后开始跟踪。在跟踪过程中，超声波模块不断测量小车与小球之间的距离，并控制车速，使它们之间的距离保持不变。如果小车与小球距离突然增大，则单片机发出指令，调整舵机的角度以及电机的转动速度，使小车加速，保持与小球的距离不变。同理，如果小车与小球距离突然减小，则单片机控制降低电机转速，使小车减速，保持与小球的距离不变。流程图如下：
图4.1 系统软件流程图
4.2图像处理软件设计
本设计在OpenMV视觉模块基础上，使用OpenMV IDE软件和OpenCV库完成图像处理。OpenCV库提供的许多常用视觉识别算法也为开发降低了难度。
视觉模块上电开启后，读取灰度图像，然后读取视频帧，是否为黑线，是，则开始循迹；否，再次扫描环境，直到锁定黑线。
视觉模块在扫描小球时，同时也扫描到了周围环境中的图像，为了更快地从图像中提取小球，需先对所有图像进行图像预处理。图像预处理的主要目的就是消除图像中噪声信息，增强有用信息。预处理包括：导入图像、去噪处理、图像增强、彩色图像转变成灰度图、灰度图转化成二值图、边缘检测/分割、直方图匹配/轮廓匹配。预处理工作完成后，得到了相应的二值化图像，然后将得到的图像与标记物作对比 ，若是则开始跟踪；若不是，则重新扫描周围环境，直到锁定标记物。
流程图如下图所示：
图4.2 循迹程序流程图
图4.3 跟踪程序流程图
4.3电机PID调速软件的设计
PID控制是通过对偏差信号进行比例、积分、微分运算的不同组合形式，适用于不同系统的控制。
电机PID控制主要实现小车在循迹时的方向和速度的控制, 即保持小车的前进方向与黑线的偏转角为零。把设定值调为零，由摄像头模块传回至主控制器的小车偏转角与设定值做差可以得到PID调节的输入偏差值, PID控制器输出值为控制电机运行的PWM信号。
位置PID控制器输出的PWM（脉宽调制）信号控制直流电机的运行。设定小车正常直线运动时的PWM阈值, 当小车转弯时，根据小车左右两轮电机对分别加减PID输出的PWM信号, 保证左右车轮实现差速转弯, 从而保证小车完成转弯, 并由摄像头模块实时返回偏转角。
流程图如下：
图4.4 电机PID调速程序流图
第5章 实验结果与分析
5.1实验步骤与结果
（1）在地上铺上黑线，将小车放在黑线上开始循迹；
（2）前方出现小球，小车转换为跟踪模式，开始跟踪小球；
（3）增大或减小小球与小车的距离，小车开始加速或减速保持与小球的距离，并测速打印传输到手机上；
实验成功，实现了循迹与跟踪功能。
5.2数据记录
表5.1 数据表
识别速度 | 与小球保持距离 | 识别准确率 |
20帧 | 20cm | 80% |
巡线偏差角度 | 调速 |
|
10度 | 正常 |
|
实验结论：
（1）识别速度由视野中像素点决定，大约为20帧，达到基本指标，60帧最佳；
（2）与小球保持的距离受单片机与外设间的处理速度影响，距离为20cm左右。当实际跟车时，要考虑安全性，需要更高识别速度的视觉模块，且需要更精准的算法使其能精确识别除了车以外其他的物体，如：人、交通灯等，且要安装激光雷达等高精度测距模块。
5.3缺点分析与改进
设计的缺点主要有两点，一是视觉模块的识别速度只有20帧，且受距离的影响视野中的像素点会有差异，所以识别速度会有所变化；二是超声波测距模块与单片机信息的传递受到处理速度的影响，实时距离测绘不太准确，造成与小球保持的距离和速度不太稳定。
改进方法:一：换用传输速率更高的视觉模块，以增加视觉模块的识别速度和准确率；二：精进PID控制算法，使其之间的差异更小，以此弥补设备的不足。
第6章 总结
本设计主要是验证基于机器视觉的无人驾驶的可能性。小车通过Openmv模块采集图像数据，运用数字图像处理技术提高目标识别率，应用超声波模块实时测量车间距离，采用PID控制算法输出控制小车运行的PWM信号，最终实现了循迹和跟踪的功能。但设计方案存在一些不足：车辆之间的距离、物体的不同外观尺寸都导致标识物的成像的像素点存在差异，影响了识别速度；受数据处理速度与外设的传输速率的影响，导致测距存在误差，进而影响电机速度的调控。后续准备运用更高传输速率的视觉模块和优化的PID算法来改进。
现今，国内无人驾驶技术发展前景最好的当属华为。华为拥有世界上最先进的5G技术，这无疑为未来彻底实现真正的无人驾驶奠定了坚实的基础。由于无人驾驶需要集感知、决策、执行为一体，所以需要处理大量的数据，现在的4G系统延迟为50ms明显达不到无人驾驶的要求，而5G的延迟仅为1ms，这在信息处理和传输速度上大大增强，极大的提高了无人驾驶技术的安全性。未来中国的无人驾驶技术必将走在世界前列。
附录一：原理图
附录二：程序
import sensor, image, time
from pid import PID
from pyb import Servo,UART,Pin,Timer
global FLAG
global flagNew
#模式的转化
FLAG = True #小球追踪
flagNew = True
#FLAG = False #巡线漠视
##############################舵机相关参数配置#####################################
pan_servo=Servo(3)
tilt_servo=Servo(4)
led = Pin('D9', Pin.OUT, Pin.PULL_NONE)
Key = Pin('D8', Pin.IN, Pin.PULL_UP)
size_threshold = 2000
#驻车指令
Break = bytearray([0x24,0x42,0x2C,0x35,0x30,0x23])#$B,50#
#前进
FOR = bytearray([0x24,0x42,0x2C,0x38,0x30,0x23])#$B,80#
#后退
REV = bytearray([0x24,0x42,0x2C,0x32,0x30,0x23])#$B,20#
#左转
Left = bytearray([0x24,0x43,0x2C,0x32,0x30,0x23])#$C,40#
#右转
Right = bytearray([0x24,0x43,0x2C,0x38,0x30,0x23])#$C,60#
#不转
NLR = bytearray([0x24,0x43,0x2C,0x35,0x30,0x23])#$C,60#
##################################End############################################
pwmA1 = Pin('A0')
pwmA2 = Pin('A1')
pwmB1 = Pin('B10')
pwmB2 = Pin('B11')
tim = Timer(2, freq=1000)
Left1 = tim.channel(1, Timer.PWM, pin=pwmB1,pulse_width_percent=0)
Left2 = tim.channel(3, Timer.PWM, pin=pwmB2,pulse_width_percent=0)
Ritht1 = tim.channel(2, Timer.PWM, pin=pwmA1,pulse_width_percent=0)
Ritht2 = tim.channel(4, Timer.PWM, pin=pwmA2,pulse_width_percent=0)
#镜头配置配置
sensor.reset() # Initialize the camera sensor.
sensor.set_pixformat(sensor.RGB565) # use RGB565.
if FLAG:
tilt_servo.angle(30)
sensor.set_framesize(sensor.QVGA) # use QQVGA for speed.
else:
sensor.set_framesize(sensor.QQVGA) # use QQVGA for speed.
tilt_servo.angle(45)
sensor.skip_frames(50) # Let new settings take affect.
sensor.set_auto_whitebal(False) #turn this off.
#sensor.set_hmirror(True)
#sensor.set_vflip((True))
#舵机角度初始化
tilt_servo.angle(50)
pan_servo.angle(0)
#串口
uart = UART(1, 19200)
uart.init(9600, bits=8, parity=None, stop=1) #8位数据位，无校验位，1位停止位、
clock = time.clock() # Tracks FPS.
def find_max(blobs):
max_size=0
for blob in blobs:
if blob[2]*blob[3] > max_size:
max_blob=blob
max_size = blob[2]*blob[3]
return max_blob
def run(left_speed, right_speed):
#print("left_speed",left_speed)
#print("right_speed",right_speed)
if left_speed < -100:
left_speed = -100
elif left_speed > 100:
left_speed = 100
if right_speed < -100:
right_speed = -100
elif right_speed > 100:
right_speed = 100
if left_speed > 0:
Left1.pulse_width_percent(abs(left_speed))
Left2.pulse_width_percent(abs(0))
else:
Left1.pulse_width_percent(abs(0))
Left2.pulse_width_percent(abs(left_speed))
if right_speed > 0:
Ritht1.pulse_width_percent(abs(right_speed))
Ritht2.pulse_width_percent(abs(0))
else:
Ritht1.pulse_width_percent(abs(0))
Ritht2.pulse_width_percent(abs(right_speed))
i = 0
while(True):
clock.tick() # Track elapsed milliseconds between snapshots().
flagNew = Key.value()
if FLAG != flagNew:
# print(i)
flagNew = Key.value()
FLAG = flagNew
if Key.value() == True:
i+=1
if i == 2:
i = 0
#print(i)
##############################色块追踪相关参数配置######################################
if i == False:
led.value(1)
sensor.set_framesize(sensor.QVGA) # use QQVGA for speed.
THRESHOLD = (94, 55, 15, -10, 9, 25)#黄色
##THRESHOLD = (50, 34, 16, 39, 21, 0)#红色
#THRESHOLD = (50, 32, 11, -23, -39, -18)#蓝色
pan_pid = PID(p=0.07, i=0.001, imax=90) #脱机运行或者禁用图像传输，使用这个PID
tilt_pid = PID(p=0.05, i=0.008, imax=90) #脱机运行或者禁用图像传输，使用这个PID
img = sensor.snapshot() # Take a picture and return the image.
# img = sensor.snapshot() # Take a picture and return the image.
blobs = img.find_blobs([THRESHOLD],pixels_threshold=200)
if blobs:
# print("blobs")
max_blob = find_max(blobs)
pan_error = max_blob.cx()-img.width()/2
tilt_error = max_blob.cy()-img.height()/2
# print("pan_error: ", pan_error)
img.draw_rectangle(max_blob.rect()) # rect
img.draw_cross(max_blob.cx(), max_blob.cy()) # cx, cy
pan_output=pan_pid.get_pid(pan_error,1)/2
tilt_output=tilt_pid.get_pid(tilt_error,1)
pan_servo.angle(pan_servo.angle()-pan_output)
tilt_servo.angle(tilt_servo.angle()+tilt_output)
x = 0x32
high, low = x >> 4, x & 0x0F
#前进与后退控制
if max_blob.pixels()>300 and max_blob.pixels()<2000:
power = ((2000- max_blob.pixels())/1700)*100
if power >100:
power = 100
uart.write(FOR)
elif max_blob.pixels()>5000:
power = -((max_blob.pixels()- 5000 )/6000)*100
if power <-100:
power = -100
uart.write(REV)
else:
uart.write(Break)
power = 0
#run(power,power)
# print("power",power)
#左右控制
if pan_output < -3:
uart.write(Left)
LeftPower = -60
RightPower = 60
elif pan_output > 3:
uart.write(Right)
LeftPower = 60
RightPower = -60
else:
uart.write(NLR)
LeftPower = 0
RightPower = 0
run(power-LeftPower,power-RightPower)
else:
run(0,0)
# print("LeftPower",LeftPower)
# print("RightPower",RightPower)
#uart.write(img.compressed(quality=50)) 图像输出
# print("max_blob.pixels",max_blob.pixels())
else:
##############################巡线相关参数配置######################################
led.value(0)
sensor.set_framesize(sensor.QQVGA) # use QQVGA for speed.
tilt_servo.angle(45)
pan_servo.angle(0)
THRESHOLD = (27, 9, -69, 31, 76, -31)#黑色线27, 9, -69, 31, 76, -31
rho_pid = PID(p=0.65, i=0.3)
theta_pid = PID(p=0.001, i=0.2)
img = sensor.snapshot().binary([THRESHOLD])
line = img.get_regression([(100,100,0,0,0,0)], robust = True)
if (line):
img.draw_line(line.line(), color = 127)
rho_err = abs(line.rho())-img.width()/2
if line.theta()>90:
theta_err = line.theta()-180
else:
theta_err = line.theta()
# print("line.magnitude",line.magnitude())
if line.magnitude()>6:#匹配线宽
rho_output = rho_pid.get_pid(rho_err,1)
theta_output = theta_pid.get_pid(theta_err,1)
output = rho_output+theta_output
# print("output",output)
run(70+output, 70-output)
else:
run(0,0)
pass
else:
run(0,0)
pass
# print(clock.fps())
