Slot Following

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

算法的核心思路是，将腿对中心点作为跟踪目标，使用轨迹跟踪算法跟踪目标，当目标距离小于设定阈值时识别下一个腿对，直到穿越完所有腿对。

步骤	动作
1. 点云预处理	将雷达点云转换为机器人坐标(右手坐标系)，忽略10mm以内的点云，然后展示在C监控器。点云被C监控器转换为世界坐标，点云位置对应D返回的机器人位姿。
2. 寻找点云中的角点	在点云中寻找角点，角点中包含了托盘腿。算法顺时针扫描点云，寻距距离雷达最近的点，然后以该点为枢纽点寻找包含它的点云团，如找到则记录为角点。此特征符合托盘腿（方管）斜对激光雷达的点云轮廓，是凸向激光雷达的点云结构。
3. 寻找托盘腿对	在角点对中搜索托盘腿对。托盘腿对是垂直于机器人前进方向的连线，其距离满足设定的托盘腿宽度及误差范围。
4. 选择最优腿对	在腿对中选择最优腿对作为跟踪目标。第一个目标的Y和角度偏差应小于给定阈值，第二个以后目标的角度偏差应小于连续跟踪角度偏差。

2.1 点云预处理

从C的车体布局读取激光雷达外参，以机器人运动中心为原点，将点云坐标转为机器人坐标系，并在C监视器显示。 C监视器将点云转为世界坐标系显示，因此点云相对机器人的位置与D相同。对机器人而言，点云使用右手坐标系，与世界坐标并无关系。

2.2 寻找点云中的角点

点云以角度与距离排序，顺时针扫描点云，以PillarFindingScope为搜索窗口尺寸，寻找距离激光雷达最近的点，然后以该点为搜索起点，寻找其是否在点云团中，其目的是寻找托盘腿特征。点云团由一定数量点构成，有一定尺寸。构成点云团的最少点数由BlobPointCount定义，其尺寸由BlobSize定义，其搜索半径（以搜索起点为圆心）由BlobSearchRadius定义。找到点云团则计算其质心以为角点，角点在C监视器以5mm半径的红色实心点表示。

参数	说明
PillarFindingScope	搜索点云团的滑动窗口尺寸，默认20mm。滑动窗口内点与搜索起点的距离小于BlobSearchRadius时，判定属于点云团。
BlobSearchRadius	点云团的搜索半径，应比点云团尺寸大。单位mm，默认100mm。该参数值应大于BlobSize。
BlobPointCount	构成点云团的最少点数。调试时应根据M的雷达视图，根据构成托盘腿的点云团规模来判断，应取稳定构成托盘腿的点数量。不同激光雷达的角分辨率不同，因此在不同距离观察到同一个腿的点数有区别，且在噪音下跳动。调试时应选择最远距离和中间距离观察，以得到稳定值。
BlobSize	点云团尺寸，单位mm。尺寸应略大于托盘腿宽度，比如托盘腿方管宽度30mm，则取40mm较好。该参数与BlobPointCount是稳定检测的关键，调试时应将激光雷达放到不同距离以观察识别稳定性，然后在运动状态下（跟踪目标前进）复核。如果参数不良，角点就会跳动，导致跟踪目标的角度偏差，进而拉偏机器人而撞上托盘。
BlobDistance	角点间的最小距离，单位mm。角点是托盘腿质心，该参数用于区分不同的点云团，比如料车腿由两根方管支撑，每根方管宽度30mm，间隔30mm。跟踪目标是内侧方管，则两根方管的点云团质心至少相距60mm，将其设为70-80mm较好。该参数与BlobPointCount和BlobSize联合使用，应根据C监视器显示的角点来判断取值是否合理。

2.3 寻找托盘腿对

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

腿对应满足机器人无碰撞穿越条件，腿对内净空区域应无障碍物。算法为此定义了倒T型区域，使用点云判断该区域是否满足条件。

                                |
                                | <------------ outerPadding
              innerIgnore       |
                |               |
                |  +------------+ <------------ deepthInner
                |  |            |
                v  |            | <------------ 停车位置
             +-----+腿        腿+-----+ 
             |                        | <------ deepthOuter
             +------------------------+ <------ deepthOuterFar

