序言

拿到X3 Module已经有一段时间了，这期间我使用RDK X3 Module参与了第十一届全国大学生光电设计竞赛中的赛题二：“迷宫寻宝”光电智能小车，通过东南区赛选拔后进入国赛现场赛，并在现场赛中取得了国赛一等奖的成绩。

不得不说，X3在比赛中的表现真的是十分优秀的，高速推理yolov5不说，其作为上位机控制整个自动小车，在开发主程序时也十分舒适。

赛题介绍（简略说明）

竞赛说明

智能车通过光电传感寻找、判断迷宫中的真、假宝藏，并在完成寻宝后走出迷宫。竞赛采用红、蓝对抗的形式，即每场比赛红、蓝两队在同一迷宫中比拼、对抗。比赛胜负依据各自的寻宝数、误判数、是否走出迷宫、耗时等确定。

竞赛规则

智能车：由参赛队自备，自动驾驶小车。严禁使用遥控或其他非光电技术方式导航、搜寻和识别。智能车应具有一键式启动开关。

识图装置：小车可附带车载或分体的识图装置用于识别藏宝图。识图装置在拍摄藏宝图后，需通过一键式操作自动完成迷宫、宝物等的识别、位置校正和分析，不得人工介入。

竞赛场地

迷宫：在室内体育馆里搭建，面积约 4m×4m，如图 1 所示。迷宫地面颜色为白色，隔板只在与边界平行或垂直的方向放置。隔板之间为迷宫车道，宽约 40cm。沿车道的中线粘贴约 2cm 宽的黑色胶带作为循迹线。迷宫左下角开口处地面涂蓝色，为蓝队小车的迷宫入口，同时也是红队的迷宫出口。迷宫右上角开口地面涂红色，是红队入口，蓝队出口。

藏宝图：彩色打印在 A4 白纸上，藏宝图四角标有用于定位的方形标志点，供参赛者拍照后进行透视校正。图中的黑线代表迷宫的隔墙，循迹线不在图中标出。用黑色圆点代表宝藏或伪宝藏位置（由软件随机生成 8 个点），红色块代表红队入口和蓝队出口；蓝色块代表蓝队入口和红队出口，如图 3 所示。

宝藏：每场比赛，双方各有 3 个与队色（蓝、红）相近的多米诺骨牌己方宝藏，另有红、蓝各 1 个骨牌牌面有贴纸的伪宝藏（如图 2 ）。宝藏及伪宝藏的位置按藏宝图随机摆放、双色交错对称，并且颜色关于中心点对称。

评分规则

双方有5分钟识图时间识别藏宝图，随后一键式启动小车开始寻宝。
小车以直接碰撞的方式推倒己方真宝藏，己方加分，碰倒对方宝藏或伪宝藏，对方加分，寻宝限时10分钟。
如果小车在比赛中发生了撞墙，倾覆等情况，由参数选手发出申请，裁判将小车拿回起点，重新启动小车，罚时15秒，中断次数+1。

胜负判定的优先级为：寻宝数（分数） > 耗时 > 中断次数。

以上为规则简略说明，具体细则请查询全国大学生光电设计竞赛官网。

实现思路

机械方面在尝试了几种方案后，最终决定采用2轮差速小车，该结构可以在迷宫中灵活运动，控制方便。

供电使用18650电池组（6节串联 25.2V 3000mAh）通过2组LM2596降压电源模块分别提供5V逻辑供电和12V动力供电。

下位机采用Arduino Uno + 自制PCB扩展板，该扩展板引出了电机接口（包括编码器），串口，iic接口以及供电接口等。并且搭载了陀螺仪和2路电机控制模块。

通过下位机扩展板引出的iic接口，连接了一板八路灰度传感器模块，并且通过12V供电接口外置一条LED灯带，用于视觉补光。

上位机使用RDK X3 Module以及官方载板，同样自制了PCB板，引出串口连接下位机，并且搭载指示灯和按键，作为控制台。

上位机主要负责主程序控制以及视觉和逻辑算法的运行，例如：基于纯OpenCV数字图像处理的藏宝图识别算法，自创路径规划算法，基于RDK X3上yolov5目标检测模型的宝藏骨牌识别算法。

下位机

