篮球机器人

原文：https://blog.csdn.net/qq_39231566/article/details/80978449 

 

篮球机器人是一个综合运动控制，感知，决策的综合系统。

而机器人的视觉系统则是其中重要的一环，承担了识别与定位目标球，并返回位置和距离信息给决策部分的重要任务。本文以篮球机器人为例，研究机器人的视觉系统原理。

首先，介绍了篮球机器人系统结构层次及篮球机器人比赛规则，明确篮球机器人视觉部分主要任务要求是对目标进行识别与定位。然后对篮球机器人从软件和硬件的组成做必要的介绍，并对视觉部分的代码运行原理做深入浅出的详细分析。最后给出了实际的调试效果，并简要的说明了篮球机器人视觉部分还存在的问题。

篮球机器人，视觉原理，Kinect，OpenCV，定位，识别篮球机器人是一个属于人工智能和机器人学科领域的应用型研究课题，它综合了人工智能、 机器视觉、机械结构和智能控制等多方面的技术，具有很大的研究意义。篮球机器人比赛是中国机器人大赛中的一个重要比赛项目，其要求参赛机器人具有独立的行为决策、运动控制与视觉感知等能力。机器人的视觉系统则是其中重要的一环，承担了识别与定位目标球，并返回x轴相对位置信息和距离信息给控制决策部分的重要任务。要理解篮球机器人的视觉原理，首先要熟悉视觉与其他部分，包括运动，硬件，控制的联系，并且要对篮球机器人的比赛规则有清楚地认识，并要对视觉系统的硬件组成，软件工作原理有着透彻的理解。本文即围绕着这些方面进行展开，实现对篮球机器人视觉系统的研究。

篮球机器人视觉系统篮球机器人一般由感知子系统、决策子系统、运动控制子系统、通信子系统和投球控制子系统构成。各子系统之间的关系各子系统的功能如下：

感知子系统：采集各传感器的数据，并通过通信子系统传递给决策子系统。感知子系统常采用的传感器主要有 Kinect 传感器、激光传感器、里程计和陀螺仪等。

决策子系统：接收并处理从通信子系统传来的信息数据，将其转化为决策指令。完成这部分功能的称为上位机，一般采用笔记本或者工业计算机。

通信子系统：完成上位机和下位机以及控制器和传感器之间的数据传递。

运动控制子系统：将决策子系统发送的决策指令转化为机器人的运动控制信息，并使其完成指定动作。

投球控制子系统：将决策子系统发送的决策指令转换为控制弹射机构的电平信号，完成拾球投球等动作。

篮球机器人比赛规则从 2012 年开始，中国机器人大赛增加了篮球机器人比赛项目。比赛的标准场地如图3所示，场地与人类篮球比赛场地一模一样，就是标准的篮球场，场地中所有分界线均为白线。比赛用球为标准的篮球和排球，颜色在比赛前由官方指定，比赛用的机器人其尺寸不能超过 65cm×65cm×90cm。 赛时，由于受目前机器人技术的限制，机器人与机器人之间要跟人类一样传递篮球很难实现，所以比赛规则规定，每场比赛同时四个高校的机器人上场比赛，每个高校只允许一台机器人上场，机器人必须避开场上的静态障碍物和动态障碍物（其它高校的机器人），移动到场上放置球的位置，按照指定的取球顺序取球，然后运动到三分线内投球，己方四分之一半场的球全部投篮完毕后，可以去寻找另外四分之三场地的球，最后准确投进球多的队伍获胜，如果两个队伍投入球的数量一致，则用时短的队伍获胜。

硬件上，篮球机器人采用了Kinect1（如图5所示），它是一种高性价比的视觉传感器，它可以同时捕捉彩色图像和深度信息。Kinect 除了有一般的 RGB 摄像头外，还配有一个 3D 景深摄像头和红外摄像头。其两侧还有 4 元线性麦克阵列，下方有一个可编程仰角控制马达。RGB 摄像头可以获取视野范围内的彩色图像。红外摄像头可以获得可视范围内的物体深度图像。

