基于光学定位的AGV无轨导引方法及系统.pdf

本发明适用于自动化领域，提供一种基于光学定位的AGV无轨导引方法，所述方法包括如下步骤：通过至少二个光学定位传感器获取AGV车体的两个位移信息，两个位移信息分别为：S1和S2；依据S1、S2通过离散运动方程计算出当前时刻的坐标值和方位角；根据该当前时间的坐标值、方位角以及预设路线的方程计算出偏差控制参数；将偏差控制参数输入驱动控制系统控制AGV线路按设定路线行驶。本发明提供的技术方案采用光学定位传感器使AGV进行自主定位，确定AGV位置和姿态信息，从而直接完成无轨导引，无需铺设轨道的优点。

权利要求书
1.  一种基于光学定位的AGV无轨导引方法，其特征在于，所述方法包括如下步骤：
通过至少二个光学定位传感器获取AGV车体的两个位移信息，两个位移信息分别为：S1和S2；
依据S1、S2通过离散运动方程计算出当前时刻的坐标值和方位角；
根据该当前时间的坐标值、方位角以及预设路线的方程计算出偏差控制参数；
将偏差控制参数输入驱动控制系统控制AGV线路按设定路线行驶。

2.  根据权利要求1所述的方法，其特征在于，所述离散运动方程具体为：
x(t)=12Σn=1tT{[S2(Tn)+S1(Tn)]cosθ(t-Tn)+θ(t)2}+x(0)y(t)=12Σn=1tT{[S2(Tn)+S1(Tn)]sinθ(t-Tn)+θ(t)2+y(0)θ(t)=1LΣn=1tT[S2(Tn)-S1(Tn)]+θ(0)]]>
其中，T表示相邻两次采样的时间，Tn表示第n次采样的时间， S1(Tn)第Tn采样的第一个位移，S2(Tn)第Tn采样的第二个位移，t表示当前时刻，x(t)表示当前时刻的X轴坐标；y(t)表示当前时刻t的Y轴坐标，θ(t)表示当前时刻的方位角，x(0)表示起始点(0)的X轴坐标；y(0)表示起始点(0)的Y轴坐标，θ(0)表示起始点(0)的方位角。

3.  根据权利要求1所述的方法，其特征在于，所述根据该当前时间的坐标值、方位角以及预设路线的方程计算出偏差控制参数具体为：
预设路线的方程具体为：ax+by+c＝0；
cosθ=NPMN;]]>
NP=|a·x(t)+b·y(t)+c|a2+b2;]]>
θ＝θ(t)-θ(线)；
其中，θ(t)表示当前时刻的方位角，θ(线)表示预设路线的夹角；MN表示偏差控制参数。

4.  根据权利要求1所述的方法，其特征在于，所述方法在将偏差控制参数输入驱动控制系统控制AGV线路按设定路线行驶之后还包括：
当AGV车体需要切换线路时，根据该当前时间的坐标值、方位角确定AGV车体在切换范围内后，根据该当前时间的坐标值、方位角以及切换后路线的方程计算出切换后的偏差控制参数，将切换后的偏差控制参数输入驱动控制系统控制AGV线路按切换后的路线行驶。

5.  根据权利要求4所述的方法，其特征在于，所述根据该当前时间的坐标值、方位角确定AGV车体在切换范围内后，根据该当前时间的坐标值、方位角以及切换后路线的方程计算出切换后的偏差控制参数具体为：
切换后的线路的方程具体可以为：a'x+b'y+c'＝0；
cosθ=NPMN′;]]>
NP=|a′·x(t)+b′·y(t)+c′|a′2+b′2;]]>
θ＝θ(t)-θ(线)'；
其中，θ(线)'表示切换后的线路的夹角，MN'为切换后的偏差控制参数。

