一种基于差速轮组AGV的导航纠偏方法

摘要

本发明涉及一种基于差速轮组AGV的导航纠偏方法，包括以下步骤：给定AGV的Y方向速度Vy；通过前磁导航传感器和后磁导航传感器实时采集导航磁条信息，通过绝对值编码器实时检测各差速轮组的当前航向角；根据前磁导航传感器和后磁导航传感器的位置，计算AGV中心相对导航磁条的偏移量DP和航向偏角AP；根据DP和AP计算纠偏速度Vx和纠偏转速w；根据Vy、Vx和w计算各差速轮组的目标速度及目标航向角；根据各自的目标速度、目标航向角和当前航向角计算各差速轮组左驱动轮和右驱动轮的目标角速度；根据各差速轮组左驱动轮和右驱动轮的目标角速度控制对应驱动电机的转速。其具有流程简单、执行速度快、纠偏精度高、稳定可靠的优点。

权利要求

1.一种基于差速轮组AGV的导航纠偏方法，所述AGV安装有四个呈中心对称分布的差速轮组，差速轮组设有对称分布的左驱动轮和右驱动轮，AGV还安装有前后对称分布的前磁导航传感器和后磁导航传感器，其特征在于，所述导航纠偏方法包括以下步骤：S1、以AGV自身坐标系为车身坐标系，车中心为原点，正前方为Y方向，正右方为X方向；

给定AGV的Y方向速度Vy；

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

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

S4、根据偏移量DP和航向偏角AP计算纠偏速度Vx和纠偏转速w，Vx方向与Vy方向垂直，w是指AGV绕中心的旋转速度；

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

S6、根据各自的目标速度、目标航向角和当前航向角计算各差速轮组左驱动轮和右驱动轮的目标角速度；

S7、根据各差速轮组左驱动轮和右驱动轮的目标角速度控制对应驱动电机的转速；

步骤S4中，所述纠偏速度Vx和纠偏转速w按以下公式计算获得，Vx＝DP/DP_MAX×Max_Vx             (1)w＝AP/AP_MAX×Max_w             (2)公式(1)、(2)中，DP_MAX为最大偏移量，AP_MAX为最大航向偏角，Max_Vx和Max_w为根据Vy设立的可变比例因子；

步骤S5中，所述各差速轮组的目标速度和目标航向角按以下公式计算获得，Vx_m＝Vx+y_m×w              (5)Vy_m＝Vy+x_m×w              (6)α_m＝atan(Vx_m/Vy_m)             (7)公式(5)、(6)、(7)中，Vx_m和Vy_m为各差速轮组的目标速度，α_m为各差速轮组的目标航向角，x_m和y_m为各差速轮组中心的坐标；

步骤S6中，所述各差速轮组左驱动轮和右驱动轮的目标角速度按以下公式计算获得，Speed_m＝Vy_m/r                                              (8)dspeed_m＝Speed_m×Scale_m                                   (9)Speed_m_1＝Speed_m+dspeed_m                                 (10)Speed_m_2＝Speed_m‑dspeed_m                                 (11)公式(8)、(9)、(10)、(11)中，Speed_m为基准角速度，dspeed_m为差速值，Scale_m为差速比例因子，Speed_m_1和Speed_m_2为目标角速度，当AGV头部偏左时，左驱动轮的目标角速度为Speed_m_1，右驱动轮的目标角速度为Speed_m_2，当AGV头偏右时，左驱动轮的目标角速度为Speed_m_2，右驱动轮的目标角速度为Speed_m_1。

2.根据权利要求1所述的一种基于差速轮组AGV的导航纠偏方法，其特征在于，所述Max_Vx和Max_w按以下公式计算获得，Max_Vx＝tan(AP_MAX/180×π)×Vy                              (3)Max_w＝tan(AP_MAX/180×π)×Vy/(y‑x×(AP_MAX/180×π))     (4)公式(4)中，x和y为各差速轮组中心坐标的绝对值。

3.根据权利要求2所述的一种基于差速轮组AGV的导航纠偏方法，其特征在于，所述DP_MAX为L/2，L为磁导航传感器的长度，所述AP_MAX为3～5°。

