CN112130558A - 一种基于差速轮组agv的导航系统及控制方法 - Google Patents

Abstract

本发明涉及一种基于差速轮组AGV的导航系统及控制方法，导航系统包括导航路径和AGV，导航路径为设置在地面的导航磁条，AGV设有四个呈中心对称分布的差速轮组和四个呈中心对称分布的磁导航传感器，差速轮组设有两个对称分布的驱动轮，磁导航传感器用于采集导航路径信息，其具有结构简单、成本低廉、定位精确、稳定性好、安全性高的优点；控制方法具有流程简单、执行速度快、纠偏精度高、稳定可靠的优点。

Description

一种基于差速轮组AGV的导航系统及控制方法

技术领域

本发明涉及一种AGV导航系统，具体涉及一种基于差速轮组AGV的导航系统，以及该导航系统的控制方法。

背景技术

随着科技和社会的发展，AGV(自动引导运输车)得到了快速发展和广泛应用。采用麦克纳姆轮的AGV可进行连续全向运动，其自动导航比较容易实现。而差速轮组AGV因不能实现连续全向运动，无法适用基于麦克纳姆轮AGV的导航系统，且目前本领域还没有针对差速轮组AGV的有效导航系统和方法。随着差速轮组AGV在重载领域的应用，亟需一种适用差速轮组AGV的导航系统或方法，以实现其自动导航和精确定位。

发明内容

本发明的目的是提供一种基于差速轮组AGV的导航系统及控制方法，导航系统具有结构简单、成本低廉、定位精确、稳定性好、安全性高的优点；其具有流程简单、执行速度快、纠偏精度高、稳定可靠的优点。

为解决现有技术中存在的上述问题，本发明提供了一种基于差速轮组AGV的导航系统，包括导航路径和AGV，所述导航路径为设置在地面的导航磁条，所述AGV设有四个呈中心对称分布的差速轮组和四个呈中心对称分布的磁导航传感器，所述差速轮组设有两个对称分布的驱动轮，所述磁导航传感器用于采集导航路径信息。

进一步的，本发明一种基于差速轮组AGV的导航系统，其中，所述导航路径的一侧设有用于定位AGV的定位磁地标，导航路径的另一侧设有RFID位置码；所述AGV上设有对应识别定位磁地标和RFID位置码的磁地标传感器和RFID传感器。

进一步的，本发明一种基于差速轮组AGV的导航系统，其中，所述AGV的前后侧中部和左右侧中部分别设有激光扫描仪，激光扫描仪用于识别导航路径上的障碍物并触发保护指令。

进一步的，本发明一种基于差速轮组AGV的导航系统，其中，所述差速轮组安装有绝对值编码器，绝对值编码器用于检测差速轮组的航向角。

进一步的，本发明一种基于差速轮组AGV的导航系统，其中，所述RFID位置码设置在导航路径的节点位置，节点位置为启动点位置、停止点位置、加速点位置、减速点位置或变向点位置。

进一步的，本发明一种基于差速轮组AGV的导航系统，其中，所述AGV以自身坐标系为车身坐标系，车中心为原点，正前方为Y方向，正右方为X方向；所述四个差速轮组对应设置在AGV的四个角处，右前方的差速轮组为一号轮组，其余的差速轮组按逆时针依次为二号轮组、三号轮组和四号轮组。

进一步的，本发明一种基于差速轮组AGV的导航系统，其中，所述四个磁导航传感器分别为前磁导航传感器、后磁导航传感器、左磁导航传感器和右磁导航传感器；前磁导航传感器和后磁导航传感器用于AGV在Y方向行进时采集导航路径信息，左磁导航传感器和右磁导航传感器用于AGV在X方向行进时采集导航路径信息。

本发明一种上述导航系统的控制方法，包括以下步骤：

S1、在Y方向的启动点位置，给定AGV的Y方向速度Vy，并通过导航纠偏方法使AGV沿导航路径行进；

S2、当RFID传感器检测到表示加速的RFID位置码时，使Vy的值以线性方式增大至预设的第一目标值；

S3、当RFID传感器检测到表示减速的RFID位置码时，使Vy的值以线性方式减小至预设的第二目标值；

