查看“开发手册 - 轨迹跟踪”的源代码


= 轨迹跟踪 =
[[文件:轨迹跟踪模型.png|无框|1015x1015像素]]


'''图 1-1 轨迹跟踪模型'''

Clumsy 根据 Simple 下发的路线行驶，路线由若干路径组成。我们将其简化为如上图的一段路径，设 Clumsy 从 A 走到 B，此时机器人不在路径上。

轨迹跟踪步骤如下：

# 以机器人起点初始化。（注意机器人可能不在路径起点上）。
# 以终点构成路径，并加入起点和终点的动作。
# 预瞄点及速度计算：
#* 计算机器人到路径的垂足。
#* 根据垂足、预瞄点距离和速度计算预瞄点。
#* 计算机器人与预瞄点的位姿关系。
#* 根据余下路程计算前进速度。
# 根据预瞄点及速度：
#* 计算速度在 XY 轴分量。
#* 计算与预瞄点对齐所需的旋转角度。如果旋转角度较大，则先自旋使车头指向预瞄点。
# 平滑速度：
#* 进入（规划轨迹）速度。
#* 进入（规划轨迹）角度。
# 爬升到设定转向速度，以最大旋转速度为限。
# 下发速度（舵轮还有角度）给 M（底盘）。
# 在每段路径循环上述步骤，并依次执行起点和终点动作。
# 到终点前减速。
# 当机器人与终点坐标小于给定偏差时，停车并终止轨迹跟踪。

Clumsy 首先计算机器人中心点到路径的垂足，然后将垂足加上预瞄距离，得到在路径上的预瞄点。

预瞄点是指机器人运动到目标点之前，先瞄准的一个虚拟点。预瞄点的作用主要有：

# 提高运动的稳定性和准确性。预瞄点可以让机器人提前做好准备，平稳地进入目标点，而不是直接朝目标点突进，可以减少末端抖动。
# 实现平滑的运动曲线。通过预瞄点可以让机器人运动轨迹平滑地过渡，而不是生硬的直线运动，有利于保护机械结构。
# 实现避障规划。可以通过设置合理的预瞄点让机器人绕过障碍物，实现从当前位置到目标位置的避障运动。
# 减小定位误差。机器人本身和环境都存在误差，预瞄点可以让机器人稍微提前抵达，从而缩小终点定位的误差。
# 方便轨迹规划。规划运动轨迹时，通过预瞄点作为中间点，可以简化轨迹规划的复杂度。

预瞄点计算与以下参数相关：

* lookAhead，机器人到路径垂足往前进方向的追踪量，可调节小车贴近路径的快慢，过小容易超调，过大则调整过慢，建议高速长直线适当增大，低速高精度适当调小。这个参数本质上是机器人、垂足和预瞄点构成的直角三角形邻边，邻边越长，角度越小，机器人围绕路径调整幅度约小。
* lookAheadPow & lookAheadFactor，调整预瞄点距离的幂函数指数和比例。前者参考图 1-1 右侧的幂函数曲线(y=x^0.5)，目的是速度越快距离越远；后者就是速度和距离的比例控制。两者结合用于细调预瞄点。
* lineSnappingDist，车体到路径的距离，控制 lookAhead 衰减。该参数给出高斯成员函数的中心点，用于平滑机器人到垂足的距离，以免超快接近。越小则优先修正到路径上，越大则修正路径越平缓。该值由经验法得到，过小则超调。
* wayPointFinBase，停车距离基数。当机器人行驶在最后一段路径，或下一段路径需要大角度转向时，则根据停车距离降低速度。
* wapPointFinSpeedPow & wayPointFinSpeedFactor，进入停车距离的减速参数。前者是幂函数指数，后者是比例系数。与 lookAheadPow & lookAheadFactor 作用相似。

Clumsy 根据机器人位姿与预瞄点角度，以及余下路程比例，计算转向角度。

当余下路程短于停车距离时，且这是最后一段路径，则减速停车。减速停车与以下参数相关：

* stoppingPow，减速幂函数指数。
* baseSpeed，如果 Simple 规划路径未带速度，则使用 Clumsy 的 baseSpeed 参数。

Clusmy 根据以下参数判断机器人是否到达路径终点：

* trackFinDist，终点圆半径，默认为 10mm。
* finThAccuracy，终点角度偏差，默认为 1°。如果停车时角度偏差大于该参数，则角度修正置位，执行 agv.wait()指令时修正角度偏差。

当机器人转向时，Clumsy 根据以下参数计算转弯速度：

* nffac，转向角度 PID 控制的负反馈系数，用于平滑角度调整量。
* firstTurnAccuracy，机器人与预瞄点的临界角度，如果大于此参数，Clumsy 旋转机器人使之对准预瞄点，以免大角度行走转向脱轨。默认值 90°。
* speedDegThres，与 firstTurnAccuracy 配合使用，当机器人和预瞄点夹角在此范围内开始移动。

在行走时，Clumsy 根据以下参数计算速度：

* thresAcc，直线启动和停止加速度。
* thresWAcc，转向加速度。
* maxWSpeed，最大转向线速度。