- 清华大学自动化系科技营-暨智能机器人挑战赛-学习方略
- 福来全向视觉智能车 | LQ_MV4 的使用
- 福来全向视觉智能车 | 车模构成与组
- 福来全向视觉智能车 | 开发软件 OpenMV IDE
- 福来全向视觉智能车 | 比赛方案与车模控制
01 车模运动模型
一、比赛任务
前面的学习都是为了本章任务服务的，这一章开始我们将开始了解比赛任务。
- 任务一：控制小车完成循黑线行驶；
- 任务二：控制小车在循黑线行驶的过程中躲避黑线上的障碍无（虚拟障碍，绿色标识）；
- 任务三：控制小车从赛道出发点开始循黑线并躲避障碍，最终到达赛道中心停车位置准确停车。
二、运动模型
在进入任务之前，我们先来了解一下《 全向福来三轮移动平台的运动模型分析 》（这篇分析文章也会放在资料中，以供参考）。文章内全向三轮移动平台与龙邱科技福来轮全向三轮车的基本结构基本相似，故可以做力学分析参考，我们的车模在向前直行运动时正好与该文章中所提的相反，文章中以一个轮作为前轮，两个后轮合力提供推进动力，而接下来我们将使用与其恰好相反得控制方式，即两轮拖一轮，前面两个轮子得合力提供前向动力，差速进行加后轮辅助进行转向和旋转绕行运动。
以下列举几个后续实验或比赛中会用到的几个运动示意（简单示意，不是完整受力分析，了解即可）：
▲ 图1.2.1 向前执行示意图
▲ 图1.2.2 左转前行示意图
▲ 图1.2.3 右转前行示意图
▲ 图1.2.4 环绕示意图
02 任务一：巡线小车
关于电机的控制，可以简单的通过下面模型来了解。正反转只是相对的。
观察上图，不难发现电机的正反转不仅与程序中所给出的正负值有关，还与硬件上的电机接线有关。如果发现程序中给相同的值，但是电机转向不一样，可以将电机与驱动板的接线交换一下。
如果同学们直接使用整车演示例程，需要注意三个电机的定义是否与实际接线一致，摄像头安装角度和高度是否基本和上图差不多，不过这些在装车视频中有说到，也就是说程序中的“motor1”实际控制的是否为视觉模块左前面的轮子，这个需要在装车接线时注意，示例程序与装车视频中安装方式以及本章第三节电机使用接线线序一致。需要确定电机驱动三个蓝色接线柱分别对应的是哪个电机如果完全按照视频接线，基本没多大问题。
前面已经学习了识别色块，通过image.find_blobs可以得到一个对象“blob”，其具有一写有关色块的位置信息。接下来就可以通过位置信息来确定色块距离小车的距离以及左右位置。进而通过这些参数可以调用电机前进后退以及转弯。
下面从新建文件开始跟随手册一步一步的实现循线小车吧。
一、电机的控制
首先学会控制电机使小车前进后退以及转弯，从这一节开始，为了统一程序与车模硬件驱动对应关系，三个电机接线分别为
这个示意图中表明车头和车尾，同时及粗红色箭头也可以表示为镜头所看的方向。
在使用电机的时候需要先引用电机模块：
# 导入LQ_Module 模块
from LQ_Module import motor # 直流电机
电机引脚的初始化：
# 电机引脚初始化
motor1 = motor(timer=4, chl=1, freq=10000, pin_pwm="P7", pin_io="P22") #左电机
motor2 = motor(timer=4, chl=2, freq=10000, pin_pwm="P8", pin_io="P23") #右电机
motor3 = motor(timer=4, chl=3, freq=10000, pin_pwm="P9", pin_io="P24") #后电机
上面代码中电机引脚初始化使用了定时器4的前三通道配置为10K Hz的 PWM输出模式,对应管脚P7,P8,P9,方向控制管脚分别对应P22,P23,P24；配置完电机控制引脚，就可以调用LQ_Module中定义好的电机执行函数，来使电机转动起来：通过以下语句实现电机运行，
可以实现小车向前运动。
motor1.run(3500) # 左电机
motor2.run(-3500) # 右电机
motor3.run(0) # 后电机
可见调用函数控制电机是比较简单的，接下来为了增加控制灵活性，我们加入中间变量来暂存程序中要加到电机输出上的PWM脉宽值：
例如：
min_speed = 2000 # 最小速度（电机的死区，低于这个值电机不会启动）
speed = 3500 # 基准速度（控制整体前进速度，小车运行时的基本速度）
speed_L = 0 # 左轮速度暂存全局变量（各电机的实际速度值：基准±巡线偏差值）
speed_R = 0 # 右轮速度暂存全局变量
speed_B = 0 # 后轮速度暂存全局变量
那么在控制时就可以更加灵活操作（其中x_error为实时变化的一个值）：
speed_L = speed + x_error*50 # 控制电机转速进行循迹，乘以放大系数，系数越大转向越迅速
speed_R = -speed + x_error*50 # 基准速度+偏差
motor1.run(speed_L) # 左电机
motor2.run(speed_R) # 右电机
要控制小车左转，那么只需要浅笑左轮的转速，加大右轮的转速，因为三轮的接口还可以使用后轮向右转动来共同完成左转，体现在最终电机的输出上则可表示为以下这样：
motor1.run(3000) # 左电机
motor2.run(-4500) # 右电机
motor3.run(-2500) # 后电机
其中的脉宽值不一定就是实际转向用到的大小，但其值的方向确定电机的转向，可见左边电机控制函数传入正值，右边电机控制函数传入负值，两个电机都向前运动，但右边电机的PWM值比左边大，通过偏差就可以实现向左转动的效果，同时后轮也缓慢向右运动辅助小车整体向左前放运动；同理将右边值减小，左边值增大，改变后电机的方向则小车整体向右前方运动。
如果想要实现小车的原地转向则，三个电机的转动方向相同即可，其运动模型如图：
▲ 图2.1.1 原地逆时针旋转
二、色块识别与偏差获取
色块识别必须用到的函数 img.find_blobs()，在前面的图像设识别章节已经讲过它的常用参数和功能，这里直接使用它来获取想要提取的颜色色块对象。
首先需要设置对比阈值，使用与主程序初始化相同图像大小的图像示例程序例如修改helloworld.py中的图像大小，这里我们使用到里屏幕因此采集图像摄像头设置为：
sensor.set_framesize(sensor.LCD) # 设置LCD图像大小 128*160，或QQVGA2
然后运行示例程序将要采集阈值的颜色移入图像画面中，按照第三节调节识别阈值的方式采集并，定义一个变量将其存放为全局，例如下面黑色的阈值（注意这个黑色是相对采集环境中的识别值，不是绝对意义上的黑色）。
# 设置要寻找的线的阈值的阈值（色块法）
line_threshold = [
(1, 40, -21, 19, -9, 17) #黑色，请在实际使用场景中采集
}
采集设置好阈值后，在主循环中使用色块获取函数来获取目标色块对象：
img = sensor.snapshot() # 获取一帧图像
# 使用img.find_blobs()函数获取图像中的各个色块,blobs即为获取到的色块对象，
# roi为感兴区域[x,y,w,h]，即只在这个范围内查找
blobs = img.find_blobs(line_threshold, roi = [5, 7, 121, 73],pixels_threshold=10, area_threshold=10, merge=True)
if blobs: # 找到追踪目标
blob = find_max(blobs) # 提取blobs中面积最大的一个黑色色块blob
img.draw_rectangle(blob.rect(),color=(255, 0, 0)) # 根据色块blob位置画红色框
img.draw_cross(blob.cx(), blob.cy(),color=(0, 0, 255)) # 根据色块位置在中心画蓝色十字
上面的识别中为了能在图像中观察到目标识别情况，使用自定义函数在传入色块对象返回面积最大的色块：
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
然后再这个基础上将目标色块使用框线和十字标识出来。
偏差获取：
对于获取到的色块对象，可以使用.cx()函数来获取其中心x轴的坐标，这个坐标值代表色块在所采集图像中的x轴上的位置，通过与图像x轴中线（也就是图像宽度的一半: img.width()/2）做差得到目标色块中心在x轴上的位置偏差，表示为：
x_error = blob.cx()-img.width()/2 # 计算黑色中心偏差x_error
经过计算得到的这个值在中线左右浮动，其符号表示偏左还是偏右，绝对值表示偏差量。
三、组合控制
这里为了直观了解程序整体控制逻辑，将整个控制流程展示如下：
将参考代码展示如下（演示视频详见资料）：
"""
--- 巡线任务参考程序，为能快速理解程序，特别写了较为详细的注释
"""
# --------------------------------导入外部文件中的包和模块减↓↓↓----------------------------
import sensor, image,display # 导入摄像头传感器，图像，显示器相关包
from pyb import Pin # 从pyb包中导入Pin模块
from LQ_Module import motor # 从LQ_Module文件中导入motor
# --------------------------------设置初始变量参数↓↓↓--------------------------------------
# 设置要寻找的线的阈值的阈值（色块法）
line_threshold = [
(1, 40, -21, 19, -9, 17) #黑色，请在实际使用场景中采集
]
min_speed = 2000 # 最小速度（电机的死区，低于这个值电机不会启动）
speed = 4000 # 基准速度（控制整体前进速度，小车运行时的基本速度）
speed_L = 0 # 左轮速度暂存全局变量（各电机的实际速度值：基准±巡线偏差值）
speed_R = 0 # 右轮速度暂存全局变量
speed_B = 0 # 后轮速度暂存全局变量
start_flag = False # 电机转动标志位，通过K0按键切换，为True时电机转动，否则电机不转
#======各个外设初始化↓↓↓==========================
# ---------------------------TFT-LCD显示初始化↓↓↓--------------------------------------
lcd = display.SPIDisplay() # 初始化显示屏（参数默认-空）
lcd.clear() # 清屏
pic=image.Image("/pic0.jpg") # 读取图片
lcd.write(pic) # 显示图片
# ------------------------------按键初始化↓↓↓--------------------------------------
#按键初始化,按键扫描，母版上K0,K1,K2分别对应P30,P31,P1
K0 = Pin('P30', Pin.IN, Pin.PULL_UP)
# ---------------------初始化三路电机控制PWM及DIR↓↓↓-----------------------
# 电机引脚初始化 使用定时器1前三通道，10K Hz PWM,对应管脚P7,P8,P9,方向控制管脚，P22,P23,P24
motor1 = motor(timer=4, chl=1, freq=10000, pin_pwm="P7", pin_io="P22")
motor2 = motor(timer=4, chl=2, freq=10000, pin_pwm="P8", pin_io="P23")
motor3 = motor(timer=4, chl=3, freq=10000, pin_pwm="P9", pin_io="P24")
# -----------------------------初始化摄像头↓↓↓-------------------------------------
sensor.reset() # 初始化摄像头
sensor.set_hmirror(True) # 镜像（如果视觉模块倒着安装，则开启这个镜像）
sensor.set_pixformat(sensor.RGB565) # 采集格式（彩色图像采集）
sensor.set_framesize(sensor.LCD) # 设置图像大小 128*160
sensor.skip_frames(time = 2000) # 跳过前两秒图像后，关闭自动增益白平衡
sensor.set_auto_gain(False) # 必须关闭自动曝光才能进行相对稳定的颜色跟踪
sensor.set_auto_whitebal(False) # 必须关闭自动白平衡才能进行相对稳定的颜色跟踪
# -----------------------------自定义函数↓↓↓-------------------------------------
# 在传入色块对象中找到面积最大的色块返回
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
# ================== 程序主循环 =======================
while(True):
#按键K0切换是否打开
if not K0.value(): #如果检测到K0按键按下
while not K0.value(): #等待按键松开
pass
start_flag = not(start_flag) # 按键松开后取反start_flag的值，控制电机启停
img = sensor.snapshot() # 获取一帧图像
# 使用img.find_blobs()函数获取图像中的各个色块,blobs即为获取到的色块对象，roi为感兴区域[x,y,w,h]，即只在这个范围内查找
blobs = img.find_blobs(line_threshold, roi = [5, 7, 121, 73],pixels_threshold=10, area_threshold=10, merge=True)
if blobs: # 找到追踪目标
blob = find_max(blobs) # 提取blobs中面积最大的一个黑色色块blob
img.draw_rectangle(blob.rect(),color=(255, 0, 0)) # 根据色块blob位置画红色框
img.draw_cross(blob.cx(), blob.cy(),color=(0, 0, 255)) # 根据色块位置在中心画蓝色十字
x_error = blob.cx()-img.width()/2 # 计算黑色中心偏差x_error
speed_L = speed + x_error*50 # 控制电机转速进行循迹，乘以放大系数，系数越大转向越迅速
speed_R = -speed + x_error*50 # 基准速度+偏差
if x_error>8: # 当偏差超过这个值，后轮才会辅助转向
speed_B = min_speed + x_error*30 # 控制后轮电机转速协助转弯，乘以放大系数，系数越大转向越迅速
elif x_error<-8:
speed_B = -min_speed + x_error*30 # 控制后轮电机转速协助转弯
else:
speed_B = 0
print(x_error, speed_L,speed_R,speed_B) # 串行终端打印，偏差和最终电机输出值
if start_flag: #标志位为True时电机转动
motor1.run(speed_L) # 左电机
motor2.run(speed_R) # 右电机
motor3.run(speed_B) # 后电机
lcd.write(img) # 显示屏显示图像
else : # 否则电机不转
motor1.run(0) # 左电机
motor2.run(0) # 右电机
motor3.run(0) # 后电机
else: # 没有找到目标线的颜色
motor1.run(0) # 左电机
motor2.run(0) # 右电机
motor3.run(0) # 后电机
lcd.write(img) # 显示屏显示图像
为了保证小车在使用过程中的电机速度控制稳定性，三个任务的跑车程序例程中对LQ_Module.py中的电机驱动增加了ADC电压控制，使得电机输出基本不会受电池电压下降导致速度下降，拷贝文件时可以将文件夹中全部文件拷贝到视觉模块。但也要注意，关注电池电压是否不足，一有空闲时间尽量给电池充电保持电池电压在7.2至8.4V的正常范围内。
03 任务二、巡线避障
在上一节中，基本详细介绍了，通过摄像头采集图像分析目标色块位置坐标最终控制小车在前行中跟踪目标的功能，接下来加入障碍误识别处理。
首先障碍无识别因为是虚拟障碍物色块，同样使用色块识别的方法。先采集目标障碍色块的图像并拉取阈值，将其元组设为全局常量在主循环中调用；其次躲避找障碍物需要对电机进行更加精确的控制，因此引入PID的控制，这里如果不清楚PID控制的可以先百度搜索了解以下，不过在程序中其计算过程被封装在了PID文件中，可以不需要了解，只需要调用计算函数传入偏差值后会自定返回计算结果。其实在上一节中电机输出变量=电机+偏差*比例系数，这个系数就可以看作是PID控制中的P控制，以应对系统而将输出值进行放大或缩小处理。
为了使得循线和避障能够同运行，设置一个状态标志位obstacle_flag用来切换循线状态和避障状态，检测到障碍物时将obstacle_flag置为1，接着执行避障处理，完成后标志位置0，执行循线控制部分的程序。
避障处理，采用电机编码器、目标色块中心与镜头图像中心距离等综合控制，其流程如下：
以下给出避障处理的部分代码作为分析参考：
import sensor, image, time, display
from pyb import Pin,Timer
from pid import PID
from LQ_Module import motor, Enc_AB
# *************************省略循线中重复部分*************
# 障碍物色块的阈值，可以一次放多个阈值，但不对返回结果做区分则被视为一个目标
obstacle_threshold = [
# (24, 71, 28, 87, 0, 55) #R
(38, 63, -62, -41, 33, 61), #G
(22, 46, -52, -34, 31, 51)
]
# 障碍物大小为此值时切换成避障模式
size_threshold = 7250
# 障碍物标志位，是否识别到障碍物
obstacle_flag = 0
# 用于控制摄像头一直朝向障碍物，仅P控制，也可尝试加入I或D
pid_x = PID(p = 150,i = 0, d = 0,imax = 50)
# *************************省略循线中重复部分*************
#=========================================================
# 霍尔编码器引脚初始化
Enc3 = Enc_AB(Timer(14, freq=5), Enc_A="P25", Enc_B="P26")
# 编码器累计值(用于记录后轮编码器转动距离)
encoder_value = 0
# *************************省略循线中重复部分*************
while(True):
#按键K0切换电机转动标志位
# *************************省略循线中重复部分*************
blobs = img.find_blobs(obstacle_threshold,pixels_threshold=500, area_threshold=50, merge=True)
if blobs: # 如果有阈值中的色块则执行if下面的代码
max_blob = find_max(blobs) # 筛选所有色块中的最大的那一个
area_g = max_blob[2]*max_blob[3] # 计算障碍物的像素面积
#print(area_g) # 打印当前障碍物的大小
if area_g > size_threshold: # 如果障碍物面积超过一定大小，则判断为有障碍物，切换标志位
obstacle_flag = 1
else:
obstacle_flag = 0
# 切换模式 obstacle_flag=1为避障模式 obstacle_flag=0为寻迹模式
if obstacle_flag == 1: #执行避障程序
# 避障方式2 特点：复杂 涉及到PID运算 稳定向相对方式1较好
# 避障思路：识别到障碍物后小车绕着障碍物走（镜头对准障碍物），此时对后轮编码器进行累计，达到一定值（实际测量编码器的值）后
# 判断为绕障碍物走了180°，此时将车模以一定速度旋转一定角度（掉头），然后将标志位切换回循迹模式。
img.draw_rectangle(max_blob[0:4]) # 画一个矩形，框出障碍
img.draw_cross(max_blob[5], max_blob[6])# 障碍中间画一个十字
area_g = max_blob[2] * max_blob[3] # 障碍标识的面积
error_x = 70 - max_blob[5] # 障碍中心点与中间的偏差，目的是使镜头一直对着障碍物,以控制障碍物与车的距离，配合后面的编码器累计值完成绕行
duty_x = pid_x.get_pid(error_x,1) # pid运算
#print(duty_x)
speed_L = -duty_x # PID计算出来的值交给电机速度变量如果加距离控制则：-duty_s
speed_R = -duty_x # PID计算出来的值交给电机速度变量如果加距离控制则：+duty_s
motor1.run(speed_L) # 左电机
motor2.run(speed_R) # 右电机
motor3.run(4950) # 后电机 绕障碍旋转，如果转不动，可以增大这个值
encoder_value += Enc3.Get() # 编码器开始累计值，累计到一定值后进行旋转180°继续循迹黑线
if encoder_value > 3050 or encoder_value < -3050: # 到达预定位置，若error_x， 后的数字越大表示车离障碍物中心越远，想要到达预定位置就需要走更远，编码器的预定值就需要增大
motor1.run(2500) # 左电机
motor2.run(2500) # 右电机
motor3.run(2500) # 后电机
time.sleep_ms(1500)
obstacle_flag = 0 # 清除标志位，切换到正常寻迹模式
encoder_value = 0 # 编码器累计值清零
lcd.write(img) # 显示屏显示图像
continue #跳出本次循环
else:
# ***********执行循迹程序*********
# *************************省略循线中重复部分*************
04 任务三、到达停车点
经过前面对任务一、任务二的实现过程分析和实验，那么接下来对于到达目标位置停车，只是在原来及基础上增加一个，中心点停车标志的识别即可，这里再任务二的程序中增加一个停车色块阈值：
将任务二的程序备份一份，然后将上面采集到的中心系徽图标的阈值添加到程序开头：
# 中心停车图标阈值
Stop_threshold = [
(14, 28, 11, 37, -22, -6), # 紫色
(20, 43, 10, 31, -16, 10)
]
由前两个任务可以，如果小车循线一只往前走，最终会在中间那块丢失目标线色块但只是将这种情况忽略并统一将电机输出设为0，原地等待，在任务三种，当丢失循线目标后开始寻找中心图标，如果判断是，则显示一张图片，关闭电机，进入空循环，将整个程序卡死在此处。
在任务二的基础上，添加以下判断：（完整代码请参考资料中整车程序任务三）
else: # 没有找到循线目标
blobs = img.find_blobs(Stop_threshold, roi = [1, 0, 127, 83],pixels_threshold=5, area_threshold=10, merge=True)
if blobs: # 找到追踪目标
blob = find_max(blobs) # 提取blobs中面积最大的一个黑色色块blob
img.draw_rectangle(blob.rect(),color=(0, 255, 0)) # 根据色块blob位置画绿色框
img.draw_cross(blob.cx(), blob.cy(),color=(255, 255, 0)) # 根据色块位置在中心画黄十字
#start_flag = 0 # 关闭电机输出
lcd.write(pic) # 显示图片
time.sleep_ms(200) # 延时后停车，在此出调节最终停车的位置是否准确，如果右的车停车前左右偏移，可加入校正（发挥部分）
motor1.run(0) # 左电机
motor2.run(0) # 右电机
motor3.run(0) # 后电机
while(1):
pass # 完成所有任务后，进入死循环等待重启
# 如有想法的，可以在此处做更多拓展，比如完成后执行其他你想要让小车执行的动作，或在屏幕上显示更多内容图片等。
也就是说一旦黑线到头就进入上面的程序检测紫色图标，如果检测到则准备停车并显示图片，至此所有任务结束！
■ 相关文献链接:
● 相关图表链接:
扫一扫
2161
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
