1. 概况

在本部分我主要负责检测球场和确定机器人位置的工作.
具体可以分为:

  1. 根据标注出白线的kinect视角图(424*512)，进行坐标映射、直线拟合来确定视野中属于的各条线;
  2. 在1的基础上确定机器人的坐标及朝向.

2. 坐标系说明

  • 图片坐标系(rc): 左上角为(0, 0), r轴向右, c轴向下;
  • kinect坐标系(xyz): 即kinect坐标系, 以kinect镜头为原点, x轴向左, z轴为镜头法线方向, y轴与z轴垂直, 指向上;pict 1

  • 机器人坐标系(uv): 以左上角为原点, u轴向右, v轴向下的机器人视角平面图. 机器人为(423, 255)点;
  • 世界坐标系(XY): 在球场中的坐标系, 面向对方半场, 以左上角为原点, u轴沿底线向右, v轴沿边线向后.

3. 坐标变换方法

通过之前同学的视觉识别工作, 已经可以标记出kinect深度图中属于白线的部分, 即知道白线的图片坐标.

在此基础上调用libfreenect2中getPointXYZ()函数, 可以获知kinect深度图视野中任一点在kinect坐标系下的坐标, 即知道图片的kinect坐标.

接下来要做的工作就是将kinect坐标转换为机器人坐标, 在实验中我们是将kinect固定在机器人最上方, 用俯瞰的方式来观察球场, 因此kinect和水平面之间有一个固定夹角. 如下图:

通过图中所示, 可知, kinect坐标系下(x, y, z)映射到机器人坐标(u, v)的映射关系为:

1
2
u = 256 +　x
v = 424 - (z*cos(theta) + ｄ)

其中ｄ是为了将坐标修正到机器人中心所加常数, theta是kinect与水平面所成夹角, 该余弦值可以经过预先测定获取．

因为kinect所得视野左右颠倒, 因此在这里对u坐标取反.

不难发现, 该映射对球场完全成立的前提是球场各点高度相等, 因此当球场上存在褶皱时, 该映射对褶皱部分所得的v值会偏小, 因此褶皱部分在所得平面图中影响会被放大, 如下图, 底线上的褶皱导致所得平面图很不规整.

4. 球场检测

接下来所要做的工作就是用直线来拟合球场的边线\禁区线, 这部分也是整个部分耗时最多, 代码量最多的部分. 究其原因就是3中所得平面图中各条边线并不规整. 因此用了很多 取平均/求拟合 的方法来尝试得到较好的匹配结果.

4.1 直线拟合

往届中有使用直线拟合来完成本工作的例子, 因此最开始选取此方法来对球场进行拟合, 尝试用拟合+筛选的结果来作为球场匹配结果, 具体思路如下:

  1. 使用Canny算子完成边检测, 即理想情况下只保留各条白线的边框, 实际操作中可以在本步之前加一些模糊操作;

  1. 使用霍夫变换来检测直线, 因为1中只保留了白线边框, 因此这里可以对拟合结果进行一定的平移, 以达到拟合白线中心的目的;
  2. 对所得直线进行筛选, 可以采用判断 经过白点总数/连续经过白点数目的方式;
  3. 如果两条直线距离在一定范围内, 进行合并, 保留经过白点数量多的直线, 反复执行直到没有可以合并的直线为止;
  4. 所得即为最终拟合结果.

因为存在之前所提的边线不规整的原因, 所以本步得到的结果很糟糕, 几乎不能用来进行匹配和定位, 如下图:

4.2 固定模式匹配

在4.1基础上, 最后采用了 “先确定球场大致位置, 再用规整的球场来拟合角度” 的思路.

4.2.1 确定角球点

因为角球区特征最为明显, 而且容易确定原点位置, 因此我们尝试先利用4.1中的算法大致确定球场是否是角球区, 如果是的话, 再找到角球点位置, 再在角球点周围, 用标准的球场线来”拟合”白点, 拟合白点最多的角度就是配准后的结果.

按这个思路, 接下来的重点就是如何判断角球区以及如何准确地找到角球点位置.