由于本文主要讲解RDK X3 Module在该赛中的应用，所以下位机不讲述具体思路，只介绍上位机对下位机的串口控制方式。下位机的代码由于反复调试，结构较乱，也就不在此做展示了，感兴趣的朋友可以前往我的代码仓库查看：github.com/hachi-leaf/guangsai_national/tree/main/Uno_1.1

根据赛题的地图，下位机需要实现的运动控制主要包括： 直线循迹， 左/右转弯， 掉头， 撞击等运动方式。

调试好各个运动方式的参数后，在下位机中将其整合，使其能够接受上位机发送的路径信息并依照次路径运动，并在路径结束后发送字符反馈给上位机，以下是串口通信控制方式图解。

按照此思路做好下位机程序后，调试稳定，上位机便可以通过串口控制小车在整张地图中运动了，接下来就可以开始编写上位机部分程序了。

上位机

上位机的程序主要包括：藏宝图识别，路径规划，藏宝图骨牌识别，以及进程控制几个方面：

藏宝图识别

赛题的藏宝图打印图如下，打印在A4纸上。

大部分人的算法思路应该都是通过四个角落的定位点来定位矫正，但是我选择了另外一种思路来处理:

首先对图片进行resize，二值化，高斯滤波操作

img = cv2.resize(img,(720,720))

img_gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)

img_GaussianBlur = cv2.GaussianBlur(img_gray, (3,3), 0)

随后进行Canny边缘检测和膨胀操作

img_Canny = cv2.Canny(img_GaussianBlur,63,255)

kernel_3 = np.ones((3,3),np.uint8)
img_dil = cv2.dilate(img_Canny,kernel_3,iterations = 1)

随后使用cv2的一个库函数cv2.findContours寻找图片中的所有矩形，并筛选出位置最靠近中间的矩形，提取其角落坐标点，根据该坐标点，对原图像进行透视矫正

# Find the contours
countours, __ = cv2.findContours(img_dil, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)

con_date = []
# Traverse the contours
for __ , cnt in enumerate(countours):
    epsilon = 0.1 * cv2.arcLength(cnt, True)  # Calculates the length of the contours line
    approx = cv2.approxPolyDP(cnt, epsilon, True)

    # Append the vertex coordinates and the distance from center
    point_date = []
    point = approx.reshape(-1,2)
    point = point.tolist()
    if len(point) == 4:
        point_date.append(point)
        ly = ( point[0][0] + point[1][0] + point[2][0] + point[3][0] )/4
        lx = ( point[0][1] + point[1][1] + point[2][1] + point[3][1] )/4
        lo = ((lx-359.5)**2 + (ly-359.5)**2)**(1/2)
        point_date.append(lo)
        con_date.append(point_date)

# Output the error1
if len(con_date) == 0:
    return 1 , None

# Find the point that has the shortest distance from center
min_len =  1000
min_i = -1
for i in range(len(con_date)):
    if con_date[i][1] < min_len:
        min_len = con_date[i][1]
        min_i = i

true_point = con_date[min_i][0]

# Calculate the central coordinate again
ty = ( true_point[0][0] + true_point[1][0] + true_point[2][0] + true_point[3][0] )/4
tx = ( true_point[0][1] + true_point[1][1] + true_point[2][1] + true_point[3][1] )/4

# Compute transformation matrix
point1 = [None,None,None,None]
for i in range(4):
    if true_point[i][0] - ty < 0 and true_point[i][1] - tx < 0:
        point1[0] = true_point[i]
    elif true_point[i][0] - ty < 0 and true_point[i][1] - tx > 0:
        point1[1] = true_point[i]
    elif true_point[i][0] - ty > 0 and true_point[i][1] - tx > 0:
        point1[2] = true_point[i]
    elif true_point[i][0] - ty > 0 and true_point[i][1] - tx < 0:
        point1[3] = true_point[i]
    else:
        return 2 , None # Output the error2

# Output the error5
try:
    point1 = np.array(point1,dtype = "float32")
except:
    return 5 , None
point2 = np.array([[0,0],[0,512],[512,512],[512,0]],dtype = "float32")
M = cv2.getPerspectiveTransform(point1,point2)

# Perspective transformation
Map_p1 = cv2.warpPerspective(img,M,(512,512))

得到如下图像

再进行灰度转化，完全矫正图像

# Grayscale conversion
Map_gray = cv2.cvtColor(Map_p1, cv2.COLOR_BGR2GRAY)