软件上，视觉系统以OpenCV，一种开源的视觉编程库为主要工具，进行深度图和彩色图的处理，从而实现定位和目标识别的效果。编译工具是微软公司的Microsoft Visual Studio13软件。接下来的篇章中，我将着重研究视觉系统的软件部分工作原理。视觉系统代码工作原理  进入主函数，首先是初始化串口，串口号是从设备管理器来看。串口通信实现上位机和硬件的直接沟通，一方面可以发送目标球的号码，命令视觉部分识别特定的目标球，另一方面，得到的定位信息也要通过串口发送给机器人的控制系统。然后，初始化OpenNI，设置图像的生成格式和帧频。OpenNI是开源的Kinect驱动中间件，我们使用它来实现驱动kinect进行工作，进而生成原始的色彩图和深度图。进入主循环，首先不断地接受串口的指令，确定目标球，利用OpenNI生成并转化为OpenCV可处理的8位深度图g_c8BitDepth和彩色图imag。这就是我们使用OpenCV进行处理的原始图像，包含了我们所需的全部信息。接着，利用Nifunction()函数将深度图前景分离，利用核心函数picprocess()找到目标球x轴位置point和距离juli，通过串口发送位置point和距离juli信息给上位机，并且我们还使用cvGetTickCount()计算程序处理时间，以便实现对程序的处理性能的监测和优化。以上是对主函数的介绍，接下来，会对几个关键的子函数进行介绍，从而加深研究的深度。

串口收发原理串口是通过ZQD_ComInit()函数设置的，定义hCom = CreateFile()允许读写，然后打开串口com，设定读超时，写超时，再写入串口超时参数，设定输入输出缓存区参数，获取当前串口状态，然后设置串口参数，具体的参数是波特率9600bps,8位，无奇偶校验，1位停止位，进行到这，com口初始化就完成了。 彩色图和深度图原理利用OpenNI的库函数xContext.Init()，初始化OpenNI驱动  ，创建深度图彩色图生成器DepthGenerator，ImageGenerator，并且设置图像大小XRes=640，nYRes=320，帧频nFPS=30帧，图像对齐，之后开启OpenNI，一切准备好后，进入主循环中，开始更新数据WaitAndUpdateAll()，利用函数GetMetaData()生成彩色图和深度图的原始数据xColorData和xDepthData。这就是我们使用OpenCV进行处理的原始图像，包含了我们所需的全部信息。图像处理核心部分原理图像处理的核心函数是Picprocess()  ，首先使用Threshold检测深度图阈值化，在深度图中使用OpenCV的函数findContours()找出轮廓，然后利用多边形逼近轮廓加获取矩形和圆形边界框，得到所有Kinect视觉传感器视野中检测到的圆形球的位置和距离信息，其中就包括一个目标球，在对应的彩色图的感兴趣区域ImgROI中计算HSV空间的颜色比例，判断是否是目标球，在找到目标球，得到x轴位置point和距离juli，这两个变量是全局变量，从而实现了对目标球的识别和定位，之后返回主函数继续循环找球。

视觉部分还存在的不足和需要改进之处在实际的使用中，篮球机器人还存在着以下问题（1）目标球识别和定位容易受光照，人影等扰动的影响，同时代码运行的速度还可以更快。（2）参数调试没有可人机交互的界面，只能反复尝试，效率低。（3）比赛规则中还要求篮球机器人能够自动回位，这点在代码中没有体现出来。此外，还有其他的问题，比如软件，硬件的升级问题，因此，虽然目前篮球机器人的视觉系统比较完善，但还是存在着不少的问题需要解决。这也是接下来我们着力解决的方向。
--------------------- 

