robogame技术文档

最新推荐文章于 2024-05-21 14:46:50 发布

00ffcc

最新推荐文章于 2024-05-21 14:46:50 发布

阅读量66

点赞数

文章标签： opencv

本文链接：https://blog.csdn.net/guizhiyu/article/details/134550412

版权

视觉和控制

author: guizhiyu@mail.ustc.edu.cn

视觉定位

综述

总的来说，如果你抛弃了巡线，而是完全依靠视觉完成车辆的定位，你需要做到的事是在场边架个相机，用电脑识别并计算车辆的位置，实现闭环控制。场边视觉平台和PC上位机是被允许的，如果你硬要做SLAM当然也好。

请注意，视觉定位的精度和速度完全决定了你能否完成比赛。请在一审前证明你的视觉方案能满足要求，而不是用开环跑到三审才发现完全行不通。

定位就是建立一个从图像平面内每一个二维点到场地上每一个三维点的一一映射。当然，既然组委会没有要求车飞起来，其实只是从二维点到二维点的映射（场地表面即可，即使场地有高低也是一样的）。如果真的需要三维定位，请使用双目视觉。

反过来，从三维点到图像平面内二维点的映射被称作投影。利用齐次坐标，投影变换可以用 $q = MTRQ$ 表示，其中 $q$ 为图像平面内的坐标， $M$ 为相机的内部参数， $T$ 为相机坐标系在世界坐标系中的平移矢量， $R$ 为相机坐标系在世界坐标系中的旋转矢量， $Q$ 为三维点在世界坐标系内的齐次坐标(4*1的)。

在最后我会通过投影实现定位。

代码已开源 https://gitee.com/guizhiyu/cv

选购相机

请在淘宝搜索工业相机，选择可手动调焦的型号，有以下值得注意的参数：

视场角(fov)，fov决定了你的相机要放在场边多远的地方。
景深，景深越大的相机可以在越大的范围能保持成像清晰。
帧率，帧率限制了你输出的频率，60帧以上是理想的帧率。你可以要求客服加钱换个更好的主控以提高帧率。
延迟，100ms以下的延迟属于正常情况。相机的延迟是控制延迟的主要部分，对控制精度和速度影响非常大。
分辨率，分辨率决定了定位误差的下限。请综合考虑处理平台的算力、相机价格再选购。1920*1080(200万像素)是我们本次使用的相机。
畸变参数，一个畸变参数很小甚至无畸变的相机能给你省很多事

你需要一个三脚架来架设相机。直观来看，三脚架越高，相机光轴与场地平面法线的夹角越小，定位精度越高。但是过高的架子在顶部的振动难以停止，请咨询机械。

读取相机

一般使用opencv-python读取和处理相机的输出。（当然如果你要用cpp也没区别，速度也不会变快）请自行了解python，opencv，numpy，multiprocessing， thread的相关语法和基本操作，这里只给出基本和进阶的实现

基本实现:

cap = cv2.VideoCapture(0)# 这个id取决于你把相机插在电脑上的哪个usb口
cap.set(cv2.CAP_PROP_FRAME_HEIGHT,1080)# 分辨率
cap.set(cv2.CAP_PROP_FRAME_WIDTH,1920)
cap.set(cv2.CAP_PROP_FPS, 60)# 帧率，这三行参数必须设置，否则会按720p 30fps读取
while cap.isOpened():
    frame=cap.read()# 读取
    # do something

这个实现已经差不多很行了，主要问题在于cap.read()是阻塞式的，对于一个60帧的相机它需要花费33ms，如果你将后续的处理优化到30ms左右，输出位置的频率也只能达到30fps。优化点在于使用多线程技术，将读取和计算同步执行。

进阶实现:

class CameraThread(Thread):# 实现来自网络
    def __init__(self, kill_event, src = 0, width = 1920, height = 1080):
        self.kill_event = kill_event
        
        self.stream = cv2.VideoCapture(src)
        self.stream.set(cv2.CAP_PROP_FRAME_HEIGHT, height)
        self.stream.set(cv2.CAP_PROP_FRAME_WIDTH, width)
        self.stream.set(cv2.CAP_PROP_FPS, 60)
        (self.grabbed, self.frame) = self.stream.read()
        self.read_lock = Lock()

        Thread.__init__(self, args = kill_event)

    def update(self):
        (grabbed, frame) = self.stream.read()
        self.read_lock.acquire()
        self.grabbed, self.frame = grabbed, frame
        self.read_lock.release()

    def read(self):
        self.read_lock.acquire()
        frame = self.frame.copy()
        self.read_lock.release()
        return frame

    def run(self):
        while not self.kill_event.is_set():
            self.update()
kill_event = Event()
cam = CameraThread(kill_event)
cam.start()
while True:
    frame=cam.read()
    # do something

标定相机

你需要通过相机标定获取相机的内参矩阵 $M$ 和畸变参数。注意，内参矩阵和相机的焦距（从小孔成像模型来看是焦距，其实是镜头到传感器的距离，应该算像距？）有关，所以测完之后就不要拧镜头了。（不过就算拧了定位结果也不会变，随便吧）

请自行百度opencv相机标定。注意，组委会有价值300rmb的棋盘格标定板，请充分利用。

标定场地

标定场地是为了获得 $R$ 和 $T$ ，即相机相对于场地坐标系的位置。注意，你需要在1分钟准备时间内完成场地标定。你应当知道一些点在场地坐标系里的位置，又知道这些点在图像坐标系里的位置，就可以通过opencv的solvePNP函数计算 $R$ 和 $T$ ，具体用法请参考官网。