S4、当RFID传感器检测到表示停止的RFID位置码时，使Vy的值以线性方式减小至零；并通过磁地标传感器检测定位磁地标对AGV进行定位；

S5、当RFID传感器检测到表示变向的RFID位置码时，使Vy的值以线性方式减小至零；并使各差速轮组旋转90°后，给定AGV的X方向速度Vx。

进一步的，本发明一种导航系统的控制方法，其中，所述导航纠偏方法包括以下步骤：

S101、通过前磁导航传感器和后磁导航传感器实时采集导航路径信息，通过绝对值编码器实时检测各差速轮组的当前航向角；

S102、根据前磁导航传感器和后磁导航传感器的位置，计算AGV中心相对导航路径的偏移量DP和航向偏角AP，航向偏角AP是指Vy方向与导航路径之间形成的夹角；

S103、根据偏移量DP和航向偏角AP计算纠偏速度Vx_j和纠偏转速w，w是指AGV绕中心的旋转速度；

S104、采用速度分解法将Vy、Vx_j和w分解到各差速轮组，以得到各差速轮组的目标速度和目标航向角；

S105、根据各自的目标速度、目标航向角计算各差速轮组驱动轮的目标角速度；

S106、根据各差速轮组驱动轮的目标角速度控制对应驱动电机的转速。

进一步的，本发明一种导航系统的控制方法，其中，

在上述步骤S102中，所述偏移量DP和航向偏角AP按以下公式计算获得，

DP＝FCV-BCV (1)

AP＝FCV+BCV-n (2)

公式(1)、(2)中，FCV为前磁导航传感器相对导航路径的位置，BCV为后磁导航传感器相对导航路径的位置，n等于磁导航传感器的探测点位数加1；

在上述步骤S103中，所述纠偏速度Vx_j和纠偏转速w按以下公式计算获得，

Vx_j＝DP/DP_MAX×Max_Vx (3)

w＝AP/AP_MAX×Max_w (4)

Max_Vx＝tan(AP_MAX/180×π)×Vy (5)

Max_w＝tan(AP_MAX/180×π)×Vy/(y-x×(AP_MAX/180×π)) (6)

公式(3)、(4)、(5)、(6)中，DP_MAX为最大偏移量，取值L/2，L为磁导航传感器的长度；AP_MAX为最大航向偏角，取值3～5°；Max_Vx和Max_w为随Vy变化的可变比例因子；x和y为各差速轮组中心坐标的绝对值；

在上述步骤S104中，所述各差速轮组的目标速度和目标航向角按以下公式计算获得，

Vx_m＝Vx_j+y_m×w (7)

Vy_m＝Vy+x_m×w (8)

α_m＝atan(Vx_m/Vy_m) (9)

公式(7)、(8)、(9)中，Vx_m和Vy_m为各差速轮组的目标速度，α_m为各差速轮组的目标航向角，x_m和y_m为各差速轮组中心的坐标；

在上述步骤S105中，所述各差速轮组驱动轮的目标角速度按以下公式计算获得，

Speed_m＝Vy_m/r (10)

dspeed_m＝Speed_m×Scale_m (11)

Speed_m_1＝Speed_m+dspeed_m (12)

Speed_m_2＝Speed_m-dspeed_m (13)

当Δα_m＝0时，Scale_m＝0 (14)

当0<|Δα_m|≤K1时，Scale_m＝0.15～0.2 (15)

当K1<|Δα_m|<K2时，Scale_m＝0.2～0.25 (16)

公式(10)、(11)、(12)、(13)、(14)、(15)、(16)中，Speed_m为基准角速度，dspeed_m为差速值，Scale_m为差速比例因子，Speed_m_1和Speed_m_2为两驱动轮的目标角速度，车头偏左时，左驱动轮的目标角速度的Speed_m_1，右驱动轮的目标角速度的Speed_m_2，车头偏右时，左驱动轮的目标角速度的Speed_m_2，右驱动轮的目标角速度的Speed_m_1，Δα_m为目标航向角与当前航向角的差值，K1取值3.6～4.5，K2取值9.6～10.5。

