stm32f429 + ov5640摄像头 Robomasters校内竞速赛自主控制小车

前言

       小车用于参加南京理工大学首届机器人大赛中的Robomasters竞速赛，所需功能主要为控制小车自主寻迹和射击框识别 ，文章将分别阐述实现以上功能的方法。

       使用软件：Keil

       使用工具：Stm32f429开发板  、OV5640摄像头 、LCD屏 、麦克纳姆轮小车 、自制云台

正文

小车自主寻迹

 

   1.OV5640摄像头数据采集

 

        OV5640摄像头通过SCCB总线控制，以最高15帧/秒的速度传输图像，数据格式为YCbCr4:2:2/RGB565 逐行视频和JPEG压缩数据。在实现所需功能的过程中，选择的数据格式为RGB565。DCMI接口接受摄像头模块发出的高速数据流，同时通过DMA将DCMI接口中的数据传送到内存中的数组。因此需要配置OV5640摄像头驱动，DCMI驱动和DMA。需要注意的是，开发板的内存空间只有256K，不足以支撑处理图像的算法，所以在正式对图像数据进行处理之前，需要驱动外部SDRAM以扩展STM32F429的内存，DMA将数据传送到固定地址定义的数组，展开接下来的数据处理。

 

   2.图像数据的处理

 

        选择的数据格式为RGB565，以16位数据空间压缩存储RGB三通道的数据，其中高5位为R通道数据，中间6位为G通道数据，低5位为B通道数据，压缩方式是截取原数据的高5位，6位，5位。之所以选择RGB565数据格式，是因为比赛过程中射击框颜色为红色，可以通过将RGB颜色空间转化为HSV颜色空间的方法，对红色数据进行筛选。

        而在寻迹过程中，则是将RGB565数据转化成灰度值，进行二值化之后将地图上的引导线识别出来，在这个步骤中，需要在场地测试过程中调节二值化的阈值，从而将引导线能够完整的识别。

        RGB565转灰度值公式如下:               

                                                                 r = (color & 0xF800) >> 8;

                                                                g = (color & 0x07E0) >> 3;
                                                                b = (color & 0x001F) << 3;
                                                                temp=(r*77+g*150+b*29+128)/2048;
                                                                gray = (0x001f&temp)<<11;
                                                                gray = color|(((0x003f)&(temp*2))<<5);
                                                                gray = color|(0x001f&temp);

        对图像进行二值化之后，就需要将路径画出，以便后面控制算法的实现。因需要考虑数据的处理速度，故无法使用较为复杂的算法进行边缘检测，需要以尽量少的遍历次数来实现自己想要的功能，在具体实现中，采用的方法如下：

 

        1.因引导线是连续且不中断的，故首次遍历数组最后一行即视野最近行来寻找引导线的左右边界，取左右边界的均值作为处理之后引导线的起始点。

        2.以起点的横坐标为基准，向上寻找，依次寻找左右边界取均值，画出处理之后的引导线。其中画线的结束条件为继续向上寻找无左右边界或左右边界相隔像素点过小。

        注：因比赛过程中需要考虑直角转弯以及岔路口左转，故在实际的实现中，画线所取的点为左边界。

 

        在画出引导线之后，需要通过取点来计算斜率以及曲率，斜率和曲率决定了小车的直行速度以及转弯速度。

 

   3.控制算法的实现

 

       控制算法主要包括两大块，其一为麦克纳姆轮小车控制，其二为小车引导控制。

 

       麦克纳姆轮小车控制中，主要用到PID控制以及麦克纳姆轮的控制。

       PID控制是常见的闭环控制，由比例单元P、积分单元I和微分单元D组成。PID控制的基础是比例控制；积分控制可消除稳态误差，但可能增加超调；微分控制可加快大惯性系统响应速度以及减弱超调趋势。该功能中使用PID主要用以控制轮子转动速度达到期望值，来保证小车行驶的稳定。

       麦克纳姆轮的特点是可以实现全向移动，麦克纳姆轮主要由轮毂和辊子组成，两者的夹角为45度，辊子的转动产生横向分量的速度，通过合理的组装麦克纳姆轮，合理的调节四个轮子的正反转，即可实现小车的横向移动。故小车的控制可分为前进速度X，横向移动速度Y以及自转速度Z，在本次小车的自动寻迹中，因装载摄像头的舵机为非360度舵机，故在小车控制中只用到了前进速度X和自转速度Z。在具体的实现中，就是将前进速度X和自转速度Z分配到四个轮子上，以合理的权值进行累加，从而达到小车的稳定控制。

 

        小车引导控制中，以图像数据为基础，选取画出的引导线中的点，分别计算视野远端引导线、视野中部引导线、视野近端引导线的曲率，根据曲率拟出适当的函数来计算小车在面临接下来的路线中，应以多大的速度行进，即算出速度X。根据速度来选取适当的预瞄点，选取的原则是：速度快看得远，速度慢看得近的。在选定了预瞄点的基础上，计算预瞄点与小车的斜率，拟出适当的函数计算应以多大的速度自转，即算出速度Z。从而实现了小车引导控制。在具体的调试过程中，主要调试的就是两个速度函数的选择，以及预瞄点的选择。X方向的速度函数好坏决定了小车是否会突然加速或者减速；Z方向的速度函数包括 了PID闭环控制，其好坏决定了小车能否完成转弯；预瞄点选择的好坏决定了合理的入弯速度，以及在合适的入弯时机。

 

    4.注意事项

       在具体调试的过程中，为了方便观察二值图，边缘图的效果，需要使用LCD屏，设置按键显示不同图像可以实时观察图像的变化情况，从而找出可能存在的问题。