# Rotation correction
point3 = np.array([[4,4],[466,0],[508,508],[47,511]],dtype = "float32")
point4 = np.array([[16,16],[656,16],[656,656],[16,656]],dtype = "float32")
M_t = cv2.getPerspectiveTransform(point3,point4)
Map_p2 = cv2.warpPerspective(Map_gray,M_t,(672,672))

# Use Binarization to get directions
__ , Map_p3 = cv2.threshold(Map_p2, 127 , 255, cv2.THRESH_BINARY_INV)

到此为止宝藏点坐标的提取就十分简单了，但我的算法中还在代码中用列表存取了地图地形，便于与矫正出的图像进行比对验证。

具体代码见：github.com/hachi-leaf/guangsai_national/tree/main/RDK_guangsai/map_read.py

效果展示见：www.bilibili.com/video/BV1FV4y167Lm

路径规划

路径规划部分，我的算法在撰写时没有参考任何文献，完全是自己的思路，以下命名为“伪粒子群算法”

首先看地图，无视入口和出口后，地图是10x10的，加上墙壁也就是21x21，可以直接用二维列表存储改地图。

明确了这个数据结构后，我们开始做路径规划部分，路径规划主要有2个部分：
1.两点之间最短距离
2.对所有的目标点进行排序，确定遍历顺序，即全局规划

首先是两点之间最短距离，我们的路径规划思路是：在起点生成一个粒子，使其朝着四个方向（前提是没有墙阻挡）运动，遇到分叉口时，粒子将会复制，分别进入各个岔路，当有粒子的坐标等于终点坐标时，即将此粒子的路径看作最短路径，结束循环。下面开始代码实现。

首先定义类class map(),init设置基本属性(地图模型和路径信息容器)

def __init__(self):
    self.map = np.array([
        [0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0],
        .....
        [0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0]])

    self.history = []

定义坐标方法

def my_loc(self,loc):
    self.myloc = loc
    self.map[loc[0]*2+1,loc[1]*2+1] = 2

定义宝藏点方法

def set_targets(self,targets):
    for target in targets:
        self.map[target[0]*2+1,target[1]*2+1] = 3

可视化打印地图方法

def map_print(self):
    print("+---0---1---2---3---4---5---6---7---8---9--> y")
    for i in range(21):
        if i%2 == 0:
            print("|",end=' ')
        else:
            print(int((i-1)/2),end=' ')
        for j in range(21):
            if self.map[i,j] == 1:
                print(' ',end=' ')
            if self.map[i,j] == 0:
                print('#',end=' ')
            if self.map[i,j] == 2:
                print('*',end=' ')
            if self.map[i,j] == 3:
                print('@',end=' ')
        print()
    print('V')
    print('x')

效果如下：

不妨在全局定义一个将地图坐标转化为数组坐标的函数，避免代码凸长。

def x(x_loc):
    return x_loc*2+1 

def y(y_loc):
    return y_loc*2+1

继续定义possible方法，该方法可以返回当前坐标在4个方向的可动性已经是否在目标点旁边

def possible(self,vim_targets):
    psb = []
    if [self.myloc[0] -1, self.myloc[1]] ==  vim_targets and self.map[x(self.myloc[0]) -1, y(self.myloc[1])] == 1:
        return True, ['w']
    if [self.myloc[0] +1, self.myloc[1]] ==  vim_targets and self.map[x(self.myloc[0]) +1, y(self.myloc[1])] == 1:
        return True, ['s']
    if [self.myloc[0], self.myloc[1]+1 ] ==  vim_targets and self.map[x(self.myloc[0]), y(self.myloc[1]) +1] == 1:
        return True, ['d']
    if [self.myloc[0], self.myloc[1]-1 ] ==  vim_targets and self.map[x(self.myloc[0]), y(self.myloc[1]) -1] == 1:
        return True, ['a']

    if self.map[x(self.myloc[0]) -1, y(self.myloc[1])] == 1 and self.map[x(self.myloc[0]) -2, y(self.myloc[1])] == 1:
        psb.append('w')
    if self.map[x(self.myloc[0]) +1, y(self.myloc[1])] == 1 and self.map[x(self.myloc[0]) +2, y(self.myloc[1])] == 1:
        psb.append('s')
    if self.map[x(self.myloc[0]), y(self.myloc[1]) -1] == 1 and self.map[x(self.myloc[0]), y(self.myloc[1]) -2] == 1:
        psb.append('a')
    if self.map[x(self.myloc[0]), y(self.myloc[1]) +1] == 1 and self.map[x(self.myloc[0]), y(self.myloc[1]) +2] == 1:
        psb.append('d')

    return False , psb

