基于颜色识别的二自由度云台目标跟踪（Opencv&Arduino控制舵机）

1、简单介绍       

        之前发布了一篇单目测距文章，发布了代码资源，没想到还有挺多同学需要，由于一直工作很忙，都没有心情发布博客，至今也只发布过一篇，希望写博客的习惯能够坚持下来。

       这是以前大学时做过的一个简单目标跟踪的二自由度云台：摄像头识别物体轮廓（Python OpenCV）——获取物体的中心坐标——程序判断摄像头画面中心是否与物体中心重合——若没有重合，则控制二自由度云台转动对准物体中心，其中Arduino控制舵机，电脑通过串口与Arduino通信。最终实现的效果如下B站视频（想上传GIF的但没成功），这段程序也可以移植到树莓派上~

简单的颜色目标跟踪-二自由度云台

---

2、摄像头如何获取物体轮廓中心坐标？

实现的方法可参考我的第一篇博客https://blog.csdn.net/qq_30130435/article/details/86601370，这篇博客讲述如何区分颜色以及获取红色物体的坐标，稍微改下代码就可以获取到红色块的中心了，在上面的视频中，我用的红色目标如下图：

 3、传输什么数据给Arduino？

如下图，我手动对摄像头获取的图像画了XY坐标系，当检测到红色方块不在坐标系中心时，程序会通过串口向Arduino发送转动的信号，每次转动2度，直到红色目标的中心移动至图像坐标中心。其中下面的舵机向左右转动或上面的舵机上下转动，因为是比较简单的二自由度舵机，我没有用线性代数的方式来写机械臂的运动学或逆运动学，所以虽然实现了目标跟踪，但是在跟踪的过程中舵机是在抖动的（抖动也与识别到的目标位置不固定有关系）

 

 4、核心代码

代码比较长，只贴核心部分，复现有问题的同学可私聊我（才不告诉你我是想要点小费）

         4.1上位机代码，这部分我只贴控制舵机的部分，图像识别部分参考上面提到的另一篇博客

# Arduino端口
serialPort="COM3"
baudRate=9600
ser=serial.Serial(serialPort,baudRate,timeout=0.5)

# 判断在哪个象限
def JudgeQuadrant(axis_x,axis_y):
    distanceX=axis_x-320;
    distanceY=axis_y-240;
    if(distanceX<0 and distanceY<0):
        return 2
    if(distanceX>0 and distanceY<0):
        return 1
    if (distanceX<0 and distanceY > 0):
        return 3
    if (distanceX>0 and distanceY > 0):
        return 4

#  返回与x轴的夹角
def computeAngle(axis_x,axis_y):
    dx=abs(axis_x-320);dy=abs(axis_y-240)
    if dx==0:
        return
    tanA=dy/dx;
    angle1=math.atan(tanA)
    return math.degrees(angle1)

while camera.isOpened():
    (grabbed, frame) = camera.read()
    marker = find_marker(frame) #获取物体轮廓，具体参考我的第一篇博客
   
       continue
  
    box = cv2.boxPoints(marker)
    box = np.int0(box)
    # 此轮廓中心  marker[0][0]为 x轴  marker[0][1]为y轴
    JudgeQuadrant(marker[0][0], marker[0][1])
    # 象限
    Quadrant=JudgeQuadrant(marker[0][0], marker[0][1]);
    # 角度
    if computeAngle(marker[0][0], marker[0][1])==None:
        continue;
    Angle=int(computeAngle(marker[0][0], marker[0][1]));
    # 下面的舵机，如果与x轴的角度相差超过20度，则根据所在象限控制左右转动
    if 90-Angle>20:
       ser.write(str.encode(str(Quadrant)))
    # 上面的舵机
    if Angle > 20 and (Quadrant==1 or Quadrant==2):
    #若是在1、2象限则控制上面的舵机向下转动
       ser.write(str.encode(str(6)))
    elif Angle >20 and (Quadrant==3 or Quadrant==4):
    #若是在3、4象限则控制上面的舵机向下转动
        ser.write(str.encode(str(5)))

         4.2 Arduino核心代码部分

void setup(){
  pinMode(11, OUTPUT);
  Serial.begin(9600);
  myservo1.attach(pin,500,2500);
  myservo2.attach(pin2,500,2500);
  setMiddle();
  delay(1000);
  }
//初始化舵机角度
void setMiddle(){
  for(pos=0;pos<=90;pos+=1){
    pos2+=1;
    myservo1.write(pos);
    myservo2.write(pos);
    delay(20);
    }
  }
//向下转
void toDown(){
    pos2-=changeRange;
    if(pos2<0)return;
    myservo2.write(pos2);
    delay(10);
  }
//向上转
  void toUp(){
     pos2+=changeRange;
    if(pos2>180)return;
     
      myservo2.write(pos2);
      delay(10);
  }
//向左转
void toLeft(){
     pos-=changeRange;
    if(pos<0)return;
      
      myservo1.write(pos);
//      delay(30);
  }
//向右转
void toRight(){
      pos+=changeRange;
      if(pos>180)return;
      
      myservo1.write(pos);
//      delay(30);
  }  
//接受上位机的象限信息，以判断是向左转还是向右转
void judgeDirection(char Quadrant){
   switch(Quadrant){
     case '1': toLeft(); break; case '2': toRight(); break; case '3':  toRight(); break; case '4': toLeft();  break; case '5': toDown(); break; case '6': toUp(); break;
    }
  }  

5、下一篇应该是二自由度云台目标跟踪与深度学习图像目标检测相结合的文章~