射击框识别

 

   1.图像数据的处理

 

        在储存的数组中，将RGB565格式的数据转化为HSV格式，根据红色在HSV空间中的分量范围，识别出视野中的红色区域并进行二值化处理，在此基础上进行矩形框的识别。

       RGB565转HSV公式如下：

                                                      r = (color & 0xF800) >> 8;
                                                      g = (color & 0x07E0) >> 3;
                                                      b = (color & 0x001F) << 3;

                                                      b1=b/255.0;
                                                      g1=g/255.0;
                                                      r1=r/255.0;
                                                      maxVal = max3v(b1, g1, r1);
                                                      minVal = min3v(b1, g1, r1);
                                                      v=maxVal*255;
                                                      if(maxVal==0)
                                                          s=0.0;
                                                      else
                                                          s=(maxVal-minVal)/maxVal*255;
                                                      if(maxVal==minVal)
                                                          h=0.0;
                                                      else
                                                      {
                                                          if(maxVal==r&&g1>=b1)
                                                          h=60*(g1-b1)/(maxVal-minVal);
                                                          else if(maxVal==r1&&g1<b1)
                                                          h = 60 * (g1 - b1) / (maxVal-minVal) + 360;
                                                          else if(maxVal==g1)
                                                          h = 60 * (b1 - r1) / (maxVal-minVal) + 120;
                                                          else if(maxVal==b1)
                                                         h= 60 * (r1 - g1) / (maxVal-minVal) + 240;
                                                      }

       矩形识别的具体实现如下：

       1. 对于得到的二值图像，采用链码进行连续边缘像素点记录。对于每一条链码，根据所需要识别的矩形框的面积对过长或过短的链码进行排除，减少计算，从而提高程序运行的速度。对于符合条件的链码，根据记录的首个点的坐标，可以推算出其余点的坐标，从而进行下一步面积的计算。

       2.对于矩形的识别，假定链码形成一个封闭的几何图形，可以计算链码所围成的封闭图形的面积，即在遍历每一条链码的过程中，将所有向左或向右移动的像素点的纵坐标分别记录，计算出差的绝对值。在遍历的过程中，同时记录遍历过程中的最左、最右、最上、最下四个方向的坐标，作为封闭几何图形的外接矩形，计算面积。计算链码围成图形面积与外接矩形面积的商值，从而判断该封闭图形是否为矩形。

       在具体的实现中主要通过设置矩形的相似度，矩形的面积范围，矩形的长宽比，来限定视野中可能出现的矩形框为目标射击框。

 

     2.射击控制

 

        在矩形框识别的基础上，计算视野中心与矩形框中心的偏移位置用以射击控制。因摄像头固定的位置为射击炮管的头部，故摄像头视野中心即为炮管的射击准心。通过计算出的水平方向和竖直方向的偏移量，控制自制云台的俯仰舵机和左右舵机，从而使摄像头视野中心与射击框中心基本重合，以此来达到炮管的瞄准。在具体的实现中存在弹珠射击落点不稳定的情况，问题可能存在于以下几个方面：

       1.弹珠由360度舵机作为拨片，使弹珠落入摩擦轮之间，从而达到射击的目的，因摩擦轮并非一直开启，故弹珠落入的时机可能并非为摩擦轮转速稳定的时间，且因弹珠的下落通道出口较大，使弹珠与摩擦轮接触的位置可能产生细微不同，从而导致了落点不同的情况。

       2.炮管的粗细会影响弹珠的射出轨迹，在实际的选择中，使用的是入口宽出口窄的3D打印炮管，但不合理的炮管直径会影响弹珠射出炮管的速度。

    3.注意事项

       在具体的调试过程中，HSV对颜色的识别仍会因为光照的影响而使分量范围发生变化，且影响较大，这是在比赛前需要着重考虑的要点。