写这篇文章的起因是参加智能车寒假校赛时，由于我们组开始着手比较晚，实验室的总钻风摄像头不够用了，所以只能使用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;
}

$target=straight+translation+spin$

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。

好，做完以上两点更改你就能用第一人称飘移青春了。

声明：本文内容由网友自发贡献，不代表【wpsshop博客】立场，版权归原作者所有，本站不承担相应法律责任。如您发现有侵权的内容，请联系我们。转载请注明出处：https://www.wpsshop.cn/w/不正经/article/detail/80873