跟踪目的是让机器人停在倒T型区域中间(然后跟踪下一腿对，为方便讨论假设它停下)。机器人停止点是其运动中心，车头到该区域的12点位置，因此要定义机器人停下的车头和车尾位置，为此定义deepthinner和deepouter参数，deepthinner是车头到倒T型12点距离， deepthouter是车尾到倒T型6点距离。如果机器人进入托盘时左右需要空间，则以outerpadding来挤压两侧以产生倒T型轮廓。 innerignore用于扩大腿侧面空间，避免该区域的点被识别为障碍物。

识别出来的腿对以青色线表示，被过滤的以深橄榄绿线表示。

参数	说明
TrayWidth	托盘腿对的距离，单位mm。
TrayWidths	多个托盘的腿距不同时，以数组给出。
TrayWidthRange	托盘腿对的距离误差范围，单位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 寻找最优腿对

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

此时如果距离跟踪目标距离小于StopDistance，则判定为到达目标，结束跟踪。

识别出来的腿对以橙色连线表示；红色箭头线表示余下里程与角度。当距离目标小于LookupNextDistance时，跟踪下一组腿。

轨迹跟踪方法与Clumsy的轨迹跟踪算法相似，略。

参数	说明
Slots	要跟踪的腿对数量。
LookupNextDistance	距离跟踪目标的余下里程小于该参数时，当前目标跟踪结束，开始跟踪下一组腿。
SafeDistance	识别到腿对时，把机器人到腿对距离加上安全距离，在此区内内的腿对不作为识别目标。
finalTurnCompensate	到达终点时是否执行最终角度调整，默认为true。

2.5 运动控制

参数	说明
StopDistance	停车距离，默认 0mm。注意点云坐标被转换到机器人运动中心，因此雷达到达目标时的距离不会是0，应加上雷达到运动中心的X距离。该参数应在调试时观察获得。
SlowdownDistance	终点前缓行距离，默认 500mm。
BaseSpeed	默认行驶速度，默认 700rpm。
StoppingPow	缓行幂指数。
speedDegThres	机器人与预瞄点夹角最大值，小于时才会下行走速度。

2.6 运控视觉反馈

图 2-1 SF图示

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

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

2.7 参数清单

Documentation Preview

Field BlobPointCount

构成点云团的最少点数。

Syntax

C#
public int BlobPointCount = 6

Documentation Preview

Field BlobSize

点云团的尺寸。

Syntax

C#
public int BlobSize = 80

Documentation Preview

Field BlobSearchRadius

点云团的搜索半径，应比点云团尺寸大。单位mm。

Syntax

C#
public double BlobSearchRadius = 100

Documentation Preview

Field BlobDistance

角点之间的最小距离，用于区分不同的点云团。单位mm。

Syntax

C#
public float BlobDistance = 80

Documentation Preview

Field TrayWidth

和TrayWidths必选其一，为托盘腿间距

Syntax

C#
public float TrayWidth = 700

Documentation Preview

Field TrayWidths

和TrayWidth必选其一，为不同宽度的相邻托盘腿腿。

Syntax

C#
public float[] TrayWidths = null

Documentation Preview

Field TrayWidthRange

托盘腿间距误差范围。单位mm。

Syntax

C#
public float TrayWidthRange = 70

Documentation Preview

Field Bias

左右方向的偏移，左正右负。单位mm。

Syntax

C#
public double Bias = 0

Documentation Preview

Field LookupNextDistance

距离目标多远时开始跟踪下一个托盘。单位mm。

Syntax

C#
public float LookupNextDistance = 1000

Remarks

激光雷达应在该距离以内稳定观察到托盘腿。角点不应有显著跳动，否则干扰跟踪目标位姿，进而导致跟踪失败。

Documentation Preview

Field IgnorePillar

忽略多少毫米以内的角点。

Syntax

C#
public int IgnorePillar = 0

Documentation Preview

Field Slots

总共要跟踪多少对腿。

Syntax

C#
public int Slots = 1

Documentation Preview

Field PillarFindingScope

角点搜索窗口尺寸。

Syntax

C#
public int PillarFindingScope = 20

Documentation Preview

Field SafeDistance

搜索到目标时安全距离，避免重复识别相同目标。单位mm。

Syntax

C#
public double SafeDistance = 200

Documentation Preview

Field SignDirection

正向跟踪还是反向跟踪

Syntax

C#
public int SignDirection = 1

Documentation Preview

Field IsReverse

小车是否倒车进入。

Syntax

C#
public bool IsReverse = false

Documentation Preview