本发明一种基于差速轮组AGV的导航系统及控制方法与现有技术相比，具有以下优点：本发明通过设置导航路径和AGV，使导航路径采用设置在地面的导航磁条，使AGV安装四个呈中心对称分布的差速轮组和四个呈中心对称分布的磁导航传感器，其中，差速轮组设有两个对称分布的驱动轮，磁导航传感器用于采集导航路径信息。由此就构成了一种结构简单、成本低廉、定位精确、稳定性好、安全性高的基于差速轮组AGV的导航系统。本发明通过采用磁条作为导航路径，降低了成本和布置难度，提高了抗干扰能力；通过使AGV安装四个呈中心对称分布的差速轮组，在需要转向时让四个差速轮组旋转90度即可，而不需要整个车身转向，降低了控制难度，节省了转向空间；通过设置四个呈中心对称分布的磁导航传感器，其中两个用于AGV在Y方向行进时采集导航路径信息，另两个用于AGV在X方向行进时采集导航路径信息，提高了采集导航路径信息的准确性和导航精度。本发明提供的导航系统的控制方法具有流程简单、执行速度快、纠偏精度高、稳定可靠的优点。

下面结合附图所示具体实施方式对本发明一种基于差速轮组AGV的导航系统及控制方法作进一步详细说明。

附图说明

图1为本发明一种基于差速轮组AGV的导航系统的示意图；

图2为本发明一种基于差速轮组AGV的导航系统中AGV的示意图。

具体实施方式

首先需要说明的，本发明中所述的上、下、左、右、前、后等方位词只是根据附图进行的描述，以便于理解，并非对本发明的技术方案及请求保护范围进行的限制。

如图1和图2所示本发明一种基于差速轮组AGV的导航系统的具体实施方式，包括导航路径1和AGV 2。使导航路径采用设置在地面的导航磁条。使AGV安装四个呈中心对称分布的差速轮组和四个呈中心对称分布的磁导航传感器。其中，差速轮组设有两个对称分布的驱动轮，磁导航传感器用于采集导航路径信息。通过以上结构设置就构成了一种结构简单、成本低廉、定位精确、稳定性好、安全性高的基于差速轮组AGV的导航系统。本发明通过采用磁条作为导航路径，降低了成本和布置难度，提高了抗干扰能力。通过使AGV安装四个呈中心对称分布的差速轮组，在需要转向时让四个差速轮组旋转90度即可，而不需要整个车身转向，降低了控制难度，节省了转向空间，提高了转向的稳定性。通过设置四个呈中心对称分布的磁导航传感器，其中两个用于AGV在Y方向行进时采集导航路径信息，另两个用于AGV在X方向行进时采集导航路径信息，提高了采集导航路径信息的准确性和导航精度。需要说明的是，在实际应用中，各差速轮组在其中心与车身的连接处安装有绝对值编码器，以便通过各自的绝对值编码器实时检测其航向角。

作为优化方案，本具体实施方式在导航路径的一侧设置了用于定位AGV的定位磁地标3，在导航路径的另一侧设置了RFID位置码4，并在AGV上设置了对应识别定位磁地标3和RFID位置码4的磁地标传感器5和RFID传感器6。这一结构通过控制定位磁地标的大小和位置，可有效提高行进过程中对AGV定位的精确性；通过设置RFID位置码和RFID传感器，提高了导航控制的灵活性以及对环境的适应能力。在实际应用中，将RFID位置码设置在导航路径的节点位置，节点位置可以为启动点位置、停止点位置、加速点位置、减速点位置或变向点位置等，以便RFID传感器检测到RFID位置码时触发相应的控制信号。为防止AGV在行进过程中撞上障碍物，本具体实施方式在AGV的前后侧中部和左右侧中部分别设置了激光扫描仪7，以通过激光扫描仪识别导航路径上的障碍物并触发保护指令。保护指令通常为停止信号或警报信号。

作为具体实施方式，本发明使AGV以自身坐标系为车身坐标系，车中心为原点，正前方为Y方向，正右方为X方向，以便于控制。在实际应用中通常将四个差速轮组对应设置在AGV的四个角处，并将右前方的差速轮组规定为一号轮组21，其余的差速轮组按逆时针依次为二号轮组22、三号轮组23和四号轮组24，以便于区分。而四个磁导航传感器根据其位置区分为前磁导航传感器25、后磁导航传感器26、左磁导航传感器27和右磁导航传感器28，其中，前磁导航传感器和后磁导航传感器用于AGV在Y方向行进时采集导航路径信息，左磁导航传感器和右磁导航传感器用于AGV在X方向行进时采集导航路径信息。

