开发手册 - 目标跟踪

来自MDCS wiki
跳到导航 跳到搜索

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 最佳实践

以连续穿行4台料车为例讲解调试方法。

图 2-1 由4台料车构成的“隧道”

潜伏式牵引车连续穿行4台料车，在最后一台料车下面停车，车头距离料车后腿约0.5米。


图 2-2 激光雷达在牵引车头

牵引车使用头部激光雷达识别料车腿对，觅中点连续穿行直至终点。


把牵引车遥控到距离首个托盘约1.5米位置，停车，使用M的前雷达视图查看托盘腿点云。

图 2-3 M的前雷达视图

使用“测量”功能，可得雷达距离跟踪目标的距离约1米（矩形框）。两个圆框是要识别的托盘腿，每个托盘腿由两根方管构成，尺寸是30mm，两根方管间距30mm。跟踪目标是内侧一对方管，间距600mm，制造误差5mm。


图 2-4 测量内侧方管尺寸

测量内侧方管尺寸为32mm，与卷尺测量值相似，考虑到激光雷达测量误差，将BlobSize设为40。构成内侧腿的点数量为6，将BlobPointCount设为6。测量内外方管点云中心距离为72mm，将BlobDistance设为80。这些参数作为角点调试的起始参数。


图 2-5 C监视器的SF视图

在C启动SF算法，C监视器显示SF状态。放大视图，能够观察到两个内侧方管被锁定为角点（红色实心圆点），角点中心位于方管内，橙色连线表示跟踪目标已锁定，其中点（跟踪目标）的红色箭头线指向机器人运动中心，运动中心到红色箭头线的黄线是预瞄点连线，夹角表示机器人向右跟踪预瞄点。跟踪目标旁的两个数字分别是距离和托盘腿宽度，前者是机器人运动中心到跟踪目标距离，加上了激光雷达到运动中心距离，调试时应注意，即目标距离有最小值，不会到0，本例中的最小距离是330mm；后者是调TrayWidth、TrayWidthRange的依据，由于激光雷达在运动中测量，角点会跳动，因此托盘腿宽度是一个范围。

满足托盘腿宽度条件的角点对以青色和粉色线表示，被选中托盘腿以橙色线表示。上图中可见机器人右侧2条被过滤的角点对。

遥控机器人前进，观察远近距离时跟踪目标是否稳定，即内侧腿角点不会大幅度跳动，使跟踪目标与机器人夹角增大，最终跑偏。本例中内侧腿角点不应跳动到内外方管中间，甚至外侧方管，黄色预瞄点连线夹角不应大幅度跳动。


图 2-6 跟踪下个目标

机器人距离目标距离小于LookupNextDistance时跟踪下个目标，如上图。在机器人在运动中顺序跟踪目标时，应注意角点是否锁定内侧方管，预瞄点连线与目标垂线应基本重合，可见距离第二个目标的距离是2005mm。


图 2-7 第二个目标的角点

放大C监视器视图，可见左侧（相对于机器人前进方向）角点在两根方管中间。内侧方管的点只有5个，不满足BlobPointCount=6条件，因此与外侧方管点混入后得到。

这种现象与激光雷达性能有关，如果角分辨率较低，则远距离测量到的点数会少。由于距离远，因此Y方向的偏差角度不至于拉偏航迹。应根据运行时观察到的点云来判断是否要调整BlobPointCount，关键在于靠近时，比如小于1.5m能否稳定观察到。


图 2-8 本例使用的SF参数值

以上是本例调试通过的SF参数值。关键参数如下：

- blobPointsCount，托盘腿的最小点数为6。

- blobSize，实车调试通过值是40。

- blobDist，角点最小距离80，这是调通的关键参数。调通前值为60，角点会飘到两个方管中间，使牵引车偏向一侧撞料车卡住。

- trayTarget，实测最小值为330mm左右。

备注：以上参数名与项目有关（在C定义的配置参数，然后传入SlotFollowing），本文给出的参数对应SlotFollowing。

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

目标角度。

取自“https://wiki.lessokaji.com/index.php?title=开发手册_-_目标跟踪&oldid=487”