Field FilterMin

选择可行线中最短的那个？

Syntax

C#
public bool FilterMin = true

Documentation Preview

Field xTruncate

过滤X轴方向给定距离内的目标。单位mm。

Syntax

C#
public float xTruncate = 100

Documentation Preview

Field MaxFirstYDiff

首个目标的最大Y偏差。单位mm。

Syntax

C#
public double MaxFirstYDiff = 500

Documentation Preview

Field MaxFirstThetaDiff

首个目标的最大角度偏差。单位度。

Syntax

C#
public double MaxFirstThetaDiff = 30

Documentation Preview

Field MaxDistance

目标搜索范围。单位mm。

Syntax

C#
public double MaxDistance = 5000

Documentation Preview

Field BaseSpeed

基础速度，上层应用不设速度时使用该速度。单位RPM。

Syntax

C#
public float BaseSpeed = 700

Documentation Preview

Field MaxAcceleration

最大加速度。单位RPM。

Syntax

C#
public double MaxAcceleration = 30

Documentation Preview

Field MaxSteeringAcceleration

最大角加速度。单位角度。

Syntax

C#
public double MaxSteeringAcceleration = 1

Documentation Preview

Field StopDistance

到达最后目标时的停车距离。单位mm。

Syntax

C#
public float StopDistance = 50

Documentation Preview

Field SlowdownDistance

到达最后目标时的减速距离。单位mm。

Syntax

C#
public float SlowdownDistance = 500

Documentation Preview

Field MaxAngleThreshold

当小车方向角和目标朝向误差小于多少时才可以下发速度移动。单位度。

Syntax

C#
public double MaxAngleThreshold = 10

Documentation Preview

Field LookaheadDistance

预瞄点距离。单位mm。

Syntax

C#
public double LookaheadDistance = 200

Remarks

底盘运动中心到路径垂足加上预瞄点距离，得到预瞄点。

Documentation Preview

Field LookaheadStopFactor

预瞄点将近时的减速系数。

Syntax

C#
public double LookaheadStopFactor = 1

Documentation Preview

Field FinalSpeed

跟踪完成后的速度。若还需要向前走，可以指定一个不为0的速度避免停车。

Syntax

C#
public float FinalSpeed = 0

Documentation Preview

Field StoppingPow

将到终点的减速幂数。

Syntax

C#
public double StoppingPow = 0.5

Remarks

=0.5	匀减速停车。
>0.5	先快速减速，再慢速减速停车。
<0.5	先慢速减速，再快速减速停车。

Documentation Preview

Field MirrorFactor

地盘运动中心到轨迹垂足的镜像系数。

Syntax

C#
public float MirrorFactor = 0.2f

Documentation Preview

Field FinalTurnCompensate

是否进行最终角度修正。

Syntax

C#
public bool FinalTurnCompensate = false

Documentation Preview

Field FinalTurnCompensateFac

最终角度修正的P系数。

Syntax

C#
public double FinalTurnCompensateFac = 1

Documentation Preview

Field FinalTurnPrecision

最终角度纠正到达精度。

Syntax

C#
public double FinalTurnPrecision = 0.1

Documentation Preview

Field TimeoutSecond

若未看到sf，超时则结束动作。默认-1表示一直等待。

Syntax

C#
public double TimeoutSecond = -1

Documentation Preview

Field NoMemory

若为true，则上一帧检测结果对当前帧不构成约束。

Syntax

C#
public bool NoMemory = false

Documentation Preview

Field PathSeg

工位所在路径

Syntax

C#
public LineSegment PathSeg = null

Documentation Preview

Field Finished

目标跟踪结束。

Syntax

C#
public bool Finished

Documentation Preview

Field raiseAlarm

写报警信息。

Syntax

C#
public Action<string> raiseAlarm = null

Documentation Preview

Delegate CloseToTargetHandle

接近目标事件。

Syntax

C#
public delegate void CloseToTargetHandle(int targetIndex, double distance, double angle)

Parameters

targetIndex

跟踪目标索引，从0开始。

distance

目标距离。目标距离必大于LookupNextDistance。

angle

目标角度。

开发手册 - 目标跟踪

目录

Slot Following

2.1 点云预处理

2.2 寻找点云中的角点

2.3 寻找托盘腿对

2.4 寻找最优腿对

2.5 运动控制

2.6 运控视觉反馈

2.7 参数清单

导航菜单