Home
机器人的感知算法架构包括雷达检测、前视相机检测和后视相机检测。感知算法流程为：雷达通过点云和地图匹配检测障碍物，前视相机和后视相机分别经过基于图像的检测方法得到障碍物位置信息。三者的检测结果经过融合后发送给控制节点。
激光雷达检测模块是利用激光雷达数据和地图数据，检测出地图上没有标出的障碍物。检测算法的核心思想是利用ICP方法将当前采集到的激光雷达数据和地图数据做匹配，对于没有匹配到地图上的点，采用聚类的方法计算点簇，作为检测到的物体，从而计算物体的空间坐标。
激光雷达检测算法的输入为激光雷达数据、定位数据、传感器配置数据；算法的输出为动态障碍物点云、当前匹配到地图的点云和检测到的运动物体。
雷达检测速度为0.001s，而雷达的采集频率为12Hz。检测距离精度误差是6.8%。
雷达检测的效果如下，图中本车的角度和位置，白色的点是雷达检测到的Robot。本车跟随检测到的Robot移动。
如图所示，图像检测模块包括前视相机和后视相机2个传感器，采集的图像输入到Yolov3-tiny网络，输出检测结果，采用tensorRT加速，检测速度达到100Hz以上。对于前视相机，采用深度学习检测机器人和装甲板，结合传统方法提取灯条具体位置解算机器人相对于相机坐标系下的距离，发送给云台控制设计模块。后视相机主要目的是为了弥补前视相机和激光雷达的视野盲区，通过检测算法，检测结果反馈给控制节点。
算法采用Yolov3-tiny网络对敌方装甲板位置进行检测，画出矩形框，然后再将矩形框区域作为ROI区域进行裁剪，再将裁剪完成后的图片输入到一个图片分类网络，用于识别装甲板上的数字。另外，由于Yolov3-tiny网络识别目标之后只能确定矩形区域，不同角度时无法完全贴合装甲板形状，因此再将裁剪完成后的图片利用传统方法对灯条进行检测，以灯条的外轮廓为准，画出四边形框，然后根据相机内外参数计算出目标物体与相机之间的距离，用作射击的参数。
其中Yolo检测速度达到100Hz以上，检测平均准确率mAP达到96%，整体检测速度8ms。距离解算的误差随着距离本车相机的远近有差异，如图所示，在距离本车2m的时候，误差0.3m以内，距离本车越远，误差越大。
装甲板检测效果如下:
提取的YOLO检测框经过卷积分类网络，分成5个类别：红1，红2，蓝1，蓝2和死车。
对于不同位置，激光雷达扫描到静态障碍物的可能性也是不同的，从而引起激光雷达的数据分布的不同。因此，当收集到一帧激光雷达数据时，对于地图上的不同位置，收集到的数据符合当前激光雷达数据分布的可能性也是不同的，而数据分布最吻合的位置，是当前最有可能出现的位置。因此，基于激光雷达的定位算法核心在于根据当前激光雷达数据，计算不同位置出现这种数据的可能性。
当前激光雷达测量模型实现中采用的是概率机器人一书中的似然域模型。 该部分的算法实现在官方公布的框架中基本实现。除此之外，官方在实现时与标准的粒子滤波（蒙特卡洛定位）有部分不同，官方采用的是Augmentd_MCL算法，该算法加入了似然的长期因子和短期因子
，用来缓解定位算法在某些情况下定位失败的情况，具体参考概率机器人P206-207。
粒子滤波的算法流程如下：
Algorithm MCL(,
,
):
for to
:
end for
for to
:
draw from
with probability
end for
return
在常用的机器人系统中，二维码经常被用于视觉辅助定位，密歇根大学开发了一套非常稳定的二维码检测系统AprilTag，被各种机器人广泛使用。该系统可以根据检测相机图像中的二维码以及相机的内参，计算相机和二维码的相对姿态。在当前的机器人系统中，也采用了AprilTag检测库，进行检测二维码在相机中的位姿。由于二维码在场地中的位置是固定且已知的，因此可以推算相机在场地中的位置。从而得到机器人的定位。
算法流程如图所示：
算法输入为相机参数和相机图片，算法输出为数组形式的二维码检测结果、图片形式的二维码检测、geometry_msgs形式的检测结果。
检测结果如图所示：
融合过程如下：
如图所示，前视相机图像检测目标和激光雷达检测目标的距离融合得到前方270度范围内的Robot。雷达检测的点投影到相机坐标系，若机器人区域有雷达点，则得到机器人的位置；后视相机得到后方100度内机器人。两者对重叠区域融合，传输给控制节点。重叠区域存在2个机器人位置小于1m内，合并其结果，采用用雷达距离。
哨兵检测系统包括如下几部分：每个哨兵采集图像经过检测算法，输出装甲板和机器人检测框。输出的装甲板或者机器人经过分类得到机器人颜色和ID号，经过距离解算和距离预测得到机器人在世界坐标下的位置，最后经过位姿估计模型，得到机器人的位姿信息。获得整个哨兵感知得到的机器人的ID，颜色，位姿，位置，信息。2个哨兵和2个机器人的信息经过信息融合模块，输出4个机器人的信息。
基于yolov3-boosting算法的检测流程：
优化后的yolov3-boosting是在原始yolov3模型的基础上进行残差块和卷积通道剪枝得到的，剪枝后的模型在保证原有精度的情况下，具有更少的计算量和更快的前向推理速度。在yolov3模型中，anchor框的预测机制是：与真值IOU最大的单个anchor框参与预测；而此在剪枝过程中借鉴了yolov4中anchor的处理方式：大于一定IOU阈值的anchor框都参与预测。这样做可以保证剪枝前稀疏训练的精度，进而保证剪枝过程中对应剪枝部分判断的准确性。得到剪枝模型后，该模型可以输出每帧图片的机器人和装甲板检测框坐标，并进行配对。
如图所示，Yolov3模型的剪枝位置分为残差模块剪枝和卷积通道剪枝。
剪枝的流程是先进行残差模块剪枝，再进行迭代式的卷积通道剪枝。两个位置的剪枝步骤均是针对性的稀疏训练→剪枝→微调。
残差模块剪枝的具体流程如下：
首先在残差模块前手动加入参数：
式中， 为第
个残差模块的输出，
为第
个残差模块的输入，
为残差模块的计算函数，
为加在该残差模块之前的权重参数，如下图所示。
通过对实现L1正则化对其进行稀疏训练，添加
参数正则项之后的loss函数为：
式中，为加入
参数后的loss函数，
为
参数的L1正则项。
稀疏训练后的排序代表残差模块的重要性排序，根据此顺序确定λ剪枝阈值，剪掉小于阈值的
对应的残差模块。
卷积通道剪枝的具体流程如下：
在通道剪枝中，用每层卷积层后的BN层的系数作为稀疏训练的对象，BN层的计算公式为：
,
对实现L1正则化，对其进行稀疏训练，添加
正则项之后的loss函数为：
式中，为原始loss函数，
为
的L1正则项。
稀疏训练后的值的排序代表其对应通道的重要性排序，根据此排序确定阈值，剪掉小于阈值的
对应的卷积通道。此过程可以迭代进行，直到剪到合适的计算量和精度。
表中的剪枝yolov3-boosting-1模型是进行1次残差模块剪枝1次卷积通道剪枝的结果；剪枝yolov3-boosting-2模型是进行了1次残差模块剪枝2次卷积通道剪枝的结果;剪枝yolov3-boosting-3模型是进行了1次残差模块剪枝3次卷积通道剪枝的结果。
图中给出了车体颜色标号6分类模型的流程框架。车体颜色标号分类模型实现了对红1车、红2车、蓝1车、蓝2车、灰1车以及灰2车的有效划分。车体颜色标号分类模型根据YOLO检测模型，判断是否可以检测出装甲板，如果检测到装甲板，则以装甲板目标框信息作为输入，进入卷积分类网络1，实现车体颜色标号的分类预测；如果YOLO检测模型无法检测到装甲板，则以车体目标框信息作为输入，进入卷积分类网络2，实现车体颜色标号的分类预测。
图中给出了以装甲板目标框信息作为输入的卷积分类网络1，其为6层网络结构，由2层卷积和4层全连接层构成。其中，两个卷积层的卷积核，大小为33，步长为1。激活函数使用ReLu，池化层采用最大池化，大小为33，步长为3。
与上图网络模型相类似，下面给出了以车体目标框信息作为输入的卷积分类网络2，该网络中卷积层的卷积核，大小为5*5。
图中分别给出了以车体目标框信息和装甲板目标信息作为输入的分类预测准确率结果。通过图示信息可以得出，卷积分类网络1和卷积分类网络2较好地实现了对车体颜色标号6分类的有效划分。
灯条识别采用传统图像处理方法，先通过YOLO算法得到装甲板的大致范围，如果YOLO无法识别出装甲板采用车体框进行灯条检测。通过颜色空间变换，膨胀腐蚀得到灯条轮廓。判断轮廓形状，大小，是否平行，相对距离，得到一组最符合条件的灯条对。最后通过外接矩形拟合灯条框并通过SolvePnP得到灯条中心点世界坐标。
首先通过SolvePnP以及图像中二维码的四点世界坐标以及像素对应坐标解算出相机坐标系和世界坐标系的相对外参，$Rcam$，$Tcam$。有对应关系：$Pc=Rcam*Po+Tcam$。其中Pc为相机坐标系的点值，Po为世界坐标系的点值。
通过灯条检测算法识别出的灯条对四角点像素坐标，以及以灯条对中心点为原点的世界坐标，用SolvePnP算法可以得到灯条对中心点相对相机的外参，根据公式：$Pc=R*Po+T，Po=0$，灯条对中心点的相机坐标系坐标为$Pc=T$。
最后将灯条对中心点的相机坐标系坐标代入Pc=RcamPo+Tcam，可得灯条对中心点的世界坐标为Po=R’(T-Tcam)。
从 Yolo 检测到的 Robot box 中提取 (x, y, w, h), 其中 (x, y) 为 box 下界中心点位置，(w, h)为 box 的侧向装甲板由于遮挡等原因，某侧灯条只能检测到一部分，造成装甲板检测框变形，从而引起距离在侧对装甲板时估计不准。我们可以利用神经网络来拟合相机坐标系到世界坐标系的转换矩阵，如图所示，从 Yolo 检测到的 Robot box 中提取 (x, y, w, h), 其中 (x, y)选择为 box 下界中心点位置，是因为选择正中心会受到车体远近box大小问题影响，模型为一个三层的 MLP 结构，将图像坐标映射为实际坐标，这种方法依赖于yolo检测bounding box的精度。
MLP方法整体拟合的很好，方差很小，但是神经网络的输出不如解算方法稳定，会在某些点产生奇异值（超出边界）。
解算方法的预测值都分布在真值附近，不会出现突变，但是在距离远的时候误差会随之增大；而MLP方法分布比较集中，大部分情况下预测准确，但奇异值的存在使得它的整体误差要比解算方法大，因此针对两种方法的优劣性进行融合。我们选择在未产生奇异值时相信MLP方法，在产生奇异值时相信解算方法。
从图中可以看出，两种方法融合后的结果去除了奇异值的影响，融合误差均小于两种方法，最后计算二维平面的L2误差为0.30。
上图给出了车体位姿估计分类模型的流程框架。该网络模型以车体目标框信息以及世界坐标系下的（x,y）坐标作为输入，实现了车体角度分类预测。该模型将角度范围（0度～180度）平均划分为8份，作为8个类别。以左侧哨兵为例，将在左侧哨兵视角下的车体目标框信息及其世界坐标系下的（x,y）坐标输入到卷积角度分类网络1中，实现角度分类。同理，将右侧哨兵视角下的车体目标框信息以及世界坐标系下的（x,y）坐标输入到卷积角度分类网络2中，实现角度分类。
图中给出了位姿估计算法的原理结构图。为了减小同一角度下，因距离不同造成哨兵分类错误的情况，该方法将车体在世界坐标系下的角度转换为在相机坐标系下的角度。分类网络将分类对象转换为哨兵-车体中心的连线与车体方向的夹角，尽可能保证同一角度不同位置的车体，在哨兵视角下角度相似，提高了卷积网络的分类成功率。
图中给出了左、右哨兵视角的位姿估计网络模型结构图，该模型由6层结构构成，其中包含两个卷积层以及四个全连接层。其中，两个卷积层的卷积核，大小为33，步长为1。激活函数使用ReLu，池化层采用最大池化，大小为33，步长为3。
图中给出了左、右哨兵视角的位姿估计准确率。通过图示信息可以得出，通过将世界坐标系下的角度转换为相机坐标系下的角度，位姿估计模型有效地实现了角度的8分类。
定位模块有多个输入来源，包括两个己方机器人的雷达传感器和两个哨兵的相机传感器，我们通过卡尔曼滤波来对传感器数据进行融合，因为不同传感器的过程噪声矩阵和测量噪声矩阵不同，我们会根据传感器的来源设置相应的矩阵，来更新同一个状态空间。在工程环境中，不同传感器的检测时间基本是不对齐的，而预测的目标是同一个，我们可以通过这种方法增加检测的频率。
哨兵以鸟瞰视角观察敌我双方的机器人，且为固定状态，误差相对较小，我们以x方向和y方向误差均为0.2的噪声来构造符合高斯分布的哨兵测量数据，以10%的概率丢失目标。而机器人感知数据相比哨兵增加了更多的不确定性，因为它本身在运动，且视线受障碍物遮挡，不能纵观全局，因此把误差范围设为0.5，且以40%的概率丢失目标。
从图中可以看出不同传感器的测量数据不规则的分布在真值周围，通过卡尔曼滤波融合定位系统可以寻找到一条轨迹，与真值有较高的吻合度，滤波后的结果较为平滑且无明显的滞后。
哨兵检测效果如下：检测框上方第一行显示颜色标号，角度（Pose）第二行显示：位置（x，y)。
