AGV 基础跟踪算法模型

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