#include<iostream>
#include<fstream>
#include<opencv2/core/core.hpp>
#include<opencv2/highgui/highgui.hpp>
#include<opencv2/imgproc/imgproc.hpp>
#include<windows.h>
#include<winnt.h>
#include<XnCppWrapper.h>
//#pragma comment( lib,"openni.lib" )
usingnamespace cv;
usingnamespace std;
HANDLE hCom;
int ZQD_ComInit()
{      
 hCom= CreateFile(TEXT("COM2"),//COM1口         
     GENERIC_READ | GENERIC_WRITE, //允许读和写         
     0, //独占方式         
     NULL,         
     OPEN_EXISTING, //打开而不是创建      
        0, //同步方式        
      NULL);     
  if (hCom == (HANDLE)-1)    
   {          
    MessageBox(NULL, TEXT("打开COM失败"), TEXT("Error"), MB_OK);        
      return -1; 
      }    
   COMMTIMEOUTS TimeOuts;      
 //设定读超时      
 TimeOuts.ReadIntervalTimeout= 10;   
    TimeOuts.ReadTotalTimeoutMultiplier= 20;   
    TimeOuts.ReadTotalTimeoutConstant= 20;   
    //设定写超时    
   TimeOuts.WriteTotalTimeoutMultiplier= 500;  
     TimeOuts.WriteTotalTimeoutConstant= 2000;     
  // 写入串口超时参数     
  if (!SetCommTimeouts(hCom,&TimeOuts))  
     {         
     MessageBox(NULL, TEXT("写入超时参数错误"), TEXT("Error"),MB_OK);                     return -1;     
  }     
  // 设置输入输出缓冲区参数，返回非0表示成功     
   if (!SetupComm(hCom, 1024, 1024))   
    {            
  MessageBox(NULL, TEXT("设置串口读写缓冲区失败"), TEXT("Error"), MB_OK);              return -1;     
  }     
  DCB dcb;  
     // 获取当前串口状态信息(只需要修改部分串口信息),调用失败返回0      
 if (!GetCommState(hCom, &dcb))      
 {      
        MessageBox(NULL, TEXT("获取串口属性失败"), TEXT("Error"), MB_OK);              return -1;  
     }      
 dcb.BaudRate= 9600; //波特率为9600     
  dcb.ByteSize= 8; //每个字节有8位   
    dcb.Parity= NOPARITY; //无奇偶校验位     
  dcb.StopBits= ONESTOPBIT; //一个停止位   
    if (!SetCommState(hCom, &dcb))  
     {         
     MessageBox(NULL, TEXT("设置串口参数出错"), TEXT("Error"), MB_OK);              return -1;    
   }     
  return0;
}
// 全局变量定义及赋值
int point = 0;
int juli = 0;
Mat g_c8BitDepth;
vector<Mat> hsvSplit;
Mat hsv_image;
Mat img;
void NIfunction(int, void*)
//修改成指针形式
{      
 for (int i = 0; i < g_c8BitDepth.rows; i++) 
      for (int j = 0; j < g_c8BitDepth.cols; j++) 
      {          
    if ((g_c8BitDepth.at<uchar>(i, j) >= (1.0 * 1200 / 65535 * 255)) || (g_c8BitDepth.at<uchar>(i, j) <= 0))    
          {       
              g_c8BitDepth.at<uchar>(i, j) = 0;      
        }     
  }     
  imshow("深度图前景分离", g_c8BitDepth);
}
double bluede(Matasrc)
{     
  Mat image = asrc;  
     cv::Mat hsv_image;   
     //转HSV   
    hsv_image.create(image.size(),image.type());  
     cv::cvtColor(image,hsv_image, CV_BGR2HSV);    
   vector<cv::Mat> channels;   
    cv::split(hsv_image,channels);  
     int num_row = image.rows;   
    int num_col = image.cols;   
    int count = 0;     
  int count_blue = 0;  
     for (int i = 0; i < num_row; i += 2)   
    {  
            const cv::Vec3b* curr_r_image = image.ptr<const cv::Vec3b>(i);  
            constuchar* curr_r_hue = channels[0].ptr<constuchar>(i);  
            constuchar* curr_r_satur = channels[1].ptr<constuchar>(i);   
           constuchar* curr_r_value = channels[2].ptr<constuchar>(i);              constuchar*inData = image.ptr<uchar>(i);   
           //     uchar*outData= mask.ptr<uchar>(i);  
            for (int j = 0; j < num_col; j ++)
{             
        if ((curr_r_hue[j] <= 120 && curr_r_hue[j] >= 90) && (curr_r_satur[j]>24 && curr_r_satur[j]<255))        
             {       
                     count_blue++;count++;   
                  }        
             else 
count++;        
      }     
  }    
   double blue;   
    blue= 1.0*count_blue / count; 
#ifdef_debug  
     cout<< "count" << count << endl; 
      cout<< "blue   " << blue << endl;
#endif      
    return blue;
}
double yellowde(Matasrc)
{     
  Mat image = asrc;  
     cv::Mat hsv_image; 
       //转HSV    
   hsv_image.create(image.size(),image.type()); 
      cv::cvtColor(image,hsv_image, CV_BGR2HSV);  
     vector<cv::Mat> channels;      
 cv::split(hsv_image,channels); 
      int num_row = image.rows;  
     int num_col = image.cols;   
    int count = 0;    
   int count_yellow = 0; 
      for (int i = 0; i < num_row; i ++)  
     {         
     const cv::Vec3b* curr_r_image = image.ptr<const cv::Vec3b>(i); 
             constuchar* curr_r_hue = channels[0].ptr<constuchar>(i);              constuchar* curr_r_satur = channels[1].ptr<constuchar>(i);    
          constuchar* curr_r_value = channels[2].ptr<constuchar>(i);              constuchar*inData = image.ptr<uchar>(i);   
           //     uchar*outData= mask.ptr<uchar>(i);   
           for (int j = 0; j < num_col; j += 2)
{                 
    if ((curr_r_hue[j] <= 36 && curr_r_hue[j] >= 16) && (curr_r_satur[j]>77 && curr_r_satur[j] < 255))        
             {                            count_yellow++;count++;    
                 }                
     else
 count++;     
         }    
   }      
 double yellow;  
     yellow= 1.0*count_yellow / count;
#ifdef_debug      
 cout<< "count" << count << endl;   
    cout<< "yellow   " << yellow << endl;
#endif         
 return yellow;
}
double redde(Matasrc)
{      
 Mat image = asrc;   
    cv::Mat hsv_image;  
      //转HSV      
 hsv_image.create(image.size(),image.type()); 
      cv::cvtColor(image,hsv_image, CV_BGR2HSV);    
   vector<cv::Mat> channels;   
    cv::split(hsv_image,channels);   
    int num_row = image.rows;    
   int num_col = image.cols;    
   int count = 0;     
  int count_red1 = 0;  
     int count_red2 = 0;   
           for (int i = 0; i < num_row; i ++)   
    {          
    const cv::Vec3b* curr_r_image = image.ptr<const cv::Vec3b>(i); 
             constuchar* curr_r_hue = channels[0].ptr<constuchar>(i);              constuchar* curr_r_satur = channels[1].ptr<constuchar>(i);      
        constuchar* curr_r_value = channels[2].ptr<constuchar>(i);    
          constuchar*inData = image.ptr<uchar>(i);   
           //     uchar*outData= mask.ptr<uchar>(i);   
           for (int j = 0; j < num_col; j += 2)
{                 
    if ((curr_r_hue[j] <= 12 && curr_r_hue[j] >= 0) && (curr_r_satur[j]>60 && curr_r_satur[j]<255) && (curr_r_value[j]>46 && curr_r_value[j]<200))
//找颜色         
            {                
            count_red1++;count++;            
         }            
         elseif ((curr_r_hue[j] <= 180 && curr_r_hue[j] >= 156) && (curr_r_satur[j]>60 && curr_r_satur[j]<255) && (curr_r_value[j]>46 && curr_r_value[j]<200))  
                
   {            
                count_red2++;count++;   
                  }       
              else count++;    
          }    
   }      
 double red1, red2; 
      red1= 1.0*count_red1 / count;   
    red2= 1.0*count_red2 / count;
#ifdef_debug    
   cout<< "red   " << red1 + red2 << endl;
#endif      
    return (red1 + red2);
}
void picprocess(Matasrc, Matdepth, intdata)
{    
   Mat srcImage = depth;   
    int a = data;      
 Mat midImage, dstImage;
//临时变量和目标图的定义     
  //【3】转为灰度图并进行图像平滑  
     //cvtColor(srcImage, midImage,CV_BGR2GRAY);
//转化边缘检测后的图为灰度图 
      //GaussianBlur(midImage, midImage,Size(5, 5), 2, 2);    
   //     sharpenImage(midImage,midImage);     
  //定义一些参数       Mat threshold_output;   
    vector<vector<Point>> contours;     
  vector<Vec4i> hierarchy;     
  // 使用Threshold检测边缘   
    threshold(srcImage,threshold_output, 20, 255, THRESH_BINARY);     
  //threshold_output =bianyuanjiance(srcImage);#ifdef_debug    
   cv::imshow("深度图阈值化", threshold_output);#endif     
  // 找出轮廓    
   findContours(threshold_output,contours, hierarchy, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_SIMPLE, Point(0, 0));    
   Mat imgROI;     
  //多边形逼近轮廓 + 获取矩形和圆形边界框   
    vector<vector<Point> >contours_poly(contours.size());   
    //vector<Rect> boundRect(contours.size() );    
   vector<Point2f>center(contours.size());   
    vector<float>radius(contours.size());   
    //一个循环，遍历所有部分    
   int j = -1; double index = 0.05, temp = 0;  
     for (unsignedint i = 0;
 i < contours.size(); i++)    
   {           
   approxPolyDP(Mat(contours[i]), contours_poly[i], 3, true);
//用指定精度逼近多边形曲线         
      minEnclosingCircle(contours_poly[i],center[i], radius[i]);//对给定的 2D点集，寻找最小面积的包围圆形      
  }    
   // 绘制多边形轮廓 + 圆形框
#ifdef_debug     
  Mat drawing = Mat::zeros(threshold_output.size(), CV_8UC3);
#endif    
   for (unsignedint i = 0; i < contours.size(); i++)  
     {
#ifdef_debug        
      Scalar color = Scalar(0, 255, 0);

//设置绿色         
     drawContours(drawing,contours_poly, i, color, 1, 8, vector<Vec4i>(), 0, Point());
//绘制轮廓       
       circle(drawing,center[i], (int)radius[i], color, 2, 8, 0);
//绘制圆
#endif      
        if (((center[i].x - radius[i])>0) && ((center[i].y -radius[i]) > 0) &&   ((center[i].x+ radius[i]) <srcImage.cols) && ((center[i].y + radius[i]) <srcImage.rows) && (radius[i]<85)&& (radius[i]>20))  
            {      
               imgROI= asrc(Rect((center[i].x - radius[i]),(center[i].y - radius[i]), 2 * radius[i], 2 * radius[i]));            
        double red, blue, yellow;              
       if (a == 1)                
     {                  
          red= redde(imgROI);                         
   if ((red >= index) &&(red>0.45))
 {                              
     index= red;
 j = i;
//index不断值改变，动态改变排除条件                      
      }              
       }          
      elseif (a == 2){                     
       blue= bluede(imgROI);         
                   if ((blue >= index) &&(blue>0.3)) 
{                           
        index= blue; j = i;                        
    }           
          }              
       elseif (a == 3)
{                      
      blue= bluede(imgROI);    
                        red= redde(imgROI);                        
    if ((blue >= index) &&(blue>0.00) && (red >= temp)&& ((red>0.03)) && (red<0.35)) 
{                         
          index= blue;                          
         temp= red;          
                         j= i;                   
         }                
     }                
     elseif (a == 4)
{                        
    yellow= yellowde(imgROI);                            red= redde(imgROI);                          
  if ((yellow >= index) &&(yellow>0.08) && (red >= temp)&& (red>0.05) && (red<0.35)) 
{       index= yellow;                                
   temp= red;                        
           j= i;  }         
            }       
       }     
  }     
  if (j>-1)
//j记录最大概率的目标圆形顺序号   
    {          
    point= center[j].x;       
       juli= g_c8BitDepth.at<uchar>(center[j].y, center[j].x);
#ifdef_debug    
          cout<< a << "球：" << point << endl;         
     cout<< "球r ：" << radius[j] << endl;   
           cout<< "距离:" << juli << endl;   
           #endif   
    }
#ifdef_debug   
    cv::imshow("绘制轮廓", drawing);
#endif
}
int main()
{     
  char com_data;    
   ZQD_ComInit();     
  //串口接收       DWORD rCount;
//+接收的字节数    
   char inputData[1];     
  // 1a. initial OpenNI       xn::Context xContext;   
    xContext.Init();      
 // 1b. create depth generator       xn::DepthGenerator xDepth;  
     xDepth.Create(xContext);     
  // 1c. create image generator       xn::ImageGenerator xImage;   
    xImage.Create(xContext);   
    XnMapOutputMode mapMode;   
    mapMode.nXRes= 640;    
   mapMode.nYRes= 480;    
   //Set out mode  EHEIGTH       mapMode.nFPS= 30;  
     xImage.SetMapOutputMode(mapMode);   
    xDepth.SetMapOutputMode(mapMode);   
    // 1d. set alternative view point       xDepth.GetAlternativeViewPointCap().SetViewPoint(xImage);     
  // 3. start OpenNI       xContext.StartGeneratingAll();  
     while (1)     
  {        
      if (ReadFile(hCom, inputData, 1, &rCount, NULL));   
           double t = (double)cvGetTickCount();     
         if (inputData[0] == '1' || inputData[0] == '2' || inputData[0] == '3' || inputData[0] == '4')           
          com_data= inputData[0];
//不断接受指令，更新目标球           
   // 4. update data     
         xContext.WaitAndUpdateAll();            
  // 5. get image data 只有颜色信息        
      xn::ImageMetaData xColorData;     
         xImage.GetMetaData(xColorData);         
     // 5a. convert to OpenCV form         
     cv::Mat r_cColorImg(xColorData.FullYRes(),xColorData.FullXRes(), CV_8UC3, (void*)xColorData.Data());            
  // 5b. convert from RGB to BGR       
       cvtColor(r_cColorImg,img, CV_RGB2BGR);            
  // 6. get depth data 只有距离信息          
    xn::DepthMetaData xDepthData;         
     xDepth.GetMetaData(xDepthData);             
 // 6a. convert to OpenCV form          
    cv::Mat r_cDepthImg(xDepthData.FullYRes(),xDepthData.FullXRes(), CV_16UC1, (void*)xDepthData.Data());           
   // 16b. convert to 8 bit          
    r_cDepthImg.convertTo(g_c8BitDepth,CV_8U, 256.0 / 65536);
 #ifdef_debug              cv::imshow("原始深度图", g_c8BitDepth);
//未前景分离的深度图              cv::imshow("原始彩色图", img);
//处理前
#endif        
      NIfunction(0, 0);       
       switch (com_data)         
     {            
  case'1'://1号目标球，红色球                     picprocess(img,g_c8BitDepth, 1);               
      break;   
           case'2':
//2号目标球，蓝色球                     picprocess(img,g_c8BitDepth, 2);                 
    break;           
   case'3':
//3号目标球，蓝红色球                     picprocess(img,g_c8BitDepth, 3);                  
   break;             
 case'4':
//4号目标球，黄红色球                     picprocess(img,g_c8BitDepth, 4);                  
   break;            
  default:                     break;        
      }             
 //串口发送，发送的水平位置和距离信息是字符格式       
       juli= juli*1.0 / 255 * 65535;     
         DWORD wCount;
//发送的字节数        
      char outputData[1024];  
            if (point / 100)      
        {             
        outputData[0] = '0' + point / 100;   
           }            
  else                  
   outputData[0] = ' ';    
          point= point % 100;      
        if (point / 10)                     outputData[1] = '0' + point / 10;          
    else      
               outputData[1] = ' ';   
           point= point % 10;    
          if (point)            
         outputData[2] = '0' + point;  
            else           
          outputData[2] = ' ';     
         if (juli / 1000)      
               outputData[3] = '0' + juli / 1000; 
             else          
           outputData[3] = ' ';    
          juli= juli % 1000;      
        if (juli / 100)             
        outputData[4] = '0' + juli / 100;    
          else               
      outputData[4] = ' ';     
         juli= juli % 100;          
    if (juli / 10)           
          outputData[5] = '0' + juli / 10;           
   else       
              outputData[5] = ' ';       
       juli= juli % 10;            
  if (juli)                
     outputData[6] = '0' + juli;       
       else           
          outputData[6] = ' ';       
       outputData[7] = 'z';         
     outputData[8] = outputData[0] + outputData[1] + outputData[2] + outputData[3] + outputData[4] + outputData[5] + outputData[6];          
    if (outputData[8] == 'z')            
         outputData[8] = outputData[8] + '1';          
    if (WriteFile(hCom, outputData, 9, &wCount, NULL) == 0)              {                 
    MessageBoxA(NULL, "写入串口数据失败", MB_OK, 0);                  
   exit(1);       
       }                 
           t= ((double)cvGetTickCount() - t) /(cvGetTickFrequency() * 1000);         
     cout<< "处理时间: " << t << "ms" << endl;    
   }           
   return0;
}

原文：https://blog.csdn.net/qq_39231566/article/details/80978449