4.根据权利要求1所述的一种基于差速轮组AGV的导航纠偏方法，其特征在于，所述Scale_m按以下方式获得，

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

3.6～4.5，K2＝9.6～11。

5.根据权利要求1所述的一种基于差速轮组AGV的导航纠偏方法，其特征在于，在上述步骤S3中，所述偏移量DP和航向偏角AP按以下公式计算获得，DP＝FCenter_Value‑BCenter_value                               (15)AP＝FCenter_Value+BCenter_value‑n‑1                         (16)公式(15)、(16)中，FCenter_Value为前磁导航传感器相对导航磁条的位置，BCenter_value为后磁导航传感器相对导航磁条的位置，n为前磁导航传感器和后磁导航传感器的探测点位数。

说明书

一种基于差速轮组AGV的导航纠偏方法

技术领域

[0001] 本发明涉及一种AGV导航系统，具体涉及一种基于差速轮组AGV的导航纠偏方法。

背景技术

[0002] 随着科技和社会的发展，AGV(自动引导运输车)得到了快速发展和广泛应用。采用麦克纳姆轮的AGV具有全向移动、控制方便的优点，但也存在着载荷小的缺陷。而差速轮组
AGV具有载荷大的特点，通常应用在航空、航天等重载运输领域，但传统的遥控操作方式影
响了差速轮组AGV的效能，基于麦克纳姆轮AGV的导航系统或纠偏方法也不适用不能连续全
向运动的差速轮组AGV，因此，亟需研发一种适用差速轮组AGV的导航纠偏系统或方法，以提
高其自动化程度和适用范围。

发明内容

[0003] 本发明的目的是提供一种基于差速轮组AGV的导航纠偏方法，其具有流程简单、执行速度快、纠偏精度高、稳定可靠的优点。

[0004] 为解决现有技术中存在的上述问题，本发明提供了一种基于差速轮组AGV的导航纠偏方法，所述AGV安装有四个呈中心对称分布的差速轮组，差速轮组设有对称分布的左驱
动轮和右驱动轮，AGV还安装有前后对称分布的前磁导航传感器和后磁导航传感器，其特征
在于，所述导航纠偏方法包括以下步骤：

[0005] S1、以AGV自身坐标系为车身坐标系，车中心为原点，正前方为Y方向，正右方为X方向；给定AGV的Y方向速度Vy；

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

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

[0008] S4、根据偏移量DP和航向偏角AP计算纠偏速度Vx和纠偏转速w，Vx方向与Vy方向垂直，w是指AGV绕中心的旋转速度；

[0009] S5、采用速度分解法将Vy、Vx和w分解到各差速轮组，以得到各差速轮组的目标速度和目标航向角；

[0010] S6、根据各自的目标速度、目标航向角和当前航向角计算各差速轮组左驱动轮和右驱动轮的目标角速度；

[0011] S7、根据各差速轮组左驱动轮和右驱动轮的目标角速度控制对应驱动电机的转速。

[0012] 进一步的，本发明一种基于差速轮组AGV的导航纠偏方法，其中，在上述步骤S4中，所述纠偏速度Vx和纠偏转速w按以下公式计算获得，

[0013] Vx＝DP/DP_MAX×Max_Vx  (1)

[0014] w＝AP/AP_MAX×Max_w  (2)

[0015] 公式(1)、(2)中，DP_MAX为最大偏移量，AP_MAX为最大航向偏角，Max_Vx和Max_w为根据Vy设立的可变比例因子。

[0016] 进一步的，本发明一种基于差速轮组AGV的导航纠偏方法，其中，所述Max_Vx和Max_w按以下公式计算获得，

[0017] Max_Vx＝tan(AP_MAX/180×π)×Vy  (3)

[0018] Max_w＝tan(AP_MAX/180×π)×Vy/(y‑X×(AP_MAX/180×π))  (4)

[0019] 公式(4)中，x和y为各差速轮组中心坐标的绝对值。

[0020] 进一步的，本发明一种基于差速轮组AGV的导航纠偏方法，其中，所述DP_MAX为L/2，L为磁导航传感器的长度，所述AP_MAX为3～5°。

[0021] 进一步的，本发明一种基于差速轮组AGV的导航纠偏方法，其中，在上述步骤S5中，所述各差速轮组的目标速度和目标航向角按以下公式计算获得，

