前言

最近，博主在比赛中有部分内容用到视觉识别二维码并进行跟随的内容。此部分主要由组内负责视觉的同学完成，此处备份留作后用。

整体逻辑主要采用PID中的P算法，根据小车摄像头实时获取到目标二维码在图像中的位置，计算其距离小车的长度以及偏离中小线的误差分别给予小车x和y方向上的线速度，从而达到，使得小车跟随二维码并当达到死区时停止的效果。

全部代码采用Python编写，主要采用了Opencv库和ROS框架实现。

一、二维码获取

首先需要完成第一步，即打开摄像头获取到图像数据并监测是否读取到二维码数据。

1.获取摄像头图片

此处为基本操作，不予分析。

import cv2
from cv2 import aruco
 
# 获取摄像头图片
cap = cv2.VideoCapture(0)
ret, frame = cap.read()
while ret:
    # 获取下一帧图片
    ret, frame = cap.read()
    # 读取摄像头画面
    gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)

2.识别二维码

识别二维码主要采用了开源的aruco库进行识别,此处需要注意选择的二维码字典需要依据自己的情况进行更换。

aruco_dict = aruco.Dictionary_get(aruco.DICT_4X4_50)

此处导入的二维码字典为包含50个标记的4X4大小的二维码。每个词典中所有的Aruco标记均包含相同数量的块或位(例如4×4、5×5、6×6或7×7)，且每个词典中Aruco标记的数量固定(例如50、100、250或1000)。

aruco_dict = aruco.Dictionary_get(aruco.DICT_4X4_50)
parameters =  aruco.DetectorParameters_create()
corners, ids, rejectedImgPoints = aruco.detectMarkers(gray, aruco_dict,parameters=parameters)
aruco.drawDetectedMarkers(frame, corners,ids)

通过上述代码即可获取到二维码在字典中的ID。可通过如下代码进行监测是否查询到目标二维码。

# 当读取到二维码ID：
if not (ids is None):
        # 打印ID值
        print(ids[0][0])

3.完整代码

全部逻辑如下所示：

import time
import cv2
from cv2 import aruco
 
# 获取摄像头图片
cap = cv2.VideoCapture(0)
ret, frame = cap.read()
while ret:
    # 获取下一帧图片
    ret, frame = cap.read()
    # 读取摄像头画面
    gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)

    # 加载二维码字典
    aruco_dict = aruco.Dictionary_get(aruco.DICT_4X4_50)
    parameters =  aruco.DetectorParameters_create()
    corners, ids, rejectedImgPoints = aruco.detectMarkers(gray,
                                                          aruco_dict,
                                                          parameters=parameters)
    aruco.drawDetectedMarkers(frame, corners,ids)

    # 当读取到二维码ID时触发：
    if not (ids is None):
            # 打印二维码的ID值
            print(ids[0][0])

二、二维码处理

获取到二维码ID后，为了实现整体逻辑需要进一步对二维码图像进行处理，从而获取到二维码的偏差值error和距离值length。

1、获取偏差

由于博主使用的摄像头宽度为680，所以在计算error时中线处即表示为340。通过获取二维码两侧像素所在行计算二维码中心线并减去图像中心，从而可以获取到二维码偏差值。

error = (corners[num][0][0][0] + corners[num][0][1][0]) / 2 - 340

2.获取距离

在计算取距离时，此处博主经过实验发现在我的摄像头中，当二维码的宽度达到190px时停止可以达到较好效果，故而用190减去二维码的宽度，从而计算取距离值。

length = 190 - (corners[num][0][1][0] - corners[num][0][0][0])

3.初步整合

通过整合上述两部，可以获取到目标二维码的偏差和距离值。

error_pub = rospy.Publisher('/cv/error', Int16, queue_size=5)
length_pub = rospy.Publisher('/cv/length', Int16, queue_size=5)
final_pub = rospy.Publisher('/cv/final', Int16, queue_size=5)

num = 0
for ider in ids[0]:
    if (ider == 0 or ider == 1 or ider == 2):
        if ider == final_ID:  # 目标二维码ID
            error = (corners[num][0][0][0] + corners[num][0][1][0]) / 2 - 340
            length = 190 - (corners[num][0][1][0] - corners[num][0][0][0])
            msg.data = int(error)
            error_pub.publish(msg)
            msg.data = int(length)
            length_pub.publish(msg)
            if abs(error) < 10 and abs(length) < 10:  # 到最终点
                msg.data = 1
                final_pub.publish(msg)
            break
num = num + 1

由于博主的使用场地中仅需要三个二维码，所以对应id仅为0-2时方为有效判别。在获取到数据后，将其发布至对应节点中即可。最后，通过监测偏差值小于10时，认定抵达终点并发布终点flag，停止发送速度。

4.进阶判断

在实际应用时，博主的场地中存在三个并排二维码，故而会进行判断若查看到的二维码id小于目标二维码像左移动，反之向右移动。由此需要增加一个flag判读当前是否监测到目标二维码。
整合逻辑后，整体代码如下所示。

#!/usr/bin/env python
# coding:utf-8

import cv2
from cv2 import aruco
from std_msgs.msg import Int16
import rospy
    
