查看“开发手册 - 目标跟踪”的源代码


= Slot Following =
Slot Following(SF)是穿越多个有腿托盘的自动驾驶算法。

设由多个有腿托盘(一般是料车)顺序摆放的场景，机器人使用导航雷达识别推盘腿对，计算 中心点作为跟踪目标，逐一穿过托盘，在穿越给定数量腿对后停下。

算法的核心思路是，将腿对中心点作为跟踪目标，使用轨迹跟踪算法跟踪目标，当余下里程 小于设定阈值时识别下一个腿对，直到穿越完所有腿对。
{| class="wikitable"
!步骤
!动作
|-
|'''1. 点云预处理'''
|将雷达点云转换为机器人坐标，并忽略10mm以内的点云。
|-
|'''2. 寻找点云中的角点'''
|在点云中寻找角点，角点中包含了托盘腿。
|-
|'''3. 寻找托盘腿对'''
|在角点对中搜索托盘腿对。
|-
|'''4. 选择最优腿对'''
|在腿对中选择最优腿对作为跟踪目标。
|}

== 2.1 点云预处理 ==
从车体布局器读取导航雷达的最近一帧数据，将点云变换为机器人坐标，并忽略10mm以内的点。点以白色点表示。

== 2.2 寻找点云中的角点 ==
以雷达扫描角度为扫描线，使用滑动窗口遍历点云，寻找窗口内满足距离阈值的点，并找到距离雷达最近的 点(凸向机器人的最近点)。如果找到符合条件的最近点，使用托盘检测算法搜索它与周围点是否构成团块(blob)， 如找到团块，则计算团块质心作为角点。角点以5mm半径的红色实心点表示。

寻找角点所用的参数：
{| class="wikitable"
!PillarFindingScope
!滑动窗口的尺寸，默认20mm。滑动窗口内所有点与遍历点云当前点的距离小于wndDist时，判定属于点云团。
|-
|'''wndDist'''
|滑动窗口中点云团判定尺度。默认100mm。
|-
|'''LidarDetectTray.BlobPtCount'''
|点云团必须包含点数的下限。调试时根据M的雷达视图选择合适参数，过大会识别不出小尺寸托盘腿。
|-
|'''LidarDetectTray.blobSz'''
|点云团尺寸。标定方法同上。
|}

== 2.3 寻找托盘腿对 ==
角点两两配对，如果两个角点的Y坐标不相同，则判定为一对。然后根据给定的托盘宽度判断角点对是否腿对，不满足 条件的角点对以灰线标记。大于搜索范围的腿对被丢弃，余下的检查是否满足托盘腿对的条件，不满足丢弃并以深橄榄绿线 标记。

腿对应满足机器人无碰撞穿越条件，腿对内净空区域应无障碍物。算法为此定义了倒T型区域，使用点云判断该区域是否满足条件。
                                 |
                                 | <------------ outerPadding
               innerIgnore       |
                 |               |
                 |  +------------+ <------------ deepthInner
                 |  |            |
                 v  |            | <------------ 停车位置
              +-----+腿        腿+-----+ 
              |                        | <------ deepthOuter
              +------------------------+ <------ deepthOuterFar
跟踪目的是让机器人停在倒T型区域中间(然后跟踪下一腿对，为方便讨论假设它停下)。机器人停止点是其运动中心，车头到该区域 的12点位置，因此要定义机器人停下的车头和车尾位置，为此定义deepthinner和deepouter参数，deepthinner是车头到倒T型12点距离， deepthouter是车尾到倒T型6点距离。如果机器人进入托盘时左右需要空间，则以outerpadding来挤压两侧以产生倒T型轮廓。 innerignore用于扩大腿侧面空间，避免该区域的点被识别为障碍物。

识别出来的腿对以青色线表示，被过滤的以深橄榄绿线表示。
{| class="wikitable"
!参数
!说明
|-
|'''width'''
|托盘腿对的距离，单位mm。
|-
|'''widths'''
|多个托盘的腿距不同时，以数组给出。
|-
|'''width_error'''
|托盘腿对的距离误差范围，单位mm。
|-
|'''depthInner'''
|停车时车头进入托盘深度，以激光雷达为原点，单位mm，默认值50。
|-
|'''depthOuter'''
|停车时车尾距离跟踪目标深度，以激光雷达为原点，单位mm，默认值100。
|-
|'''depthOuterFar'''
|倒T型区域的6点位置，与depthInner构成净空区域的长度，单位mm，默认值400。
|-
|'''outerPadding'''
|挤压(矩形)净空区域形成倒T型轮廓，与depthOuter配套，单位mm，默认值50。
|-
|'''innerIgnore'''
|托盘腿的尺寸，单位mm，默认值150。由于角点是没有体积的点，因此用该参数定义腿尺寸，以免误判障碍物点。
|-
|'''allowedBadpoints'''
|净空区域内的最大点数量，用于过滤噪音点。默认值5。
|}

== 2.4 寻找最优腿对 ==
识别出来的腿对可能有多个。第一组腿必须满足进入的角度和宽度条件，以maxRangeTh和maxRangeY定义。 第二组以后则必须满足连续穿越的角度偏差条件，以seqThDiffTolerance定义。

如果找不到腿对则停车。此时如果距离跟踪目标距离小于100mm，则判定为到达目标，结束跟踪。

识别出来的腿对取第一组，以橙色连线表示；红色箭头线表示余下里程与角度。当余下里程小于nextSlotDist时， 判定当前目标跟踪结束，开始跟踪下一组腿。

轨迹跟踪方法与Clumsy的轨迹跟踪算法相似，略。
{| class="wikitable"
!参数
!说明
|-
|'''slots'''
|要跟踪的腿对数量。
|-
|'''nextSlotDist'''
|距离跟踪目标的余下里程小于该参数时，当前目标跟踪结束，开始跟踪下一组腿。
|-
|'''safeDist'''
|识别到腿对时，把机器人到腿对距离加上安全距离，在此区内内的腿对不作为识别目标。
|-
|'''finalTurnCompensate'''
|到达终点时是否执行最终角度调整，默认为true。
|}

== 2.5 运动控制 ==
{| class="wikitable"
!参数
!说明
|-
|target，
|停车距离，默认 0mm。
|-
|slowDownDist，
|终点前缓行距离，默认 500mm。
|-
|baseSpeed，
|行驶速度，默认 700rpm。
|-
|stoppingPow，
|缓行幂函数指数。
|-
|speedDegThres
|机器人与预瞄点夹角最大值，小于时才会下行走速度。
|}

== 2.6 运控视觉反馈 ==
[[文件:SF运控视觉反馈.png|无框|515x515像素]]

'''图 2-1 SF图示'''

启用Slow Folling算法后，Clumsy显示算法计算过程。

# 角点。具有“凸包”特征的角点，红点即其中心。
# 腿对连线。成组角点（腿对）的连线。
# 选中腿对中心点。是机器人行走路径的终点。
# SLAM 测算角度。根据激光雷达测算的机器人位姿，SF 不会使用 SLAM 定位结果，仅用于监控参考。
# 轨迹跟踪角度。箭头指向预瞄点。
# 规划轨迹角度。当前路径的角度。