以及移动方法

def move(self,dir,apd=True):
    if dir == 'w':
        self.map[self.myloc[0]*2+1 -2, self.myloc[1]*2+1] = 2
        self.map[self.myloc[0]*2+1 -1, self.myloc[1]*2+1] = 2
        self.myloc[0] -= 1
        if apd:
            self.history.append('w')

    if dir == 's':
        self.map[self.myloc[0]*2+1 +2, self.myloc[1]*2+1] = 2
        self.map[self.myloc[0]*2+1 +1, self.myloc[1]*2+1] = 2
        self.myloc[0] += 1
        if apd:
            self.history.append('s')

    if dir == 'a':
        self.map[self.myloc[0]*2+1, self.myloc[1]*2+1 -2] = 2
        self.map[self.myloc[0]*2+1, self.myloc[1]*2+1 -1] = 2
        self.myloc[1] -= 1
        if apd:
            self.history.append('a')

    if dir == 'd':
        self.map[self.myloc[0]*2+1, self.myloc[1]*2+1 +2] = 2
        self.map[self.myloc[0]*2+1, self.myloc[1]*2+1 +1] = 2
        self.myloc[1] += 1
        if apd:
            self.history.append('d')

建模完成后，开始最短路径代码实现：

定义函数接口

def shortest_road(begin_loc,end_loc,last_dir,targets,prt = False,len_mode = False):
    ...
    return

实例化map，初始化起点和宝藏点，并将这第一个粒子放进对象列表中

    first_sam = map()
    first_sam.set_targets(targets)
    group = [first_sam]
    group[0].my_loc(cp.deepcopy(begin_loc))

在代码中要注意使用深拷贝

开始循环并遍历对象列表

    flag = -1
    while True:
        for i in range(len(group)):

当位于开始位置时，要避免粒子穿过宝藏，必须指定一个方向让其移动，该方向就是函数接口中last_dir的反方向

            if group[i].myloc == begin_loc:
                print(i)
                psb_flag = False
                if last_dir == 'w':
                    psb = ['s']
                if last_dir == 's':
                    psb = ['w']
                if last_dir == 'a':
                    psb = ['d']
                if last_dir == 'd':
                    psb = ['a']
            else:
                psb_flag , psb = group[i].possible(end_loc)

如果粒子已经到达了目标旁边，处理完这最后一次移动后就修改flag并结束循环

            if psb_flag:
                if psb[0] == 'w':
                    group[i].history.append('w')
                    group[i].map[x(group[i].myloc[0])-1 , y(group[i].myloc[1])] = 2
                if psb[0] == 's':
                    group[i].history.append('s')
                    group[i].map[x(group[i].myloc[0])+1 , y(group[i].myloc[1])] = 2
                if psb[0] == 'a':
                    group[i].history.append('a')
                    group[i].map[x(group[i].myloc[0]) , y(group[i].myloc[1])-1] = 2
                if psb[0] == 'd':
                    group[i].history.append('d')
                    group[i].map[x(group[i].myloc[0]) , y(group[i].myloc[1])+1] = 2

                flag = i
                break

剩下的便是正常的移动处理

            for j in range(len(psb)):
                if len(psb)-j == 1:
                    group[i].move(psb[j],apd=True)
                else:
                    new_map = cp.deepcopy(group[i])
                    new_map.move(psb[j],apd=True)
                    group.append(new_map)

处理好flag判断

        if flag != -1:
            break

打印接口处理

    if prt:
        group[flag].map_print()

到这一步，循环结束后的group[flag]就是最短路径的粒子对应的类了，我们导出其history就可以得到最短路径，但该路径是上下左右的移动信息，我们需要将其转化成便于发送给下位机的格式

    road = []
    road_len = 1
    for i in range(len(group[flag].history)-1):
        if group[flag].history[i] == group[flag].history[i+1]:
            road_len += 1
        if (group[flag].history[i] == 'w' and group[flag].history[i+1] == 'a') or (
            group[flag].history[i] == 'a' and group[flag].history[i+1] == 's') or (
            group[flag].history[i] == 's' and group[flag].history[i+1] == 'd') or (
            group[flag].history[i] == 'd' and group[flag].history[i+1] == 'w'):
            road.append(road_len)
            road_len = 1
            road.append('l')
        if (group[flag].history[i+1] == 'w' and group[flag].history[i] == 'a') or (
            group[flag].history[i+1] == 'a' and group[flag].history[i] == 's') or (
            group[flag].history[i+1] == 's' and group[flag].history[i] == 'd') or (
            group[flag].history[i+1] == 'd' and group[flag].history[i] == 'w'):
            road.append(road_len)
            road_len = 1
            road.append('r')

    road.append(road_len)
    road.append('e')