if __name__ == '__main__':
    cv_flag = 0
    cap = cv2.VideoCapture(0)
    ret, frame = cap.read()
    # 创建节点
    rospy.init_node('mark_node')
    error_pub = rospy.Publisher('/cv/error', Int16, queue_size=5)
    length_pub = rospy.Publisher('/cv/length', Int16, queue_size=5)
    final_pub = rospy.Publisher('/cv/final', Int16, queue_size=5)

    final_ID = rospy.get_param("~final_ID", 1)

    while True:
        ret, frame = cap.read()
        frame = cv2.flip(frame, 1)  ##图像左右颠倒
        h1, w1 = frame.shape[:2]
        # 读取摄像头画面
        gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
        aruco_dict = aruco.Dictionary_get(aruco.DICT_4X4_50)
        parameters = aruco.DetectorParameters_create()
        corners, ids, rejectedImgPoints = aruco.detectMarkers(gray,
                                                              aruco_dict,
                                                              parameters=parameters)

        aruco.drawDetectedMarkers(frame, corners, ids)
        if not (ids is None):
            # print(ids[0][0])
            num = 0
            idflag = 0
            for ider in ids[0]:
                if (ider == 0 or ider == 1 or ider == 2):
                    if ider == final_ID:  # 目标二维码ID
                        error = (corners[num][0][0][0] + corners[num][0][1][0]) / 2 - 340
                        length = 190 - (corners[num][0][1][0] - corners[num][0][0][0])
                        msg.data = int(error)
                        error_pub.publish(msg)
                        msg.data = int(length)
                        length_pub.publish(msg)
                        idflag = 1
                        if abs(error) < 10 and abs(length) < 10:  # 到最终点
                            msg.data = people
                            final_pub.publish(msg)
                        break
                num = num + 1
            # 没看到目标二维码
            if idflag < 1:
                for ider in ids[0]:
                    if (ider == 0 or ider == 1 or ider == 2):
                        if ider < final_ID:  # 在目标二维码ID 右侧
                            error = -170 # 左移
                            length = 190 #确保190-length为0
                            msg.data = int(error)
                            error_pub.publish(msg)
                            msg.data = int(length)
                            length_pub.publish(msg)
                        elif ider > final_ID:  # 在目标二维码ID 左侧
                            error = 170 # 右移
                            length = 190 #确保190-length为0
                            msg.data = int(error)
                            error_pub.publish(msg)
                            msg.data = int(length)
                            length_pub.publish(msg)

三、控制移动

在完成图像处理后，需要采用ROS框架，发布速度控制小车移动。

1、数据处理

对于获取到的偏差值和距离值数据，需要进行限幅和死区设置，此处参数的具体值需要依据实际进行调参。

def errorCallBack(msg):
    error = msg.data
    # 限幅115右， -150左
    if error > 115:
        error = 115
    elif error < -150:
        error = -150
    elif -50 < error < 10:              # 死区
        error = 0
    print("获取error:{}".format(error))
    

def lengthCallBack(msg):
    length_error = msg.data
    if length_error > 180:
        length_error = 180
    elif length_error < -180:
        length_error = -180
    elif -10 < length_error < 10:       # 死区
        length_error = 0
    
    print("获取lenerror:{}".format(length_error))

2、计算速度值

根据处理后获得的偏差值，需要乘上系数从而得到实际速度值：

def setVel(error, length_error, err_kp, length_err_kp):
    vel = Twist()
    v_y = -err_kp * error
    
    if -30 < length_error < 30:
        v_x = length_err_kp * length_error * 0.5

    elif -80 < length_error < -30 or 30 < length_error < 80:
        v_x = length_err_kp * length_error * 0.8

    else:
        v_x = length_err_kp * length_error

    vel.linear.x = v_x
    vel.linear.y = v_y
    cmd_pub.publish(vel)

为了提高速度，对不同距离范围进行分段式赋予不同参数，从而确保较远处给予的速度较大，近处则给予的速度较小。

3、整体逻辑

将上述逻辑封装成类后如下所示：

#!/usr/bin/env python2.7
# encoding: utf-8
import rospy

from std_msgs.msg import Int16
from geometry_msgs.msg import Twist

class CVControl:
    def __init__(self):
        rospy.init_node('CVControl', anonymous=True) 
        # 订阅话题
        self.error_sub = rospy.Subscriber('/cv/error', Int16, self.errorCallBack)
        self.length_sub = rospy.Subscriber('/cv/length', Int16, self.lengthCallBack)
        # 发布数据
        self.cmd_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=5)
        
    def errorCallBack(self, msg):
        self.error = msg.data
        # 限幅115右， -150左
        if self.error > 115:
            self.error = 115
        elif self.error < -150:
            self.error = -150
        elif (self.error < 10) and (self.error > -50):              # 死区
            self.error = 0
        # print("获取error:{}".format(self.error))
        
    def lengthCallBack(self, msg):
        self.length_error = msg.data
        if self.length_error > 180:
            self.length_error = 180
        elif self.length_error < -180:
            self.length_error = -180
        elif (self.length_error < 10) and (self.length_error > -10):
            # 死区
            self.length_error = 0
        # print("获取lenerror:{}".format(self.length_error))
    
    def setVel(self, err_kp, length_err_kp):
        vel = Twist()
        self.v_y = -err_kp * self.error
        
        if -30 < self.length_error < 30:
            self.v_x = length_err_kp * self.length_error * 0.5

        elif -80 < self.length_error < -30 or 30 < self.length_error < 80:
            self.v_x = length_err_kp * self.length_error * 0.8

        else:
            self.v_x = length_err_kp * self.length_error

        vel.linear.x = self.v_x
        vel.linear.y = self.v_y
        self.cmd_pub.publish(vel)

if __name__ == '__main__':
    cvcontrol = CVControl()
    cvcontrol.setVel(0.001, 0.001)