开发手册 - 目标跟踪：修订间差异
无编辑摘要 |
（更新SF） |
||
| 第141行： | 第141行： | ||
|} | |} | ||
== 2.6 | == 2.6 最佳实践 == | ||
以连续穿行4台料车为例讲解调试方法。 | |||
[[文件:63e24d26163f760a492da6264c02027.jpg|无框|791x791像素]] | |||
'''图 2-1 由4台料车构成的“隧道”''' | |||
潜伏式牵引车连续穿行4台料车，在最后一台料车下面停车，车头距离料车后腿约0.5米。 | |||
[[文件:A3084267272a61c3028c920be5c7f2c.jpg|无框]] | |||
'''图 2-2 激光雷达在牵引车头''' | |||
牵引车使用头部激光雷达识别料车腿对，觅中点连续穿行直至终点。 | |||
把牵引车遥控到距离首个托盘约1.5米位置，停车，使用M的前雷达视图查看托盘腿点云。 | |||
[[文件:1702888433921.png|无框|788x788像素]] | |||
'''图 2-3 M的前雷达视图''' | |||
使用“测量”功能，可得雷达距离跟踪目标的距离约1米（矩形框）。两个圆框是要识别的托盘腿，每个托盘腿由两根方管构成，尺寸是30mm，两根方管间距30mm。跟踪目标是内侧一对方管，间距600mm，制造误差5mm。 | |||
[[文件:2312181638.png|无框|377x377像素]] | |||
'''图 2-4 测量内侧方管尺寸''' | |||
测量内侧方管尺寸为32mm，与卷尺测量值相似，考虑到激光雷达测量误差，将BlobSize设为40。构成内侧腿的点数量为6，将BlobPointCount设为6。测量内外方管点云中心距离为72mm，将BlobDistance设为80。这些参数作为角点调试的起始参数。 | |||
[[文件:2312181646.png|无框|854x854像素]] | |||
'''图 2-5 C监视器的SF视图''' | |||
在C启动SF算法，C监视器显示SF状态。放大视图，能够观察到两个内侧方管被锁定为角点（红色实心圆点），角点中心位于方管内，橙色连线表示跟踪目标已锁定，其中点（跟踪目标）的红色箭头线指向机器人运动中心，运动中心到红色箭头线的黄线是预瞄点连线，夹角表示机器人向右跟踪预瞄点。跟踪目标旁的两个数字分别是距离和托盘腿宽度，前者是机器人运动中心到跟踪目标距离，加上了激光雷达到运动中心距离，调试时应注意，即目标距离有最小值，不会到0，本例中的最小距离是330mm；后者是调TrayWidth、TrayWidthRange的依据，由于激光雷达在运动中测量，角点会跳动，因此托盘腿宽度是一个范围。 | |||
满足托盘腿宽度条件的角点对以青色和粉色线表示，被选中托盘腿以橙色线表示。上图中可见机器人右侧2条被过滤的角点对。 | |||
遥控机器人前进，观察远近距离时跟踪目标是否稳定，即内侧腿角点不会大幅度跳动，使跟踪目标与机器人夹角增大，最终跑偏。本例中内侧腿角点不应跳动到内外方管中间，甚至外侧方管，黄色预瞄点连线夹角不应大幅度跳动。 | |||
[[文件:2312181702.png|无框|861x861像素]] | |||
'''图 2-6 跟踪下个目标''' | |||
机器人距离目标距离小于LookupNextDistance时跟踪下个目标，如上图。在机器人在运动中顺序跟踪目标时，应注意角点是否锁定内侧方管，预瞄点连线与目标垂线应基本重合，可见距离第二个目标的距离是2005mm。 | |||
[[文件:2312181705.png|无框|675x675像素]] | |||
'''图 2-7 第二个目标的角点''' | |||
放大C监视器视图，可见左侧（相对于机器人前进方向）角点在两根方管中间。内侧方管的点只有5个，不满足BlobPointCount=6条件，因此与外侧方管点混入后得到。 | |||
这种现象与激光雷达性能有关，如果角分辨率较低，则远距离测量到的点数会少。由于距离远，因此Y方向的偏差角度不至于拉偏航迹。应根据运行时观察到的点云来判断是否要调整BlobPointCount，关键在于靠近时，比如小于1.5m能否稳定观察到。 | |||
[[文件:2312181711.png|无框|663x663像素]] | |||
'''图 2-8 本例使用的SF参数值''' | |||
以上是本例调试通过的SF参数值。关键参数如下： | |||
- blobPointsCount，托盘腿的最小点数为6。 | |||
- blobSize，实车调试通过值是40。 | |||
- blobDist，角点最小距离80，这是调通的关键参数。调通前值为60，角点会飘到两个方管中间，使牵引车偏向一侧撞料车卡住。 | |||
- trayTarget，实测最小值为330mm左右。 | |||
备注：以上参数名与项目有关（在C定义的配置参数，然后传入SlotFollowing），本文给出的参数对应SlotFollowing。 | |||
== 2.7 参数清单 == | == 2.7 参数清单 == | ||
2023年12月18日 (一) 17:17的最新版本
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
目标角度。