另外，该函数还有road_len模式，会返回路径长度，便于全局规划，处理好该接口：

    if len_mode:
        road_len = 0
        for date in road:
            if isinstance(date,int):
                road_len += date
            else:
                road_len += 1

        return road_len - 1 , group[flag].history[-1]

函数的正常返回（标准路径格式和最后一次移动方向）

    return road , group[flag].history[-1]

于是我们可以通过该函数继续两点间的最短路径计算

len模式

由于该地图结构较为简单，没有使用哈密顿路径等高级的全局规划算法，全局规划算法仅是遍历剩下所有目标的坐标对自己坐标的距离，选取距离最短的点，在这样简单的地图下，其实大多数结果和最优解无异。

def target_sort(targets_):
    targets_new = []
    targets = cp.deepcopy(targets_)
    last_dir = 'a'
    last_loc = [9,0]
    stst_i = -1
    stst_len = 9999

    while len(targets) > 0:
        for i in range(len(targets)):
            l , dirs = shortest_road(last_loc,targets[i],last_dir,targets,prt = False,len_mode = True)
            if l < stst_len:
                stst_len = l
                ok_dir = dirs
                stst_i = i

        print(last_loc)
        targets_new.append(targets.pop(stst_i))
        last_loc = targets_new[-1]
        stst_i = -1
        stst_len = -1
        last_dir = ok_dir
        stst_len = 9999

    return targets_new

具体代码见：github.com/hachi-leaf/guangsai_national/blob/main/RDK_guangsai/Road_plan_new.py

骨牌识别

骨牌识别直接使用yolov5模型推理即可，X3的工具链上板其实并不复杂，大概分为：PC端环境部署，转化模型，板端环境部署，板端运行。

关于转化模型和板端运行方面的一些内容，大家可以参考我在地平线社区的另外一篇帖子：developer.horizon.ai/forumDetail/163807123501918330

日后我也会根据我自己的经验，再写一些关于工具链使用的教程帖

使用yolov5目标检测进行骨牌识别，一般准确率很高，但需要注意以下问题：
1.摄像头的机械安装问题，最好让目标位于摄像头正中间且角度合适
2.很多sensor会自动曝光平衡，这就导致了如果环境过亮或过暗，由于摄像头曝光平衡，目标很有可能就会在画面中显得过暗或过亮了，这时就有可能造成检测不到的结果

上位机小总结

解决完以上3个主要算法后，上位机便可以通过：识别藏宝图 —> 路径规划 —> 发送路径 —> 等待 —> 识别骨牌 —> 路径规划 ··· ··· 的进程顺序，完成赛题了。

值得一提的是，比赛中需要结合按键开关一建是启动小车。并且在与对方撞击纠缠时，根据规则也需要回到起点重新出发，这些都需要按键控制，所以需要在主程序的进程控制方面做好调试。我的主程序文件：github.com/hachi-leaf/guangsai_national/tree/main/RDK_guangsai/LEV.py

同时，主程序也不能忘记设置开机自启。

总结

以上是我使用RDK X3 Module参加第11届全国大学生光电设计竞赛的技术总结。
所有的代码已开源，可以在我的仓库查看：github.com/hachi-leaf/guangsai_national

区赛效果见：www.bilibili.com/video/BV14h4y1r7GM
国赛效果见：www.bilibili.com/video/BV19P411s72J

自从我开始学习嵌入式，很快就接触到了X3这块板子，通过X3和地平线提供的平台我学到了很多东西；并且通过这次比赛，我更是硕果累累，除了赢得国家一等奖，收获的技术经验才是最珍贵的，接下来我也会不断学习电子设计，软件工程，人工智能，计算机视觉，机器人，嵌入式等方面的知识，并且会持续结合RDK X3等开发板为基础，做出更多设计作品。