6.  一种基于光学定位的AGV无轨导引系统，其特征在于，所述系统包括：定位系统、导航系统和驱动控制系统，所述定位系统包括：至少两个光学定位传感器和与所述至少两个光学等位传感器配合的光源；其中，
所述定位系统用于获取AGV车体的两个位移信息，两个位移信息分别为：S1和S2，并将所述两个位移信息传送给导航系统；
导航系统，用于依据S1、S2通过离散运动方程计算出当前时刻的坐标值和方位角；根据该当前时间的坐标值、方位角以及预设路线的方程计算出偏差控制参数；导航系统将所述偏差控制参数传递给驱动控制系统；
所述驱动控制系统，用于依据该偏差控制参数控制AGV线路按 设定路线行驶。

7.  根据权利要求6所述的系统，其特征在于，所述离散运动方程具体为：
x(t)=12Σn=1tT{[S2(Tn)+S1(Tn)]cosθ(t-Tn)+θ(t)2}+x(0)y(t)=12Σn=1tT{[S2(Tn)+S1(Tn)]sinθ(t-Tn)+θ(t)2+y(0)θ(t)=1LΣn=1tT[S2(Tn)-S1(Tn)]+θ(0)]]>
其中，T表示相邻两次采样的时间，Tn表示第n次采样的时间，S1(Tn)第Tn采样的第一个位移，S2(Tn)第Tn采样的第二个位移，t表示当前时刻，x(t)表示当前时刻的X轴坐标；y(t)表示当前时刻t的Y轴坐标，θ(t)表示当前时刻的方位角，x(0)表示起始点(0)的X轴坐标；y(0)表示起始点(0)的Y轴坐标，θ(0)表示起始点(0)的方位角。

8.  根据权利要求6所述的系统，其特征在于，所述导航系统具体用于：
预设路线的方程具体为：ax+by+c＝0；
cosθ=NPMN;]]>
NP=|a·x(t)+b·y(t)+c|a2+b2;]]>
θ＝θ(t)-θ(线)；
其中，θ(t)表示当前时刻的方位角，θ(线)表示预设路线的夹角；MN表示偏差控制参数。

9.  根据权利要求6所述的系统，其特征在于，
当AGV车体需要切换线路时，所述导航系统还用于根据该当前时间的坐标值、方位角确定AGV车体在切换范围内后，根据该当前时间的坐标值、方位角以及切换后路线的方程计算出切换后的偏差控制参数，将切换后的偏差控制参数输入驱动控制系统；
驱动控制系统，还用于依据所述切换后的偏差控制参数控制AGV线路按切换后的路线行驶。

10.  根据权利要求9所述的系统，其特征在于，所述导航系统具体用于：
切换后的线路的方程具体可以为：a'x+b'y+c'＝0；
cosθ=NPMN′;]]>
NP=|a′·x(t)+b′·y(t)+c′|a′2+b′2;]]>
θ＝θ(t)-θ(线)'；
其中，θ(线)'表示切换后的线路的夹角，MN'为切换后的偏差控制参数。