首先为了排除干扰, 先反复使用erode和dialite函数来获取白线的skeleton, 再去除图中的孤立点, 结果如下图:

  • 判断角球区:

    1. 角球区一般拟合结果会有4条距离较近的直线, 因此可以用直线距离作为判断准则;
    2. 角球区另一个特点就是有两条边线, 边线的确定可以遍历所有白点, 如果大多数白点都在直线的某一侧, 就可以将该条直线看做边线.

  • 获取角球点:

    1. 之前判断出的两条变线的交点就是角球点的大致方位;
    2. 在该交点旁边寻找最近的白点来当做角球点即可, 经过检验准确度基本可以得到保证.

实际算法中, 在上述判断标准的基础上都进行了一些 遍历相邻节点/取最优 操作, 以对结果做一定修正.

4.2.2 匹配整体球场

在完成4.2.1操作后, 如果机器人视野中包括角球区, 那么已经可以确定角球点的位置, 接下来需要用已知标准的球场来拟合即可, 思路如下:

  1. 用一根直线绕角球点旋转, 取经过白点最多角度, 如果经过的白点数量大于某一阙值, 进行2;
  2. 在1基础上, 用垂直于该边线的另一条直线来拟合白点, 如果经过白点数量大于一个阙值, 进行3;
  3. 在2基础上, 用经过白点数量来确定大禁区线位置, 同时也确定了1和2中确定直线哪条是底线, 哪条是边线.

为了防止角球点位置不对导致错误匹配结果, 选取4.2.1中所得角球点周围若干点(实验中选16个)同时进行上述1/2操作, 如果1和2都成立, 则记录该角度, 到最后取平均角度作为最终边线/底线的角度. 通过检验, 可以得到较好效果, 随着机器人行走也可以较为及时地确定当前球场.

需要注意的是, 在某些特定角度, 和底线平行的大禁区线同样会被认定为底线, 因此4.2.1有可能会返回多个角球点, 这里可以按照角球点的图片坐标来区分, 因为机器人视野下的底线一定比禁区线更远, 因此选取图片坐标系中更靠上方的交点即可.

实际算法中, 在上述基础上, 加入了一些对特殊情况的判断. 参见代码.

5. 检测机器人坐标及朝向

在完成4操作后, 已经得到了匹配后的球场结果. 接下来就需要根据机器人看到的机器人坐标系比来得到机器人在世界坐标系(XY)下的坐标.

因为已经确定了球场的两条边线的直线方程, 又已知机器人位于(423, 255)点, 因此只需要判断该角球点是在左还是在右即可, 这里先得到机器人和底线中点所连直线的方程, 再将角球点坐标代入, 判断正负即可知道左右.

在获知左右后, 即可知道机器人的世界坐标(X, Y), 判断底线和水平线夹角即可知道机器人的朝向.

具体参见代码相应部分.

6. 检测足球世界坐标

在之前同学的视觉检测部分, 可以获取到球的机器人坐标, 在已知机器人的世界坐标的前提下, 可以求出足球的世界坐标, 需要注意的是在不同朝向范围内计算公式有一定区别, 虽然只是高中数学但还是需要认真推导, 实验中为了这个调了挺长时间Orz

具体参见代码相应部分.

7. 配置

在qt下导入运行即可.
加入配置如下:

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
CONFIG += c++11


DEFINES += TEST_LIBRARY

SOURCES += test.cpp \
    matchpos.cpp

HEADERS += test.h\
        test_global.h \
    matchpos.h

INCLUDEPATH += /usr/local/include \
                /usr/local/include/opencv \
                /usr/local/include/opencv2

LIBS += /usr/local/lib/libopencv_highgui.so \
        /usr/local/lib/libopencv_core.so    \
        /usr/local/lib/libopencv_imgproc.so \
        /usr/local/lib/libopencv_photo.so

8. 总结

本部分实现原理比较简单, 主要工作都在调试改动上, 因为平面图像的误差导致用了很多奇怪的方法来匹配球场, 拟合坐标.

总结来看, 在刚开始还是对整体识别思路理得不够清楚, 有些地方可以用更好更简洁的方法来处理; 此外在各种角度计算统一上也出了很多bug, 也属于一开始没有统一好的锅..

最后郑重感谢老师和助教在小学期里的辛勤指导, 在很多地方给予了我们很关键的帮助.