基于同一构思，本发明还提供了一种上述导航系统的控制方法，包括以下步骤：

S1、在Y方向的启动点位置，给定AGV的Y方向速度Vy，并通过导航纠偏方法使AGV沿导航路径行进。

S2、当RFID传感器检测到表示加速的RFID位置码时，使Vy的值以线性方式增大至预设的第一目标值，以保证稳定性。

S3、当RFID传感器检测到表示减速的RFID位置码时，使Vy的值以线性方式减小至预设的第二目标值，以保证稳定性。

S4、当RFID传感器检测到表示停止的RFID位置码时，使Vy的值以线性方式减小至零；并通过磁地标传感器检测定位磁地标对AGV进行定位，以提高定位的精确度。

S5、当RFID传感器检测到表示变向的RFID位置码时，使Vy的值以线性方式减小至零；并使各差速轮组旋转90°后，给定AGV的X方向速度Vx。这一转向方式降低了控制难度，节省了转向空间，提高了转向的稳定性。在实际应用中，第一目标值和第二目标值应根据AGV的行进距离和具体需要设置。

作为优化方案，上述导航纠偏方法包括以下步骤：

S101、通过前磁导航传感器和后磁导航传感器实时采集导航路径信息，通过绝对值编码器实时检测各差速轮组的当前航向角。

S102、根据前磁导航传感器和后磁导航传感器的位置，计算AGV中心相对导航路径的偏移量DP和航向偏角AP，航向偏角AP是指Vy方向与导航路径之间形成的夹角。

S103、根据偏移量DP和航向偏角AP计算纠偏速度Vx_j和纠偏转速w，w是指AGV绕中心的旋转速度。

S104、采用速度分解法将Vy、Vx_j和w分解到各差速轮组，以得到各差速轮组的目标速度和目标航向角。

S105、根据各自的目标速度、目标航向角计算各差速轮组驱动轮的目标角速度。

S106、根据各差速轮组驱动轮的目标角速度控制对应驱动电机的转速。

这一导航纠偏方法在AGV以给定的速度Vy行进时，通过前后磁导航传感器实时采集导航路径信息，通过绝对值编码器实时检测各差速轮组的当前航向角，并根据前后磁导航传感器的位置依次计算偏移量DP、航向偏角AP、纠偏速度Vx_j、纠偏转速w、各差速轮组的目标速度和目标航向角及其驱动轮的目标角速度，让控制系统根据各差速轮组驱动轮的目标角速度控制对应驱动电机的转速，即可使Vy方向与导航路径保持一致，从而实现导航纠偏目的，具有流程简单、执行速度快、纠偏精度高、稳定可靠的优点。

作为具体实施方式，在上述步骤S102中，所述偏移量DP和航向偏角AP按以下公式计算获得，

DP＝FCV-BCV (1)

AP＝FCV+BCV-n (2)

公式(1)、(2)中，FCV为前磁导航传感器相对导航路径的位置，BCV为后磁导航传感器相对导航路径的位置，n等于磁导航传感器的探测点位数加1。

本发明通过采用FCV、BCV和n作为计算DP和AP参数，具有方法简单、效率高的特点。在实际应用中，本发明通常采用16探测点位的磁导航传感器，并使前磁导航传感器的探测点位由右至左依次排列，使后磁导航传感器的探测点位由左至右依次排列，此时，FCV表示前磁导航传感器检测到导航路径的探测点位数，BCV表示后磁导航传感器检测到导航路径的探测点位数，n取值17。需要说明的是，磁导航传感器不限于选用16探测点位的。

作为具体实施方式，在上述步骤S103中，所述纠偏速度Vx_j和纠偏转速w按以下公式计算获得，

Vx_j＝DP/DP_MAX×Max_Vx (3)

w＝AP/AP_MAX×Max_w (4)

