AGV Path Tracking
AGV的轨迹已经保存在文件中，t=0时，[x0，y0，V0]；经过单位时间T，[x1，y1，V1]，……。
提出2种轨迹跟踪算法。
建议先采用轨迹跟踪算法1进行调试，然后再采用轨迹跟踪算法2。因为轨迹跟踪算法1简单明确，容易理解和验证以及发现问题。轨迹跟踪算法2要复杂一些，但是效果会更好一些。
跟踪算法1
（1）AGV驱动轮的角度?
AGV与轨迹中的下一个位置如图所示。
图1
AGV与轨迹中的下一个位置
AGV根据当前自己的位置(xc,yc)和目标位置(xt,yt)，计算AGV的前进方向误差?，根据当前AGV车体的方向和前向驱动轮的方向，输出前向驱动轮调整到目标位置(xt,yt)方向的角度?。
（2）AGV驱动轮的速度
采用保存在轨迹文件中的速度参数。
跟踪算法2
（1）AGV驱动轮的角度?
AGV与轨迹中的下一个位置如图所示。
图2 AGV与轨迹中的下一个位置
图2有2个坐标系，1个是全局坐标系，另一个是以AGV为原点的坐标系，简称局部坐标系。
在局部坐标系中，目标位置为(xt,yt)。
AGV的当前位置到目标位置(xt,yt)的直线距离为D。
AGV车体的前方方向与全局坐标系x轴的方向夹角?（通过计算得到）。 直线距离为D与全局坐标系x轴的方向夹角?（通过计算得到）。 AGV当前位置点和目标位置点(xt,yt)在一个以r为半径的圆上。根据图2的几何图形，求半径r推导如下：
x?yt r?d?yt?(r?xt)?yt?r?2rxt?xt?yt -> r?t2xt222222222则驱动轮的角度?
wr其中w是AGV前后轮的距离。
??2sin?1()
目标位置点(xt,yt)的计算：
由AGV在全局坐标系中的当前位置点(x'c,y'c)、目标位置点(x't,y't)和AGV
的前进方向与全局坐标系的Y轴的夹角?（如图2中的标注?为正值，坐标系逆时针旋转。反之，?为负值）求的： xt?(x't-x'c)cos??(y't-y'c)sin? yt??(x't-x'c)sin??(y't-y'c)cos?
（2）AGV驱动轮的速度
采用保存在轨迹文件中的速度参数。
2012-11-17
AGV 基础跟踪算法模型