问题在于如何又快又准地找到这些点在图像坐标系里的位置。比较平凡的方法是在图形化界面上手动点，这样精度很低，速度也很慢，所以能使用的点也很少，又降低了 $RT$ 计算的精度。

合理的做法是使用图像配准。我使用了superglue，你也可以试试LoFTR。具体的做法是我挑选了一些（30+）特征足够明显的点，保证它们可以被superpoint识别为特征点，然后拍摄一幅场地的图像 $A$ ，手工标出 $A$ 中每个选中的特征点的坐标。在使用图像 $B$ 场地标定时，superglue可以使用深度学习计算出 $A$ 中每个特征点投影到 $B$ 中时的位置。所以你的图像处理平台既要有很好的cpu也要有很好的gpu。

当然，标出每个选中的特征点的坐标也很累。我写了一个程序方便你标位置，不过里面是用opencv的角点检测实现的，你最好换成superglue输出的特征点。

aruco识别

暂时抛开定位的问题，我们先来解决一个更急迫的问题：如何从图像中识别出车辆？

你当然有一些炫酷的方法，比如OnePose by ZJU（这个方法把定位也一并解决了，很强大）。不过我们还是考虑一些传统而稳定的方法，即aruco。你可以打印一个巨大的aruco图标（用雪弗板打，周围围一圈白的，如果你有不会反光的材料就更好了）盖在车上，然后使用cv2.aruco.detectMarkers检测aruco图标的四角。

这套方法在大部分情况下都很好用，但是场地光照情况的变化可能会让你前功尽弃，比如过强的光照导致反光增强而识别失败。所以你需要调整参数并对图像进行预处理。

一个相当可靠的预处理是将图像按亮度的阈值二值化，这样可以去除雪弗板上反光的影响。当然，这个阈值应当根据场地的光照调整。

cv2.aruco.DetectorParameters_create()自动生成的参数也需要调整。

arucoDict = cv2.aruco.Dictionary_get(cv2.aruco.DICT_4X4_50)
arucoParams = cv2.aruco.DetectorParameters_create()
arucoParams.adaptiveThreshConstant=7
arucoParams.adaptiveThreshWinSizeStep=5
# arucoParams.adaptiveThreshWinSizeMax=40
# arucoParams.aprilTagMaxNmaxima=40
# arucoParams.aprilTagCriticalRad=0
# arucoParams.aprilTagMinClusterPixels=0
arucoParams.maxErroneousBitsInBorderRate=0.4
# arucoParams.errorCorrectionRate=0.8
arucoParams.perspectiveRemoveIgnoredMarginPerCell=0.3
arucoParams.polygonalApproxAccuracyRate=0.01
arucoParams.maxMarkerPerimeterRate =4
arucoParams.minCornerDistanceRate=0
arucoParams.minDistanceToBorder=0
arucoParams.minMarkerDistanceRate=0.005 #重要
arucoParams.aprilTagMaxLineFitMse=0.00

注意：如果你将aruco图标放在雪弗板的正中心，请将minMarkerDistanceRate 调整至0.005或更小，并在输出的四边形中取最大的一个，详见github。这是opencv的一个bug，即它在判断每个四边形框是否是合法的aruco前，会将中心过于靠近的四边形过滤掉。

定位算法

铺垫完了，现在唯一的问题就是如何快速且准确的定位。

对于投影，我们有cv2.projectPoints()，所以我们其实只需要对于场地上的每个点，计算其在图像上的投影，定位时通过查表，查到投影对应的三维点即可。结束了。

当然对于如何在一分钟准备时间内建完这张表还需要一些优化，比如多进程、使用cython、使用gpu加速。

识别矿石颜色

既然矿物的位置是已知的，你又知道相机的内参和外参，通过投影你当然知道矿石在图像上的坐标及其颜色。

控制

整体架构

主体gui架构在这里，与控制相关的代码在这里。

主题部分我使用了pyqt进行人机交互，包括显示相机实时图像，启动车辆等。

点击启动后会启动控制车辆运动的子进程，车辆的位置数据通过multiprocessing的Queue传给子进程。这个Queue的最大大小只有1，所以可以保证收到的是最新的数据。multiprocessing的Array和Value可能会出一些神必问题所以这样写。

旋转

把旋转的角速度传给车辆即可，轮子怎么动是电控的问题。考虑到相机+计算+无线通信的延迟可以达到200ms，应当让角速度在快转到时减小已提高精度。出于方便我直接写了分段函数，理论上应该做个PID。

平移

每个时刻都令轮子朝向目标点即可，只需要把轮子角度和速度传给车辆。同理，应当在快到达时减速。

速度解算

由于安装精度的误差，平移的时候车体会发生相当大的自转，每次停下来纠正相当花时间。使用速度解算可以令车辆边平移边自转，既可以保持车辆平移时角度不变，又可以节省停车转向的时间。确定好平移速度方向，自转角速度后把所需参数传给车辆即可。同理，你应当写个PID之类的，不过分段函数也够用.

00ffcc

关注

0
点赞
踩
0

收藏

觉得还不错? 一键收藏
0
评论
robogame技术文档

（当然如果你要用cpp也没区别，速度也不会变快）请自行了解python，opencv，numpy，multiprocessing， thread的相关语法和基本操作，这里只给出基本和进阶的实现。总的来说，如果你抛弃了巡线，而是完全依靠视觉完成车辆的定位，你需要做到的事是在场边架个相机，用电脑识别并计算车辆的位置，实现闭环控制。直观来看，三脚架越高，相机光轴与场地平面法线的夹角越小，定位精度越高。，所以我们其实只需要对于场地上的每个点，计算其在图像上的投影，定位时通过查表，查到投影对应的三维点即可。
复制链接

扫一扫