Max_Vx＝tan(AP_MAX/180×π)×Vy (5)

Max_w＝tan(AP_MAX/180×π)×Vy/(y-x×(AP_MAX/180×π)) (6)

公式(3)、(4)、(5)、(6)中，DP_MAX为最大偏移量，取值L/2，L为磁导航传感器的长度；AP_MAX为最大航向偏角，取值3～5°；Max_Vx和Max_w为随Vy变化的可变比例因子；x和y为各差速轮组中心坐标的绝对值。

本发明通过设立最大偏移量和最大航向偏角可避免AGV脱离导航路径，通过设立随Vy变化的可变比例因子Max_Vx和Max_w，可避免因差速轮组连续摆动出现晃动，提高了导航纠偏的平顺性和可靠性。需要说明的是，AP_MAX应根据前后磁导航传感器的长度及相对位置具体取值。

作为具体实施方式，在上述步骤S104中，所述各差速轮组的目标速度和目标航向角按以下公式计算获得，

Vx_m＝Vx_j+y_m×w (7)

Vy_m＝Vy+x_m×w (8)

α_m＝atan(Vx_m/Vy_m) (9)

公式(7)、(8)、(9)中，Vx_m和Vy_m为各差速轮组的目标速度，α_m为各差速轮组的目标航向角，x_m和y_m为各差速轮组中心的坐标。

需要说明的是，各差速轮组的目标速度是指其中心的速度。公式(7)、(8)、(9)是计算各差速轮组的总体公式，具体到差速轮组个体应根据自身的坐标进行计算，以一号轮组为例，其目标速度和目标航向角的计算公式如下：

Vx_1＝Vx_j+y_1×w (7-1)

Vy_1＝Vy+x_1×w (8-1)

α_1＝atan(Vx_1/Vy_1) (9-1)

公式(7-1)、(8-1)、(9-1)中，Vx_1和Vy_1为一号轮组的目标速度，α_1为一号轮组的目标航向角，x_1和y_1为一号轮组中心的坐标。

作为具体实施方式，在上述步骤S105中，所述各差速轮组驱动轮的目标角速度按以下公式计算获得，

Speed_m＝Vy_m/r (10)

dspeed_m＝Speed_m×Scale_m (11)

Speed_m_1＝Speed_m+dspeed_m (12)

Speed_m_2＝Speed_m-dspeed_m (13)

当Δα_m＝0时，Scale_m＝0 (14)

当0<|Δα_m|≤K1时，Scale_m＝0.15～0.2 (15)

当K1<|Δα_m|<K2时，Scale_m＝0.2～0.25 (16)

公式(10)、(11)、(12)、(13)、(14)、(15)、(16)中，Speed_m为基准角速度，dspeed_m为差速值，Scale_m为差速比例因子，Speed_m_1和Speed_m_2为两驱动轮的目标角速度，车头偏左时，左驱动轮的目标角速度为Speed_m_1，右驱动轮的目标角速度为Speed_m_2，车头偏右时，左驱动轮的目标角速度为Speed_m_2，右驱动轮的目标角速度为Speed_m_1，Δα_m为目标航向角与当前航向角的差值，K1取值3.6～4.5，K2取值9.6～10.5。目标航向角和当前航向角是指差速轮组的中轴线与Y方向之间形成的夹角。

本发明通过设立差速比例因子计算各差速轮组两驱动轮的差速值及目标角速度，进一步提高了导航纠偏的稳定性。

需要指出的是，公式(10)、(11)、(12)、(13)、(14)、(15)、(16)是计算各差速轮组驱动轮目标角速度的总体公式，具体到各差速轮组应根据自身的目标速度、目标航向角和当前航向角进行计算，以一号轮组为例，其驱动轮目标角速度的计算公式如下：

Speed_1＝Vy_1/r (10-1)

dspeed_1＝Speed_1×Scale_1 (11-1)

Speed_1_1＝Speed_1+dspeed_1 (12-1)

Speed_1_2＝Speed_1-dspeed_1 (13-1)

当Δα_1＝0时，Scale_1＝0 (14-1)

当0<|Δα_1|≤K1时，Scale_1＝0.15～0.2 (15-1)

