目录
前言
一、找路部分
1.总钻风
2.OpenMV
二、循路部分
1.麦轮的运动解算
2.运动信息的计算
3.驱动
三、效果视频
四、飘移
1.三个自由度间的互相影响
2.画面裁切
前言
写这篇文章的起因是参加智能车寒假校赛时，由于我们组开始着手比较晚，实验室的总钻风摄像头不够用了，所以只能使用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->outputoutput: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)=160||(unsigned short int) (moving[2])700||PID1.target700||PID2.target700||PID3.target700||PID4.target
发表评论:
◎欢迎参与讨论，请在这里发表您的看法、交流您的观点。