[0022] Vx_m＝Vx+y_m×w  (5)

[0023] Vy_m＝Vy+x_m×w  (6)

[0024] α_m＝atan(Vx_m/Vy_m)  (7)

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

[0026] 进一步的，本发明一种基于差速轮组AGV的导航纠偏方法，其中，在上述步骤S6中，所述各差速轮组左驱动轮和右驱动轮的目标角速度按以下公式计算获得，

[0027] Speed_m＝Vy_m/r  (8)

[0028] dspeed_m＝Speed_m×Scale_m  (9)

[0029] Speed_m_1＝Speed_m+dspeed_m  (10)

[0030] Speed_m_2＝Speed_m‑dspeed_m  (11)

[0031] 公式(8)、(9)、(10)、(11)中，Speed_m为基准角速度，dspeed_m为差速值，Scale_m为差速比例因子，Speed_m_1和Speed_m_2为目标角速度，当AGV头部偏左时，左驱动轮的目
标角速度为Speed_m_1，右驱动轮的目标角速度为Speed_m_2，当AGV头部偏右时，左驱动轮
的目标角速度为Speed_m_2，右驱动轮的目标角速度为Speed_m_1。

[0032] 进一步的，本发明一种基于差速轮组AGV的导航纠偏方法，其中，所述Scale_m按以下方式获得，

[0033] 当Δαt_m＝0时，Scale_m＝0  (12)

[0034] 当0＜|Δα_m|≤K1时，Scale_m＝0.15～0.2  (13)

[0035] 当K1＜|Δα_m|＜K2时，Scale_m＝0.2～0.25  (14)

[0036] 公式(12)、(13)、(14)中，Δα_m为各差速轮组目标航向角与当前航向角的差值，K1＝3.6～4.5，K2＝9.6～11。

[0037] 进一步的，本发明一种基于差速轮组AGV的导航纠偏方法，其中，在上述步骤S3中，所述偏移量DP和航向偏角AP按以下公式计算获得，

[0038] DP＝FCenter_Value‑BCenter_value  (15)

[0039] AP＝FCenter_Value+BCenter_value‑n‑1  (16)

[0040] 公式(15)、(16)中，FCenter_Value为前磁导航传感器相对导航磁条的位置，BCenter_value为后磁导航传感器相对导航磁条的位置，n为前磁导航传感器和后磁导航传
感器的探测点位数。

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

[0042] 下面结合附图所示具体实施方式对本发明一种基于差速轮组AGV的导航纠偏方法作进一步详细说明。

附图说明

[0043] 图1为差速轮组AGV的示意图；

[0044] 图2为本发明一种基于差速轮组AGV的导航纠偏方法的流程图。

具体实施方式

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

[0046] 如图1和图2所示本发明一种基于差速轮组AGV的导航纠偏方法的具体实施方式，AGV安装有四个呈中心对称分布的差速轮组1，差速轮组1设有对称分布的左驱动轮11和右
驱动轮12，AGV还安装有前后对称分布的前磁导航传感器2和后磁导航传感器3，导航纠偏方
法包括以下步骤：

[0047] S1、以AGV自身坐标系为车身坐标系，车中心为原点，正前方为Y方向，正右方为X方向；给定AGV的Y方向速度Vy。

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

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

[0050] S4、根据偏移量DP和偏移量AP计算纠偏速度Vx和纠偏转速w，Vx方向与Vy方向垂直，w是指AGV绕中心的旋转速度。

[0051] S5、采用速度分解法将Vy、Vx和w分解到各差速轮组，以得到各差速轮组的目标速度和目标航向角。

[0052] S6、根据各自的目标速度、目标航向角和当前航向角计算各差速轮组左驱动轮和右驱动轮的目标角速度。

[0053] S7、根据各差速轮组左驱动轮和右驱动轮的目标角速度控制对应驱动电机的转速。

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

[0055] 作为具体实施方式，在上述步骤S4中，本发明使纠偏速度Vx和纠偏转速w采用了以下公式计算获得，

[0056] Vx＝DP/DP_MAX×Max_Vx  (1)

[0057] w＝AP/AP_MAX×Max_w  (2)