当k1<|Δα_1|<k2时，Scale_1＝0.2～0.25 (16-1)

公式(10-1)、(11-1)、(12-1)、(13-1)、(14-1)、(15-1)、(16-1)中，Speed_1为一号轮组驱动轮的基准角速度，dspeed_1为差速值，Scale_1为差速比例因子，Speed_1_1和Speed_1_2为一号轮组两驱动轮的目标角速度，车头偏左时，左驱动轮的目标角速度的Speed_1_1，右驱动轮的目标角速度的Speed_1_2，车头偏右时，左驱动轮的目标角速度的Speed_1_2，右驱动轮的目标角速度的Speed_1_1，Δα_1为一号轮组目标航向角与当前航向角的差值，K1取值3.6～4.5，K2取值9.6～10.5。

需要说明的是，上述导航纠偏方法是AGV沿Y方向行进的情况，此时给定的Vy为主速度，当AGV沿X方向行进时，应进行相应的参数转换，此时给定的Vx为主速度。

经实际应用表示，本发明可产生以下有益效果：解决了差速轮组AGV的自动导航问题，不仅能满足差速轮组AGV的功能需求，还填补了差速轮组AGV导航系统的空白；与传统的AGV导航相比，实现了差速轮组AGV的自动导航和精确定位，提高了差速轮组AGV的自动化程度，拓宽了差速轮组AGV的适用领域。

以上实施例仅是对本发明的优选实施方式进行的描述，并非对本发明请求保护范围进行限定，在不脱离本发明设计构思的前提下，本领域技术人员依据本发明的技术方案做出的各种变形，均应落入本发明的权利要求书确定的保护范围内。

Claims (10)

1.一种基于差速轮组AGV的导航系统，包括导航路径和AGV，其特征在于，所述导航路径为设置在地面的导航磁条，所述AGV设有四个呈中心对称分布的差速轮组和四个呈中心对称分布的磁导航传感器，所述差速轮组设有两个对称分布的驱动轮，所述磁导航传感器用于采集导航路径信息。

2.根据权利要求1所述的一种基于差速轮组AGV的导航系统，其特征在于，所述导航路径的一侧设有用于定位AGV的定位磁地标，导航路径的另一侧设有RFID位置码；所述AGV上设有对应识别定位磁地标和RFID位置码的磁地标传感器和RFID传感器。

3.根据权利要求2所述的一种基于差速轮组AGV的导航系统，其特征在于，所述AGV的前后侧中部和左右侧中部分别设有激光扫描仪，激光扫描仪用于识别导航路径上的障碍物并触发保护指令。

4.根据权利要求3所述的一种基于差速轮组AGV的导航系统，其特征在于，所述差速轮组安装有绝对值编码器，绝对值编码器用于检测差速轮组的航向角。

5.根据权利要求4所述的一种基于差速轮组AGV的导航系统，其特征在于，所述RFID位置码设置在导航路径的节点位置，节点位置为启动点位置、停止点位置、加速点位置、减速点位置或变向点位置。

6.根据权利要求5所述的一种基于差速轮组AGV的导航系统，其特征在于，所述AGV以自身坐标系为车身坐标系，车中心为原点，正前方为Y方向，正右方为X方向；所述四个差速轮组对应设置在AGV的四个角处，右前方的差速轮组为一号轮组，其余的差速轮组按逆时针依次为二号轮组、三号轮组和四号轮组。

7.根据权利要求6所述的一种基于差速轮组AGV的导航系统，其特征在于，所述四个磁导航传感器分别为前磁导航传感器、后磁导航传感器、左磁导航传感器和右磁导航传感器；前磁导航传感器和后磁导航传感器用于AGV在Y方向行进时采集导航路径信息，左磁导航传感器和右磁导航传感器用于AGV在X方向行进时采集导航路径信息。

8.一种权利要求7所述导航系统的控制方法，其特征在于，包括以下步骤：

S1、在Y方向的启动点位置，给定AGV的Y方向速度Vy，并通过导航纠偏方法使AGV沿导航路径行进；

S2、当RFID传感器检测到表示加速的RFID位置码时，使Vy的值以线性方式增大至预设的第一目标值；

S3、当RFID传感器检测到表示减速的RFID位置码时，使Vy的值以线性方式减小至预设的第二目标值；