说明书基于光学定位的AGV无轨导引方法及系统
技术领域
本发明属于自动化领域，尤其涉及一种基于光学定位的AGV无轨导引方法及系统。
背景技术
自动导引车(英文全称：Automatic Guided Vehicle，英文简称：AGV)系统已经发展成为生产物流系统中最大的专业分支之一。其导引技术主要可分为两个方面，即有轨式导引和无轨式导引。现阶段AGV中最基本的自动导引技术主要包括电磁感应导引、磁带导引、视觉导引、激光导引、惯性导航导引和超声波导引等。其中视觉导引技术是近年来国内外AGV行业的研究的热点之一，视觉传感器以光信号为基础，反应速度快，不易受电磁干扰和环境影响，适应能力强，因此视觉导引技术具有很大的发展潜力。然而，在现有的视觉导引方法中，通常都是利用铺设于地面的色带或者导引带，通过对摄像机或视觉传感器采入的色带图象信号进行一定的处理后来实现AGV的导引，这种视觉导引方法仍属于有轨导引，外界复杂环境对色带轨道或导引带轨道的污染磨损和意外破坏等行为都会导致AGV导引的可靠性降低，甚至导致AGV无法正常行驶和工作。
例如现有技术(CN102608998A)嵌入式系统的视觉导引AGV系统及方法；该现有技术采用DSP图像处理器、ARM处理器和FPGA 为协处理器组成嵌入式硬件系统，通过摄像机采集路径的图像信息进行处理后对AGV进行导引。其采用的技术方法需要在小车的运行路线上铺设黑色或白色色带以获得路径信息，属于有轨导引方式。
例如另一现有技术(CN103064417A)一种基于多传感器的全局定位导引系统及方法；所述的方法其中也采用了视觉传感器，虽然基于多传感器进行全局定位导引，该现有技术仍采用导引带铺设于AGV车体工作区域地面上以实现AGV导引，仍属于有轨导引方式。
在实现现有技术的方案中，发现现有技术存在如下技术问题：
现有技术提供的视觉导引技术无法实现无轨导引。
发明内容
本发明实施例的目的在于提供一种基于光学定位的AGV无轨导引方法及系统，旨在解决现有的技术方案提供的视觉导引技术无法实现无轨导引的问题。
第一方面，提供一种基于光学定位的AGV无轨导引方法，所述方法包括如下步骤：
通过至少二个光学定位传感器获取AGV车体的两个位移信息，两个位移信息分别为：S1和S2；
依据S1、S2通过离散运动方程计算出当前时刻的坐标值和方位角；
根据该当前时间的坐标值、方位角以及预设路线的方程计算出偏差控制参数；
将偏差控制参数输入驱动控制系统控制AGV线路按设定路线行驶。
可选的，所述离散运动方程具体为：
x(t)=12Σn=1tT{[S2(Tn)+S1(Tn)]cosθ(t-Tn)+θ(t)2}+x(0)y(t)=12Σn=1tT{[S2(Tn)+S1(Tn)]sinθ(t-Tn)+θ(t)2+y(0)θ(t)=1LΣn=1tT[S2(Tn)-S1(Tn)]+θ(0)]]>
其中，T表示相邻两次采样的时间，Tn表示第n次采样的时间，S1(Tn)第Tn采样的第一个位移，S2(Tn)第Tn采样的第二个位移，t表示当前时刻，x(t)表示当前时刻的X轴坐标；y(t)表示当前时刻t的Y轴坐标，θ(t)表示当前时刻的方位角，x(0)表示起始点(0)的X轴坐标；y(0)表示起始点(0)的Y轴坐标，θ(0)表示起始点(0)的方位角。
可选的，所述根据该当前时间的坐标值、方位角以及预设路线的方程计算出偏差控制参数具体为：
预设路线的方程具体为：ax+by+c＝0；
cosθ=NPMN;]]>
NP=|a·x(t)+b·y(t)+c|a2+b2;]]>
θ＝θ(t)-θ(线)；
其中，θ(t)表示当前时刻的方位角，θ(线)表示预设路线的夹角；MN表示偏差控制参数。
可选的，所述方法在将偏差控制参数输入驱动控制系统控制AGV线路按设定路线行驶之后还包括：
当AGV车体需要切换线路时，根据该当前时间的坐标值、方位角确定AGV车体在切换范围内后，根据该当前时间的坐标值、方位角以及切换后路线的方程计算出切换后的偏差控制参数，将切换后的偏差控制参数输入驱动控制系统控制AGV线路按切换后的路线行驶。
可选的，所述根据该当前时间的坐标值、方位角确定AGV车体在切换范围内后，根据该当前时间的坐标值、方位角以及切换后路线的方程计算出切换后的偏差控制参数具体为：
切换后的线路的方程具体可以为：a'x+b'y+c'＝0；
cosθ=NPMN′;]]>
NP=|a′·x(t)+b′·y(t)+c′|a′2+b′2;]]>
θ＝θ(t)-θ(线)'；
其中，θ(线)'表示切换后的线路的夹角，MN'为切换后的偏差控制参数。
另一方面，提供一种基于光学定位的AGV无轨导引系统，所述系统包括：定位系统、导航系统和驱动控制系统，所述定位系统包括：至少两个光学定位传感器和与所述至少两个光学等位传感器配合的 光源；其中，
所述定位系统用于获取AGV车体的两个位移信息，两个位移信息分别为：S1和S2，并将所述两个位移信息传送给导航系统；
导航系统，用于依据S1、S2通过离散运动方程计算出当前时刻的坐标值和方位角；根据该当前时间的坐标值、方位角以及预设路线的方程计算出偏差控制参数；导航系统将所述偏差控制参数传递给驱动控制系统；
所述驱动控制系统，用于依据该偏差控制参数控制AGV线路按设定路线行驶。
可选的，所述离散运动方程具体为：
x(t)=12Σn=1tT{[S2(Tn)+S1(Tn)]cosθ(t-Tn)+θ(t)2}+x(0)y(t)=12Σn=1tT{[S2(Tn)+S1(Tn)]sinθ(t-Tn)+θ(t)2+y(0)θ(t)=1LΣn=1tT[S2(Tn)-S1(Tn)]+θ(0)]]>
其中，T表示相邻两次采样的时间，Tn表示第n次采样的时间，S1(Tn)第Tn采样的第一个位移，S2(Tn)第Tn采样的第二个位移，t表示当前时刻，x(t)表示当前时刻的X轴坐标；y(t)表示当前时刻t的Y轴坐标，θ(t)表示当前时刻的方位角，x(0)表示起始点(0)的X轴坐标；y(0)表示起始点(0)的Y轴坐标，θ(0)表示起始点(0) 的方位角。
可选的，所述导航系统具体用于：
预设路线的方程具体为：ax+by+c＝0；
cosθ=NPMN;]]>
NP=|a·x(t)+b·y(t)+c|a2+b2;]]>
θ＝θ(t)-θ(线)；
其中，θ(t)表示当前时刻的方位角，θ(线)表示预设路线的夹角；MN表示偏差控制参数。
可选的，
当AGV车体需要切换线路时，所述导航系统还用于根据该当前时间的坐标值、方位角确定AGV车体在切换范围内后，根据该当前时间的坐标值、方位角以及切换后路线的方程计算出切换后的偏差控制参数，将切换后的偏差控制参数输入驱动控制系统；
驱动控制系统，还用于依据所述切换后的偏差控制参数控制AGV线路按切换后的路线行驶。
可选的，所述导航系统具体用于：
切换后的线路的方程具体可以为：a'x+b'y+c'＝0；
cosθ=NPMN′;]]>
NP=|a′·x(t)+b′·y(t)+c′|a′2+b′2;]]>
θ＝θ(t)-θ(线)'；
其中，θ(线)'表示切换后的线路的夹角，MN'为切换后的偏差控制参数。
在本发明实施例中，本发明提供的技术方案采用光学定位传感器使AGV进行自主定位，确定AGV位置和姿态信息，从而直接完成无轨导引，无需铺设轨道的优点。
附图说明
图1为本发明提供的基于光学定位的AGV无轨导引系统框图，
图2为光学定位传感器加装激光光源的照射方式示意图；
图3为光学定位传感器的安装位置示意图；
图4为光学定位传感器定位AGV的位置姿态(x0，y0，θ0)的算法解析图；
图5为AGV行驶过程中导航坐标的偏差控制参数算法解析图；
图6为AGV行驶过程中的路线切换处理方式示意图；
图7为本发明提供的一种基于光学定位的AGV无轨导引方法的流程图。
具体实施方式
为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描 述的具体实施例仅仅用以解释本发明，并不用于限定本发明。
本发明具体实施方式提供一种基于光学定位的AGV无轨导引方法，该方法如图7所示，包括如下步骤：
101、通过至少二个光学定位传感器获取AGV车体的两个位移信息，两个位移信息分别为：S1和S2；
102、依据S1、S2通过离散运动方程计算出当前时刻的坐标值和方位角；
103、根据该当前时间的坐标值、方位角以及预设路线的方程计算出偏差控制参数；
104、将偏差控制参数输入驱动控制系统控制AGV线路按设定路线行驶。
可选的，上述离散运动方程具体可以为：
x(t)=12Σn=1tT{[S2(Tn)+S1(Tn)]cosθ(t-Tn)+θ(t)2}+x(0)y(t)=12Σn=1tT{[S2(Tn)+S1(Tn)]sinθ(t-Tn)+θ(t)2+y(0)θ(t)=1LΣn=1tT[S2(Tn)-S1(Tn)]+θ(0)]]>
其中，T表示相邻两次采样的时间，Tn表示第n次采样的时间，S1(Tn)第Tn采样的第一个位移，S2(Tn)第Tn采样的第二个位移，t表示当前时刻，x(t)表示当前时刻的X轴坐标；y(t)表示当前时刻t 的Y轴坐标，θ(t)表示当前时刻的方位角，x(0)表示起始点(0)的X轴坐标；y(0)表示起始点(0)的Y轴坐标，θ(0)表示起始点(0)的方位角。
可选的，上述103的实现方法具体可以为：预设路线的方程具体可以为：ax+by+c＝0；
cosθ=NPMN;]]>
NP=|a·x(t)+b·y(t)+c|a2+b2;]]>
θ＝θ(t)-θ(线)；
其中，θ(t)表示当前时刻的方位角，θ(线)表示预设路线的夹角；MN表示偏差控制参数。
可选的，上述方法在104之后，还可以包括：
当AGV车体需要切换线路时，根据该当前时间的坐标值、方位角确定AGV车体在切换范围内后，根据该当前时间的坐标值、方位角以及切换后路线的方程计算出切换后的偏差控制参数，将切换后的偏差控制参数输入驱动控制系统控制AGV线路按切换后的路线行驶。
其具体的实现方式为：
切换后的线路的方程具体可以为：a'x+b'y+c'＝0；
cosθ=NPMN′;]]>
NP=|a′·x(t)+b′·y(t)+c′|a′2+b′2;]]>
θ＝θ(t)-θ(线)'；
其中，θ(线)'表示切换后的线路的夹角，MN'为切换后的偏差控制参数。
本发明具体实施方式还提供一种基于光学定位的AGV无轨导引系统，所述系统包括：定位系统、导航系统和驱动控制系统，所述定位系统包括：至少两个光学定位传感器和与所述至少两个光学等位传感器配合的光源；其中，
所述定位系统用于获取AGV车体的两个位移信息，两个位移信息分别为：S1和S2，并将所述两个位移信息传送给导航系统；
导航系统，用于依据S1、S2通过离散运动方程计算出当前时刻的坐标值和方位角；根据该当前时间的坐标值、方位角以及预设路线的方程计算出偏差控制参数；导航系统将所述偏差控制参数传递给驱动控制系统；
所述驱动控制系统，用于依据该偏差控制参数控制AGV线路按设定路线行驶。
可选的，上述离散运动方程具体为：
x(t)=12Σn=1tT{[S2(Tn)+S1(Tn)]cosθ(t-Tn)+θ(t)2}+x(0)y(t)=12Σn=1tT{[S2(Tn)+S1(Tn)]sinθ(t-Tn)+θ(t)2+y(0)θ(t)=1LΣn=1tT[S2(Tn)-S1(Tn)]+θ(0)]]>
其中，T表示相邻两次采样的时间，Tn表示第n次采样的时间，S1(Tn)第Tn采样的第一个位移，S2(Tn)第Tn采样的第二个位移，t表示当前时刻，x(t)表示当前时刻的X轴坐标；y(t)表示当前时刻t的Y轴坐标，θ(t)表示当前时刻的方位角，x(0)表示起始点(0)的X轴坐标；y(0)表示起始点(0)的Y轴坐标，θ(0)表示起始点(0)的方位角。
可选的，上述导航系统具体用于：
预设路线的方程具体为：ax+by+c＝0；
cosθ=NPMN;]]>
NP=|a·x(t)+b·y(t)+c|a2+b2;]]>
θ＝θ(t)-θ(线)；
其中，θ(t)表示当前时刻的方位角，θ(线)表示预设路线的夹角；MN表示偏差控制参数。
可选的，当AGV车体需要切换线路时，所述导航系统还用于根据该当前时间的坐标值、方位角确定AGV车体在切换范围内后，根据该当前时间的坐标值、方位角以及切换后路线的方程计算出切换后的偏差控制参数，将切换后的偏差控制参数输入驱动控制系统；
驱动控制系统，还用于依据所述切换后的偏差控制参数控制AGV线路按切换后的路线行驶。
可选的，导航系统具体用于：
切换后的线路的方程具体可以为：a'x+b'y+c'＝0；
cosθ=NPMN′;]]>
NP=|a′·x(t)+b′·y(t)+c′|a′2+b′2;]]>
θ＝θ(t)-θ(线)'；
其中，θ(线)'表示切换后的线路的夹角，MN'为切换后的偏差控制参数。
下面结合附图简述一下本发明的原理：
图1为本发明的系统框图，其中AGV定位系统包括光学定位传感器和位置姿态计算，将光学定位传感器按照一定的安装方式安装在AGV车体的驱动轮平台上，通过相应的算法计算出AGV的运动方程，从而解算出AGV的在任一时刻的位置和姿态信息。将得到的位置和姿态信息以坐标起始点(x0，y0，θ0)的数据格式上传至AGV路线导航系统中，起始点的坐标为用户自行确定的点。AGV路线导航系统跟据现场的实际情况建立参考坐标系。基于此参考坐标系，设立AGV所要行驶的不同路线方程，通过AGV坐标和方位角信息(x0，y0，θ0)与路线方程比较计算得出偏差控制参数Err。将此偏差控制参数Err上传至驱动控制系统。驱动控制系统可用PLC或单片机作为主控单元，通过PID闭环控制算法来驱动电机，从而达到AGV按规定路线行驶的目的。
图2为光学定位传感器加装激光光源的照射方式。为了使光学定位传感器能够在距离地面一定的高下正常工作，需要加装一定波长范 围内的光源，本实施例采用650nm的红线激光。通过调整激光头的照射角度，可以调整光学定位传感器距离地面的正常工作高度。从而保证AGV可以在不同地面环境下正常行驶。如图2所示，其中，激光头202，光学定位传感器201、感光器镜头2011，AGV车体203。
图3为光学定位传感器的安装位置。本实施例中采用两个光学定位传感器，提供了两个定位传感器的安装及其相应的位置姿态的解算。将光学定位传感器安装在AGV车体的驱动轮平台的底部，安装位置为与两个驱动轮在同一轴线且到中心点O的距离相等，两个光学定位传感器AB之间的距离为L。这种安装方式可以保证光学定位传感器在移动时只返回y轴方向的位移，x轴方向并不返回值。
图4为光学定位传感器定位AGV的位置姿态(x0，y0，θ0)的算法解析图。由光学定位传感器的安装位置可以保证，AGV在行驶时，光学定位传感器并不返回其x轴方向的位移值，只返回y轴方向的位移值。任何物体的运动都可看作惯性运动，可将参考坐标系看作一个二维的平面坐标系。通过二维惯性平台定律，已知物体任意两点的实时速度，即可知道物体的位姿信息。则已知AGV驱动轮平台两点A和B的速度信息，即可以推导出AGV驱动轮的运动方程如下：
设AGV任一时刻O点的坐标为(x0，y0)，方位角为θ0，O点的实时速度为v0，角速度为ω0，则有
x·0y·0θ·0=cosθ00sinθ0001·v0ω0]]>
设安装了光学定位传感器的A点实时速度为vA，B点的实时速度为vB，两点间的距离为L，则
v0=vB-vA2,ω0=vB-vAL]]>
可推倒出AGV驱动轮平台的O点的运动方程为：
x(t)=12∫0t[(vB+vA)cosθ]dt+x(0)y(t)=12∫0t[(vB+vA)sinθ]dt+t(0)θ(t)=1L∫0t(vB-vA)dt+θ(0)]]>
式中，[x(0)，y(0)，θ(0)]为初始时刻的坐标和方位角。
由于在进行软件程序编写时，读取光学定位传感器的数据是周期性的离散数据，所获得的数据是A、B两点的位移信息，且每个周期的采样时间很短，在每个采样周期内A、B两点所运动的弧长很短，所以可将其近似为直线位移，分别为S2和S1。设采样周期为Tn，其中(n＝1,2,3……)，且T1＝T2＝T3＝……＝T，则第n个周期AB两点采样的弧长位移可分别表示为S2(Tn)和S1(Tn)。则根据O点的运动方程，将其离散化后可得到利于程序编写的运动方程为：
x(t)=12Σn=1tT{[S2(Tn)+S1(Tn)]cosθ(t-Tn)+θ(t)2}+x(0)y(t)=12Σn=1tT{[S2(Tn)+S1(Tn)]sinθ(t-Tn)+θ(t)2+y(0)θ(t)=1LΣn=1tT[S2(Tn)-S1(Tn)]+θ(0)]]>
其中在利用角度计算坐标时，将角度的取值取为上一时刻的角度值和当前时刻的角度值的加权平均值，以此来减小采样和计算误差。由此运动方程便获得了AGV任一时刻的坐标和方位角[x(t)，y(t)，θ(t)]。将此位置和姿态上传于路线导航系统，计算出偏差控制参数，用以控制电机驱动AGV行驶。
图5为AGV行驶过程中导航坐标的偏差控制参数算法解析图。本实施例给出一条直线路线的算法解析，已知建立的参考坐标系中的一条路线Line的直线方程为ax+by+c＝0，路线Line的方位角为θ1，AGV的坐标和方位角为(x0，y0，θ0)。设AGV与路线的方位角的夹角为θ＝θ0-θ1，为了更好的控制AGV按指定路线行驶，选择沿AGV方位角超前ON距离的N点坐标(x1，y1)作为控制坐标，以达到超前校正的目的。经过N点，与ON方向垂直作垂线与路线Line交于M点。则线段MN的长即作为所要控制的偏差控制参数Err。计算如下：
N点到路线Line的距离NP为NP=|a·x1+b·y1+c|a2+b2;]]>
由cosθ=NPMN;]]>即可计算出MN的长度Err。
这种方式计算的偏差控制参数Err，即可以反映AGV坐标与路线的偏差，也反映了AGV的方位角与路线的方位角的相对关系。将偏差控制参数Err上传至驱动控制系统，经过PID控制即可控制AGV按指定路线行驶。
图6为AGV行驶过程中的路线切换处理方式。设当AGV由路线Line1向路线Line2切换行驶时，AGV的控制坐标为(x1,y1),切换点Q为路线Line1与Line2的交点，坐标为(x2,y2)。由于AGV的控制坐标并非时时刻刻都在路线轨迹Line1上，所以在进行路线切换时需要进行容错处理。以Q点为圆心，切换半径为r作圆，当N点运动至该切换圆内时进行参考路线的切换，即需同时满足|x2-x1|<＝r和|y2-y1|<＝r时，将参考路线由Line1切换至Line2，从而保证AGV能够正常地进行路线切换。
以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。