[0058] 在公式(1)、(2)中，DP_MAX为最大偏移量，AP_MAX为最大航向偏角，Max_Vx和Max_w为根据Vy设立的可变比例因子。

[0059] 本发明通过设立最大偏移量，并采用DP/DP_MAX作为计算纠偏速度Vx的主要参数，可保证AGV不会脱离导航磁条，通过乘以随Vy变化的可变比例因子Max_Vx，提高了导航纠编
的率效和稳定性；同理，本发明通过设立最大航向偏角，并采用AP/AP_MAX作为计算纠偏转
速w的主要参数，通过乘以随Vy变化的可变比例因子Max_w，进一步提高了导航纠编的率效
和稳定性。

[0060] 作为具体实施方式，根据速度与夹角的关系，本发明使Max_Vx和Max_w采用了以下公式计算获得，

[0061] Max_Vx＝tan(AP_MAX/180×π)×Vy  (3)

[0062] Max_w＝tan(AP_MAX/180×π)×Vy/(y‑x×(AP_MAX/180×π))  (4)

[0063] 在公式(4)中，x和y为各差速轮组中心坐标的绝对值。在实际应用中，DP_MAX通常采用L/2，L为磁导航传感器的长度；根据前后磁导航传感器之间的距离，AP_MAX通常取值3
～5°。通过采用公式(3)和公式(4)计算的可变比例因子，可避免AGV因差速轮组连续摆动出
现晃动，提高了导航纠偏的平顺性。

[0064] 作为具体实施方式，本发明通过采用速度分解法将AGV中心的Vy，Vx，w分解到各差速轮组，得到了各差速轮组的目标速度及目标航向角，具体分解公式如下，

[0065] Vx_m＝Vx+y_m×w  (5)

[0066] Vy_m＝Vy+x_m×w  (6)

[0067] α_m＝atan(Vx_m/Vy_m)  (7)

[0068] 在公式(5)、(6)、(7)中，Vx_m和Vy_m为各差速轮组的目标速度，α_m为各差速轮组的目标航向角，x_m和y_m为各差速轮组中心的坐标。

[0069] 需要说明的是，目标速度是指各差速轮组中心的速度。公式(5)、(6)、(7)是计算各差速轮组的总体公式，具体到差速轮组个体，应根据其自身的坐标进行计算。在实际应用
中，本发明通常将右前方的差速轮组规定为1号轮组，其余的按逆时针依次为2号轮组、3号
轮组和4号轮组。

[0070] 1号轮组的目标速度和目标航向角的具体计算公式如下：

[0071] Vx_1＝Vx+y_1×w  (5‑1)

[0072] Vy_1＝Vy+x_1×w  (6‑1)

[0073] α_1＝atan(Vx_1/Vy_1)  (7‑1)

[0074] 在公式(5‑1)、(6‑1)、(7‑1)中，Vx_1和Vy_1表示1号轮组中心的目标速度，α_1表示1号轮组的目标航向角，x_1和y_1表示1号轮组中心的坐标。

[0075] 2号轮组的目标速度和目标航向角的具体计算公式如下：

[0076] Vx_2＝Vx+y_2×w  (5‑2)

[0077] Vy_2＝Vy+x_2×w  (6‑2)

[0078] α_2＝atan(Vx_2/Vy_2)  (7‑2)

[0079] 在公式(5‑2)、(6‑2)、(7‑2)中，Vx_2和Vy_2表示2号轮组中心的目标速度，α_2表示2号轮组的目标航向角，x_2和y_2表示2号轮组中心的坐标。

[0080] 3号轮组的目标速度和目标航向角的具体计算公式如下：

[0081] Vx_3＝Vx+y_3×w  (5‑3)

[0082] Vy_3＝Vy+x_3×w  (6‑3)

[0083] α_3＝atan(Vx_3/Vy_3)  (7‑3)

[0084] 在公式(5‑3)、(6‑3)、(7‑3)中，Vx_3和Vy_3表示3号轮组中心的目标速度，α_3表示3号轮组的目标航向角，x_3和y_3表示3号轮组中心的坐标。

[0085] 4号轮组的目标速度和目标航向角的具体计算公式如下：

[0086] Vx_4＝Vx+y_4×w  (5‑4)