S4、当RFID传感器检测到表示停止的RFID位置码时，使Vy的值以线性方式减小至零；并通过磁地标传感器检测定位磁地标对AGV进行定位；

S5、当RFID传感器检测到表示变向的RFID位置码时，使Vy的值以线性方式减小至零；并使各差速轮组旋转90°后，给定AGV的X方向速度Vx。

9.根据权利要求8所述的导航系统的控制方法，其特征在于，所述导航纠偏方法包括以下步骤：

S101、通过前磁导航传感器和后磁导航传感器实时采集导航路径信息，通过绝对值编码器实时检测各差速轮组的当前航向角；

S102、根据前磁导航传感器和后磁导航传感器的位置，计算AGV中心相对导航路径的偏移量DP和航向偏角AP，航向偏角AP是指Vy方向与导航路径之间形成的夹角；

S103、根据偏移量DP和航向偏角AP计算纠偏速度Vx_j和纠偏转速w，w是指AGV绕中心的旋转速度；

S104、采用速度分解法将Vy、Vx_j和w分解到各差速轮组，以得到各差速轮组的目标速度和目标航向角；

S105、根据各自的目标速度、目标航向角计算各差速轮组驱动轮的目标角速度；

S106、根据各差速轮组驱动轮的目标角速度控制对应驱动电机的转速。

10.根据权利要求9所述的导航系统的控制方法，其特征在于，

在上述步骤S102中，所述偏移量DP和航向偏角AP按以下公式计算获得，

DP＝FCV-BCV (1)

AP＝FCV+BCV-n (2)

公式(1)、(2)中，FCV为前磁导航传感器相对导航路径的位置，BCV为后磁导航传感器相对导航路径的位置，n等于磁导航传感器的探测点位数加1；

在上述步骤S103中，所述纠偏速度Vx_j和纠偏转速w按以下公式计算获得，

Vx_j＝DP/DP_MAX×Max_Vx (3)

w＝AP/AP_MAX×Max_w (4)

Max_Vx＝tan(AP_MAX/180×π)×Vy (5)

Max_w＝tan(AP_MAX/180×π)×Vy/(y-x×(AP_MAX/180×π)) (6)

公式(3)、(4)、(5)、(6)中，DP_MAX为最大偏移量，取值L/2，L为磁导航传感器的长度；AP_MAX为最大航向偏角，取值3～5°；Max_Vx和Max_w为随Vy变化的可变比例因子；x和y为各差速轮组中心坐标的绝对值；

在上述步骤S104中，所述各差速轮组的目标速度和目标航向角按以下公式计算获得，

Vx_m＝Vx_j+y_m×w (7)

Vy_m＝Vy+x_m×w (8)

α_m＝atan(Vx_m/Vy_m) (9)

公式(7)、(8)、(9)中，Vx_m和Vy_m为各差速轮组的目标速度，α_m为各差速轮组的目标航向角，x_m和y_m为各差速轮组中心的坐标；

在上述步骤S105中，所述各差速轮组驱动轮的目标角速度按以下公式计算获得，

Speed_m＝Vy_m/r (10)

dspeed_m＝Speed_m×Scale_m (11)

Speed_m_1＝Speed_m+dspeed_m (12)

Speed_m_2＝Speed_m-dspeed_m (13)

当Δα_m＝0时，Scale_m＝0 (14)

当0<|Δα_m|≤K1时，Scale_m＝0.15～0.2 (15)

当K1<|Δα_m|<K2时，Scale_m＝0.2～0.25 (16)

公式(10)、(11)、(12)、(13)、(14)、(15)、(16)中，Speed_m为基准角速度，dspeed_m为差速值，Scale_m为差速比例因子，Speed_m_1和Speed_m_2为两驱动轮的目标角速度，车头偏左时，左驱动轮的目标角速度的Speed_m_1，右驱动轮的目标角速度的Speed_m_2，车头偏右时，左驱动轮的目标角速度的Speed_m_2，右驱动轮的目标角速度的Speed_m_1，Δα_m为目标航向角与当前航向角的差值，K1取值3.6～4.5，K2取值9.6～10.5。

Images