本文详细介绍了作者在智能车比赛中使用OpenMV进行视觉循迹的过程，包括摄像头数据处理、OpenMV中线检测、麦轮运动解算、PID控制策略以及如何实现飘移效果。作者通过编程实现自旋量和平移量的计算，并针对飘移需求进行了优化，以提升车辆的动态性能。
目录
前言
写这篇文章的起因是参加智能车寒假校赛时，由于我们组开始着手比较晚，实验室的总钻风摄像头不够用了，所以只能使用OpenMV进行视像头视觉循迹。而当我在网上搜索OpenMV循智能车赛道时，发现竟然什么也没有（可能有简单循黑线的），所以我打算分享一下自己的代码。同时我发现麦轮循迹的资料也比较少，所以我打算把二者结合做个小开源。
一、找路部分
提前声明，本篇扫线用的是传统的大津法。
1.总钻风
这些摄像头传感器模块的工作原理都差不多，基本都是把捕获到的图片以各个像素点存储在一个一维数组里。
我们本次暂时用的主控是沁恒的CH32V307，沁恒芯片使用DVP去控制图像传输，DVP内部调用了DMA，总钻风获取的灰度图通过DMA传输，在每个场中断时初始化DMA传输，在传输结束后触发DMA中断，标志着一场图像传输结束。在程序中设定两个缓冲区，一边MCU读取摄像头数据，一边DMA将要传输的数据放入另一个缓冲区，提高摄像头的帧率。
如果想了解总钻风拍摄后如何找中线的，网上一大把。我这里主要说OpenMV。
2.OpenMV
OpenMV与主控之间通过串口通信，如果把整幅图像都传递给单片机显然有亿点慢。所以我让OpenMV捕获到图片后自己找中线，计算后只把小车需要的两个量（自旋量、平移量）传递给单片机。OpenMV获取一张图片计算一张图片，相比上述两个缓冲区的模式，速率有点慢。
OpenMV从捕获图片到传递参量的全部代码如下：
import sensor, image, time #导入所需的库
from pyb import millis, UART
###声明摄像头
sensor.reset() #摄像头重启
sensor.set_pixformat(sensor.GRAYSCALE) #8位灰度模式
sensor.set_framesize(sensor.QQVGA) #160*120分辨率
#sensor.set_windowing(0,60,160,60)
sensor.set_auto_whitebal(True) #关闭自动白平衡
sensor.set_auto_gain(False) #关闭自动亮度
clock = time.clock() #声明clock，用于主循环中统计帧速。
uart = UART(3, 115200) #开启串口
uart.init(115200, bits=8, parity=None, stop=1) #8位数据位，无校验位，1位停止位
###变量
midline=[0]*120 #存储赛道中线的数组
leftline=[0]*120 #存储赛道左边线的数组
rightline=[0]*120 #存储赛道右边线的数组
Row=119 #画面120行
Col=160 #画面160列
#例：若要调用img[2][3],则需调用img[2*Col+3]
white=255 #白色为255
black=0 #黑色为0
translation=0 #平移量
spin=0 #自旋量
###找赛道中线函数
def Way_Finding():
if img[Col*Row+(Col//2)] == black: #判断车身中点是否在赛道上，决定从哪里开始爬线
if img[Col*Row+5] == white:
midline[Row] = 5
elif img[Col*Row+(Col-5)] == white:
midline[Row] = Col-5
else:
midline[Row] = Col//2
else:
midline[Row] = Col//2
for i in range(Row, 50, -1): #画面横向120行，这里只往上爬到50行，车速不快，爬多了也没用
for j in range(midline[i], -1, -1):
if img[int(Col*i+j)]==black or j==0: #找到黑色边界或最后什么也没找到 跳出
leftline[i] = j
break
for j in range(midline[i], Col):
if img[int(Col*i+j)]==black or j==Col: #找到黑色边界或最后什么也没找到 跳出
rightline[i] = j
break
midline[i] = (leftline[i] + rightline[i]) / 2 #左右边线相加除以二得到中线
if img[Col*i+int(midline[i])]==0 or i==60: #若img的中线爬到黑色则退出循环
break
###主循环
while True:
img = sensor.snapshot() #获取图像
histogram = img.get_histogram() #获取图像的直方图
Thresholds = histogram.get_threshold() #获取直方图的阈值
img.binary([(Thresholds.value(), 255)]) #二值化图像，将低于阈值的像素设为黑色（0），高于阈值的像素设为白色（255）
Way_Finding() #调用找中线函数
translation=midline[Row]-Col/2 #计算平移量
for k in range(Row, 65, -1): #计算自旋量
img.draw_line((int(midline[k]), int(k), int(midline[k-1]), int(k-1)), color=(0,0,0)) #在屏幕上标识出中线
spin=spin+midline[k]
spin=spin/(Row-k)-Col/2
moving=bytearray([130,130,int(translation),int(spin)]) #打包数据，包头为两个130，无包尾
uart.write(moving) #发送给单片机
print(midline[Row],midline[60],translation,spin) #在串行终端中打印一下以便观察
spin=0 #清零自旋量先初始化摄像头和串口，然后声明一些变量。
在主循环中，先获取一张图像，再调用OpenMV自带的Otsu算法二值化图像，得到赛道基本样貌，效果如下：
接着调用我们自己定义的Way_Finding()函数找出赛道中线。在Way_Finding()中，先判断屏幕最下面一行的中心像素点是否为白色，如若不是，则向左或向右随便找一个是的作为midline[Row]的值开始爬线。画面横向有120行，我们只遍历60行，剩下的60行太靠前了会造成干扰。最后通过判断黑色像素跳变点一行一行地找出左右边线和中线（判断一个跳变点就基本够用，我看画面中的赛道没有噪点）。
二、循路部分
1.麦轮的运动解算
关于麦轮的运动解算，网上的资料还是蛮多的，但基本都是从辊子的受力分析开始一顿推算，容易把新手搞懵圈。这里我推荐去看b站up主程欢欢的视频，个人感觉他讲得很清晰。我在本篇中也借用他的图片简单阐述一下。
假设四个轮子的标号如下：
我们通过几个已知运动合成最终的全向运动。蓝色表示车轮旋转方向，绿色表示车轮对地面施力方向。
上面是三个自由度各自的分解运动，把它们合成就会得到小车总体的运动，即
小车的运动=前后+左右+旋转
那么我们再把这个公式代入四个轮各自的运动，规定向前、向右、顺时针自转为正方向，就会得到四个轮各自的运动公式：
左前轮（轮1）= 前后 + 左右 + 自转
右前轮（轮2）= 前后 - 左右 - 自转
左后轮（轮3）= 前后 - 左右 + 自转
右后轮（轮4）= 前后 + 左右 - 自转
综上，为控制车整体的运动，我们需要三个量：前进量、平移量、自旋量。下面我们开始获取这三个量。
2.运动信息的计算
（1）前进量
小车在直道行驶时的平移量和自旋量为0，匀速直线运动的速度作为前进量，在主控中自己定义，为车的基准速度，大小与编码器读取的转速值范围有关，因车而异，各不相同，自己根据自己调的PID给一个合适的值即可，不过这个基准速度的大小会影响后面平移量和自旋量乘的比例系数。
（2）自旋量
自旋量是决定小车转弯的重要因素，计算方法如下：
for k in range(Row, 65, -1): #计算自旋量
spin=spin+midline[k]
spin=spin/(Row-k)-Col/2可见，先求midline中线数组里的元素（每一行的中点）的平均值，再把该值与画面中线（160/2）做差，得到的就是小车的自旋量spin，像这样：
（3）平移量
有的时候，车身与赛道会出现平行偏差，像这样：
这种情况通过自旋量也能调整过来，无非多经过几个s型的摇摆，但显然这种水多了加面、面多了加水的处理方式效率低下，故我们引出平移量：
平移量的计算很简单，只需要把画面最后一行的midline[Row]值与画面中线值做差。
translation=midline[Row]-Col/2 #计算平移量综上，我们便得到了小车运动所需的三个量，把它们通过串口传递给单片机。
moving=bytearray([130,130,int(translation),int(spin)]) #打包数据，包头为两个130，无包尾
uart.write(moving) #发送给单片机3.驱动
（1）PID
首先，你要封装好自己的PID。定义个结构体，声明四个轮子的变量：
typedef struct
{
float Kp,Ki,Kd;
float error[3]; //error[0]代表本次误差，error[1]代表上次误差，error[2]代表上上次误差
float target;
int actual;
float p,i,d;
float output,delta_output;
}PID;
PID PID1,PID2,PID3,PID4;开个10ms的定时中断：
void TIM5_IRQHandler(void)
{
if(TIM_GetITStatus(TIM5, TIM_IT_Update) != RESET)
{
TIM_ClearITPendingBit(TIM5, TIM_IT_Update );
PID_Interrupt(); //中断回调函数
}
}中断处理函数：
void PID_Interrupt(void)
{
PID1.actual=-encoder_get_count(TIM4_ENCOEDER);
encoder_clean_count(TIM4_ENCOEDER);
PID2.actual=-encoder_get_count(TIM9_ENCOEDER);
encoder_clean_count(TIM9_ENCOEDER);
PID3.actual=encoder_get_count(TIM8_ENCOEDER);
encoder_clean_count(TIM8_ENCOEDER);
PID4.actual=-encoder_get_count(TIM3_ENCOEDER);
encoder_clean_count(TIM3_ENCOEDER);
Set_All(PID_Set(&PID1),PID_Set(&PID2),PID_Set(&PID3),PID_Set(&PID4));
}其中的PID_Set()计算函数：
float PID_Set(PID* pid) //增量式
{
pid->error[0]=pid->target-pid->actual;
pid->p=pid->Kp*(pid->error[0]-pid->error[1]);
pid->i=pid->Ki*pid->error[0];
pid->d=pid->Kd*(pid->error[0]-2*pid->error[1]+pid->error[2]);
pid->delta_output=pid->p+pid->i+pid->d;
pid->output +=pid->delta_output;
pid->output=pid->output<Output_Max?pid->output:Output_Max;
pid->output=pid->output>-Output_Max?pid->output:-Output_Max;
pid->error[2]=pid->error[1];
pid->error[1]=pid->error[0];
return pid->output;
}调参的话调成这样就差不多：
常规的PID封装准备工作完成，想要更改速度的时候直接更改PIDx.target就行了。
（2）接收中断
然后开启串口接收中断，在中断里接收OpenMV发过来的数据并进行处理：
void USART3_IRQHandler(void)
{
if(USART_GetITStatus(USART3, USART_IT_RXNE) != RESET)
{
static uint8_t rebuf[4]={0},i=0;
uart_query_byte(UART_3, &rebuf[i++]);
if(rebuf[0]!=130) //帧头
i=0;
if((i==2)&&(rebuf[1]!=130)) //判断帧头
i=0;
if(i>=4) //代表一帧数据完毕
{
memcpy(moving,rebuf,i); //把rebuf数组里的内容拷贝到moving数组
i = 0;
OpenMV_Track(); //中断回调函数
}
USART_ClearITPendingBit(USART3, USART_IT_RXNE);
}
}其中的OpenMV_Track()函数：
void OpenMV_Track(void)
{
translation=0.9*(unsigned short int)(moving[2]);
spin=1.4*(unsigned short int)(moving[3]);
straight=200;
if(fabs(spin)<5) spin=0;
McNamm_Set(straight,translation,spin);
if((unsigned short int)(moving[2])>=160||(unsigned short int) (moving[2])<=40)McNamm_Set(0,0,0); //偏离赛道时停车
}其中的McNamm_Set()函数：
void McNamm_Set(float straight,float translation,float spin)
{
PID1.target=straight+translation+spin;
PID2.target=straight-translation-spin;
PID3.target=straight-translation+spin;
PID4.target=straight+translation-spin;
if(PID1.target>700||PID1.target<-700) PID1.target=0;
if(PID2.target>700||PID2.target<-700) PID2.target=0;
if(PID3.target>700||PID3.target<-700) PID3.target=0;
if(PID4.target>700||PID4.target<-700) PID4.target=0;
}straight前进量是一个固定值，不设定为可变值的原因是translation和spin是OpenMV传过来的直接量，需要乘以一个比例系数再作用于target，这个比例系数需要上赛道调试，不同的straight对应的比例系数不同，但并非成简单的线性关系，所以每更改一次straight都需要手动调出一个合适的比例系数。
另外为了防止走直道时左右摇摆不稳定，设立一条判断语句，若自旋量spin小于某值则spin=0，只让translation作用。
三、效果视频
智能视觉循迹
四、飘移
既然麦轮有能够平移的特点，那不用来飘移岂不是白瞎了！！！
想要达到飘移的效果，大致需要做两点改动。
1.三个自由度间的互相影响
（1）translation
在前面我们说translation是画面最后一行的midline[Row]值与画面中线值的差，所以试想当小车遇见弯道时，translation的作用会使小车偏向弯道内侧。举个例子，当遇到右转的弯道时，摄像头看到的画面中最后一行的midline[Row]值在画面中线的右侧，所以translation是正值，会作用使小车向右平移，而如果向右平移的话车就会贴近弯道内侧行驶，所以没有飘移的效果。
相反的，若想达使车身过弯横漂，那么应该使translation的符号取反，相应的作用也是与从前相反。另外，此时的translation还应该受spin的影响，spin越大，translation越大，调试出一个合适的比例系数。
（2）straight
straight不再是一个固定的值，而是受spin的影响。当遇到弯路时，spin会增大，straight需随之减小，以平移代替前进，凸显飘移的效果。
综上，修改后OpenMV_Track()函数如下：
void OpenMV_Track(void)
{
translation=3.1*((unsigned short int)(moving[2])-100); //平移量
spin=2.6*((unsigned short int)(moving[3])-100); //自旋量
straight=300-1.4*fabs(spin)-0.7*fabs(translation); //前进量=基准量-1.4倍自旋量-0.7倍平移量
if(straight<0) straight=0; //防止前进量被减成负数
if(spin>20) translation=-1.5*fabs(translation)-0.9*fabs(spin); //右转时往左飘
if(spin<-20) translation=1.5*fabs(translation)+0.9*fabs(spin); //左转时往右飘
McNamm_Set(straight,translation,spin);
if((unsigned short int)(moving[2])>=160||(unsigned short int) (moving[2])<=40)McNamm_Set(0,0,0); //偏离赛道时停车
}2.画面裁切
经实践发现，改版飘移后的小车过弯时具有滞后性，反应有些迟钝。那么提升预判的方法便是提高画面处理的起始高度。
在前文中提到spin自旋量是这样得到的：
for k in range(Row, 65, -1):
spin=spin+midline[k]
spin=spin/(Row-k)-Col/2现在我们或许可以改成这样：
for k in range(Row-20, 55, -1):
spin=spin+midline[k]
spin=spin/(Row-20-k)-80二者的区别就是处理的画面区域不一样。第一段代码是使用第65行到第120行的img，第二段代码是使用第55行到第100行的img。
好，做完以上两点更改你就能用第一人称飘移青春了。
3.嘚飘嘚飘嘚儿咿地飘~
飘移
巴音布鲁克永远土！
扫一扫
1915
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