[0087] Vy_4＝Vy+x_4×w  (6‑4)

[0088] α_4＝atan(Vx_4/Vy_4)  (7‑4)

[0089] 在公式(5‑4)、(6‑4)、(7‑4)中，Vx_4和Vy_4表示4号轮组中心的目标速度，α_4表示4号轮组的目标航向角，x_4和y_4表示4号轮组中心的坐标。

[0090] 作为具体实施方式，在上述步骤S6中，本发明使各差速轮组左驱动轮和右驱动轮的目标角速度采用了以下公式计算获得，

[0091] Speed_m＝Vy_m/r  (8)

[0092] dspeed_m＝Speed_m×Scale_m  (9)

[0093] Speed_m_1＝Speed_m+dspeed_m  (10)

[0094] Speed_m_2＝Speed_m‑dspeed_m  (11)

[0095] 在公式(8)、(9)、(10)、(11)中，Speed_m为基准角速度，dspeed_m为差速值，Scale_m为差速比例因子，Speed_m_1和Speed_m_2为两个目标角速度，AGV头部偏左时，左驱动轮的
目标角速度为Speed_m_1，右驱动轮的目标角速度为Speed_m_2，AGV头部偏右时，左驱动轮
的目标角速度为Speed_m_2，右驱动轮的目标角速度为Speed_m_1。

[0096] 需要指出的是，公式(8)、(9)、(10)、(11)是计算各差速轮组左驱动轮和右驱动轮目标角速度的总体公式，具体到差速轮组个体，应根据其自身的目标速度、目标航向角和当
前航向角进行计算。

[0097] 1号轮组左驱动轮和右驱动轮的目标角速度的具体计算公式如下：

[0098] Speed_1＝Vy_1/r  (8‑1)

[0099] dspeed_1＝Speed_1×Scale_1  (9‑1)

[0100] Speed_1_1＝Speed_1+dspeed_1  (10‑1)

[0101] Speed_1_2＝Speed_1‑dspeed_1  (11‑1)

[0102] 在公式(8‑1)、(9‑1)、(10‑1)、(11‑1)中，Speed_1为基准角速度，dspeed_1为差速值，Scale_1为差速比例因子，Speed_1_1和Speed_1_2为两个目标角速度，AGV头部偏左时，
左驱动轮的目标角速度为Speed_1_1，右驱动轮的目标角速度为Speed_1_2，AGV头部偏右
时，左驱动轮的目标角速度为Speed_1_2，右驱动轮的目标角速度为Speed_1_1。

[0103] 2号轮组左驱动轮和右驱动轮的目标角速度的具体计算公式如下：

[0104] Speed_2＝Vy_2/r  (8‑2)

[0105] dspeed_2＝Speed_2×Scale_2  (9‑2)

[0106] Speed_2_1＝Speed_2+dspeed_2  (10‑2)

[0107] Speed_2_2＝Speed_2‑dspeed_2  (11‑2)

[0108] 在公式(8‑2)、(9‑2)、(10‑2)、(11‑2)中，Speed_2为基准角速度，dspeed_2为差速值，Scale_2为差速比例因子，Speed_2_1和Speed_2_2为两个目标角速度，AGV头部偏左时，
左驱动轮的目标角速度为Speed_2_1，右驱动轮的目标角速度为Speed_2_2，AGV头部偏右
时，左驱动轮的目标角速度为Speed_2_2，右驱动轮的目标角速度为Speed_2_1。

[0109] 3号轮组左驱动轮和右驱动轮的目标角速度的具体计算公式如下：

[0110] Speed_3＝Vy_3/r  (8‑3)

[0111] dspeed_3＝Speed_3×Scale_3  (9‑3)

[0112] Speed_3_1＝Speed_3+dspeed_3  (10‑3)

[0113] Speed_3_2＝Speed_3‑dspeed_3  (11‑3)

[0114] 在公式(8‑3)、(9‑3)、(10‑3)、(11‑3)中，Speed_3为基准角速度，dspeed_3为差速值，Scale_3为差速比例因子，Speed_3_1和Speed_3_2为两个目标角速度，AGV头部偏左时，
左驱动轮的目标角速度为Speed_3_1，右驱动轮的目标角速度为Speed_3_2，AGV头部偏右
时，左驱动轮的目标角速度为Speed_3_2，右驱动轮的目标角速度为Speed_3_1。

[0115] 4号轮组左驱动轮和右驱动轮的目标角速度的具体计算公式如下：

[0116] Speed_4＝Vy_4/r  (8‑4)

[0117] dspeed_4＝Speed_4×Scale_4  (9‑4)

[0118] Speed_4_1＝Speed_4+dspeed_4  (10‑4)

[0119] Speed_4_2＝Speed_4‑dspeed_4  (11‑4)

[0120] 在公式(8‑4)、(9‑4)、(10‑4)、(11‑4)中，Speed_4为基准角速度，dspeed_4为差速值，Scale_4为差速比例因子，Speed_4_1和Speed_4_2为两个目标角速度，AGV头部偏左时，
左驱动轮的目标角速度为Speed_4_1，右驱动轮的目标角速度为Speed_4_2，AGV头部偏右
时，左驱动轮的目标角速度为Speed_4_2，右驱动轮的目标角速度为Speed_4_1。

[0121] 本发明通过设立差速比例因子计算各差速轮组左驱动轮和右驱动轮的差速值及目标角速度，进一步增强了导航纠偏的稳定性和可靠性。

[0122] 作为具体实施方式，本发明使Scale_m采用了以下方式获得，

[0123] 当Δα_m＝0时，Scale_m＝0  (12)

[0124] 当0<|Δα_m|≤K1时，Scale_m＝0.15～0.2  (13)

[0125] 当K1<|Δα_m|

[0126] 在公式(12)、(13)、(14)中，Δα_m为各差速轮组的目标航向角与当前航向角的差值，K1＝3.6～4.5，K2＝9.6～11。

[0127] 1号轮组的差速比例因子的具体计算公式如下：

[0128] 当Δα_1＝0时，Scale_1＝0  (12‑1)

[0129] 当0<|Δα_1|≤K1时，Scale_1＝0.15～0.2  (13‑1)

[0130] 当K1<|Δα_1|

[0131] Δα_1表示1号轮组的目标航向角与当前航向角的差值。

[0132] 2号轮组的差速比例因子的具体计算公式如下：

[0133] 当Δα_2＝0时，Scale_2＝0  (12‑2)

[0134] 当0<|Δα_2|≤K1时，Scale_2＝0.15～0.2  (13‑2)

[0135] 当K1<|Δα_2|

[0136] Δα_2表示2号轮组的目标航向角与当前航向角的差值。

[0137] 3号轮组的差速比例因子的具体计算公式如下：

[0138] 当Δα_3＝0时，Scale_3＝0  (12‑3)

[0139] 当0<|Δα_3|≤K1时，Scale_3＝0.15～0.2  (13‑3)

[0140] 当K1<|Δα_3|

[0141] Δα_3表示3号轮组的目标航向角与当前航向角的差值。

[0142] 4号轮组的差速比例因子的具体计算公式如下：

[0143] 当Δα_4＝0时，Scale_4＝0  (12‑4)

[0144] 当0<|Δα_4|≤K1时，Scale_4＝0.15～0.2  (13‑4)

[0145] 当K1<|Δα_4|

[0146] Δα_4表示4号轮组的目标航向角与当前航向角的差值。

[0147] 各差速轮组的目标航向角及当前航向角是指其中轴线与Y方向之间形成的夹角。

[0148] 作为具体实施方式，在上述步骤S3中，本发明使偏移量DP和航向偏角AP采用了以下公式计算获得，

[0149] DP＝FCenter_Value‑BCenter_value  (15)

[0150] AP＝FCenter_Value+BCenter_value‑n‑1  (16)

[0151] 在公式(15)、(16)中，FCenter_Value为前磁导航传感器相对导航磁条的位置，BCenter_value为后磁导航传感器相对导航磁条的位置，n为前磁导航传感器和后磁导航传
感器的探测点位数。

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

[0153] 在实际应用中，当DP<0时，表示AGV中心偏左，当DP>0时，表示AGV中心偏右，设顺时针角度为正，当AP<0时，表示Vy方向与导航磁条之间的夹角为负角，当AP<0时，表示Vy方向
与导航磁条之间的夹角为正角。

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