基于叉车式AGV运动学模型的取货定位方法及系统与流程

基于叉车式agv运动学模型的取货定位方法及系统
技术领域
1.本发明涉及一种基于叉车式agv运动学模型的取货定位方法及系统，属于移动机器人技术领域。


背景技术：

2.当前移动机器人技术发展较快，叉车底层运动定位的方法是agv中一项重要的技术，主要通过建立数学模型，采用一些数学方法解决叉车式agv移动到目的点位置不准的问题，这个问题主要由于叉车舵轮反应不够快，具有延时性的特点以及地面不平导致。从硬件上很难再提高叉车舵轮性能。


技术实现要素：

3.本发明所要解决的技术问题是克服现有技术的缺陷，提供一种基于叉车式agv运动学模型的取货定位方法及系统。
4.为解决上述技术问题，本发明提供一种基于叉车式agv运动学模型的取货定位方法，
5.1)获取当前叉车的运动学模型数据，调取预先以该叉车的运动学模型数据确定的试验数据库；所述试验数据库包括若干个位于修正点的修正点数据库和若干个位于转弯目标点的转弯点数据库，每个数据库包括若干组车身角度、横向偏差、舵轮角度的数据；相邻转弯目标点之间以及最后一个转弯目标点与取货目标点之间至少设有一个修正数据库；
6.2)采集叉车位于某个转弯目标点的叉车的车身角度和横向偏差，与该转弯目标点的转弯点数据库进行比较，获取该转弯目标点的转弯点数据库中与所述叉车的车身角度和横向偏差的误差均在预先设定的第一误差内的一组数据，若存在多组数据满足条件，则将每一组数据中的车身角度和横向偏差的误差相加，确定误差较小的那组数据为最终一组数据，根据最终一组数据中的舵轮角度控制叉车进行前叉行驶；
7.3)在叉车行驶预先设定的距离后，采集当前位置叉车的车身角度和横向偏差，使用前一个位置的数据库的数据作一次判断，如果车身角度的误差和横向偏差的误差均小于预选设定的第二误差时，则继续行驶，
8.否则，将当前位置叉车的车身角度和横向偏差与该修正点的修正点数据库进行比较，获取该修正点的修正点数据库中与当前位置叉车的车身角度和横向偏差的误差均在预先设定的第二误差内的一组数据，若存在多组数据满足条件，则将每一组数据中的车身角度和横向偏差的误差相加，确定误差较小的那组数据为最终一组数据，根据最终一组数据中的舵轮角度控制叉车进行前叉行驶；
9.4)重复步骤3)直至叉车行驶到下一个转弯目标点；
10.5)重复步骤2)至步骤4)直至叉车行驶到取货目标点。
11.进一步的，所述预先以该叉车的运动学模型数据确定的试验数据库的确定过程包括：
12.建立当前叉车的运动学模型：
13.x^2+y^2＝1,0＜x＜1,0＜y＜1
14.其中，x表示仓库地图坐标系下横坐标,y表示仓库地图坐标系下纵坐标；
15.根据叉车的运动学模型，推导出叉车到达转弯目标点的横向偏差：
16.en
′
＝1
‑
sin(beta)
17.其中，beta表示车身角度；
18.在叉车横向预留横向误差a，则当前横向偏差为：
19.en＝en
′
+a
20.其中，en
′
为叉车转弯目的点的横向偏差；
21.进行至少一百次转弯叉车前叉到取货目标点的试验，在转弯目标点处，采集至少一百组车身角度、横向偏差、舵轮角度的数据，再在转弯目标点和取货目标点之间预设的距离处，采集至少一百组车身角度、横向偏差、舵轮角度的数据，建立修正点数据库和转弯点数据库。
22.进一步的，所述第一误差中的车身角度的误差值大于第二误差中的车身角度的误差值；所述第一误差中的横向偏差的误差的值大于第二误差中的横向偏差的误差值。
23.一种基于叉车式agv运动学模型的取货定位系统，
24.调取模块，用于获取当前叉车的运动学模型数据，调取预先以该叉车的运动学模型数据确定的试验数据库；所述试验数据库包括若干个位于修正点的修正点数据库和若干个位于转弯目标点的转弯点数据库，每个数据库包括若干组车身角度、横向偏差、舵轮角度的数据；相邻转弯目标点之间以及最后一个转弯目标点与取货目标点之间至少设有一个修正数据库；
25.第一预测模块，用于采集叉车位于某个转弯目标点的叉车的车身角度和横向偏差，与该转弯目标点的转弯点数据库进行比较，获取该转弯目标点的转弯点数据库中与所述叉车的车身角度和横向偏差的误差均在预先设定的第一误差内的一组数据，若存在多组数据满足条件，则将每一组数据中的车身角度和横向偏差的误差相加，确定误差较小的那组数据为最终一组数据，根据最终一组数据中的舵轮角度控制叉车进行前叉行驶；
26.第二预测模块，用于在叉车行驶预先设定的距离后，采集当前位置叉车的车身角度和横向偏差，使用前一个位置的数据库的数据作一次判断，如果车身角度的误差和横向偏差的误差均小于预选设定的第二误差时，则继续行驶，
27.否则，将当前位置叉车的车身角度和横向偏差与该修正点的修正点数据库进行比较，获取该修正点的修正点数据库中与当前位置叉车的车身角度和横向偏差的误差均在预先设定的第二误差内的一组数据，若存在多组数据满足条件，则将每一组数据中的车身角度和横向偏差的误差相加，确定误差较小的那组数据为最终一组数据，根据最终一组数据中的舵轮角度控制叉车进行前叉行驶；
28.第一控制模块，用于控制第二预测模块进行工作，直至叉车行驶到下一个转弯目标点；
29.第二控制模块，用于控制第一预测模块和第二预测模块工作，直至叉车行驶到取货目标点。
30.进一步的，所述调取模块包括：
31.构建模块，用于建立当前叉车的运动学模型：
32.x^2+y^2＝1,0＜x＜1,0＜y＜1
33.其中，x表示仓库地图坐标系下横坐标,y表示仓库地图坐标系下纵坐标；
34.推到模块，用于根据叉车的运动学模型，推导出叉车到达转弯目标点的横向偏差：
35.en
′
＝1
‑
sin(beta)
36.其中，beta表示车身角度；
37.在叉车横向预留横向误差a，则当前横向偏差为：
38.en＝en
′
+a
39.其中，en
′
为叉车转弯目的点的横向偏差；
40.试验模块，用于进行至少一百次转弯叉车前叉到取货目标点的试验，在转弯目标点处，采集至少一百组车身角度、横向偏差、舵轮角度的数据，再在转弯目标点和取货目标点之间预设的距离处，采集至少一百组车身角度、横向偏差、舵轮角度的数据，建立修正点数据库和转弯点数据库。
41.进一步的，所述第一误差中的车身角度的误差值大于第二误差中的车身角度的误差值；所述第一误差中的横向偏差的误差的值大于第二误差中的横向偏差的误差值。
42.本发明所达到的有益效果：
43.本发明在叉车转弯到转弯终点时，预留一定的横向偏差，然后根据横向误差、车身角度和舵轮角度建立数据库，从而预测需要给定的舵轮角度，使叉车能够准确行驶到取货目的点。在叉车行驶到转弯目标点时以及在叉车行驶一段路程时均建立有数据库，进行预测，共计至少两次预测舵轮角度，确保叉车行驶到取货目的点误差较小。
具体实施方式
44.为使得本发明的发明目的、特征、优点能够更加的明显和易懂，下面对本发明实施例中的技术方案进行清楚、完整地描述，显然，下面所描述的实施例仅仅是本发明一部分实施例，而非全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其它实施例，都属于本发明保护的范围。
45.一种基于叉车式agv运动学模型的取货定位方法，目的是根据采集并输入的车身角度beta和横向偏差en，输出舵轮角度theta，控制叉车进行前叉形式，准确到达取货目标点，本实施例中以一个转弯目标点和一个修正点为例，但不限于只有一个转弯目标点和一个修正点，具体如下：
46.包括如下步骤：
47.步骤1：建立叉车的转弯运动学模型。
48.x^2+y^2＝1,0＜x＜1,0＜y＜1
49.其中，x表示仓库地图坐标系下横坐标,y表示仓库地图坐标系下纵坐标；
50.步骤2：由叉车运动学模型，推导出叉车到达转弯目标点的横向偏差。
51.en
′
＝1
‑
sin(beta)
52.其中，beta表示车身角度；
53.en＝1
‑
sin(beta)
54.步骤3：在叉车横向预留3cm的横向误差，即叉车转弯到达目的转弯点，目标转弯点
和目标取货点的横向误差为3cm。则当前横向偏差为：
55.en＝en'+3
56.其中en’为叉车转弯目的点的横向偏差。
57.步骤4：进行一百次转弯前叉到取货点的试验，在转弯目标点处，采集一百组车身角度、横向偏差、舵轮角度的数据，再在转弯目标点和取货目标点之间一半距离处，采集一百组车身角度、横向偏差、舵轮角度的数据，建立两个数据库。
[0058][0059]
步骤5：进行第一次预测时，叉车位置为转弯目标点位置，采集当前车身角度和横向偏差，遍历数据库一中车身角度与横向偏差，当采集的数据与数据库中的数据组相比，车身角度误差(errbeta)小于0.2度、横向偏差误差(erren)小于2mm时，则取出数据库一中该组数据，预测舵轮角度为该组数据的舵轮角度。如果存在多组数据满足条件，则将两项误差相加，取误差较小的那组数据。
[0060]
n
→0…
100
[0061]
errbeta＝beta
‑
beta1n
[0062]
erren＝en
‑
en1n
[0063]
if errbeta<0.2&erren<2
[0064]
min(errbeta+erren)
→
theta1n
[0065]
其中beta1n为数据库一中车身角度某个数据，en1n为数据库一中横向偏差某个数据，beta为转弯目标点的车身角度，en为转弯目标点的横向偏差。
[0066]
步骤6：使用数据库一预测的舵轮角度进行前叉行驶，行驶到一半距离时，根据第一次预测选取的那组数据作一次判断。如果车身角度误差(errbeta)小于0.15度、横向偏差误差(erren)小于1.5mm时，则继续行驶，否则进入下一步骤。
[0067]
errbeta＝beta
‑
beta2i
[0068]
erren＝en
‑
en2i
[0069]
if errbeta<0.15&erren<1.5
[0070]
continue
[0071]
else next step
[0072]
步骤7：进行第二次预测时，叉车为转弯目标点到叉车取货点的中间位置，采集当前车身角度和横向偏差，遍历数据库二中车身角度与横向偏差，当采集的数据与数据库中的数据组相比，车身角度误差(errbeta)小于0.15度、横向偏差误差(erren)小于1.5mm时，则取出数据库二中该组数据，预测舵轮角度为该组数据的舵轮角度。如果存在多组数据满足条件，则将两项误差相加，取误差较小的那组数据。
[0073]
n
→0…
100
[0074]
errbeta＝beta
‑
beta2n
[0075]
erren＝en
‑
en2n
[0076]
if errbeta<0.15&erren<1.5
[0077]
min(errbeta+erren)
→
theta2n
[0078]
其中beta2n为数据库二中车身角度某个数据，en2n为数据库二中横向偏差某个数据，beta为转弯目标点到叉车取货点中间位置的车身角度，en为转弯目标点到叉车取货点中间位置的横向偏差。
[0079]
步骤8：使用数据库二预测的舵轮角度进行前叉行驶，到达取货目标点。
[0080]
相应的本发明还提供了一种基于叉车式agv运动学模型的取货定位系统，
[0081]
调取模块，用于获取当前叉车的运动学模型数据，调取预先以该叉车的运动学模型数据确定的试验数据库；所述试验数据库包括若干个位于修正点的修正点数据库和若干个位于转弯目标点的转弯点数据库，每个数据库包括若干组车身角度、横向偏差、舵轮角度的数据；相邻转弯目标点之间以及最后一个转弯目标点与取货目标点之间至少设有一个修正数据库；
[0082]
第一预测模块，用于采集叉车位于某个转弯目标点的叉车的车身角度和横向偏差，与该转弯目标点的转弯点数据库进行比较，获取该转弯目标点的转弯点数据库中与所述叉车的车身角度和横向偏差的误差均在预先设定的第一误差内的一组数据，若存在多组数据满足条件，则将每一组数据中的车身角度和横向偏差的误差相加，确定误差较小的那组数据为最终一组数据，根据最终一组数据中的舵轮角度控制叉车进行前叉行驶；
[0083]
第二预测模块，用于在叉车行驶预先设定的距离后，采集当前位置叉车的车身角度和横向偏差，使用前一个位置的数据库的数据作一次判断，如果车身角度的误差和横向偏差的误差均小于预选设定的第二误差时，则继续行驶，
[0084]
否则，将当前位置叉车的车身角度和横向偏差与该修正点的修正点数据库进行比较，获取该修正点的修正点数据库中与当前位置叉车的车身角度和横向偏差的误差均在预先设定的第二误差内的一组数据，若存在多组数据满足条件，则将每一组数据中的车身角度和横向偏差的误差相加，确定误差较小的那组数据为最终一组数据，根据最终一组数据中的舵轮角度控制叉车进行前叉行驶；
[0085]
第一控制模块，用于控制第二预测模块进行工作，直至叉车行驶到下一个转弯目标点；
[0086]
第二控制模块，用于控制第一预测模块和第二预测模块工作，直至叉车行驶到取货目标点。
[0087]
所述调取模块包括：
[0088]
构建模块，用于建立当前叉车的运动学模型：
[0089]
x^2+y^2＝1,0＜x＜1,0＜y＜1
[0090]
推到模块，用于根据叉车的运动学模型，推导出叉车到达转弯目标点的横向偏差：
[0091]
en
′
＝1
‑
sin(beta)
[0092]
其中，beta表示车身角度；
[0093]
在叉车横向预留横向误差a，则当前横向偏差为：
[0094]
en＝en
′
+a
[0095]
其中，en
′
为叉车转弯目的点的横向偏差；
[0096]
试验模块，用于进行至少一百次转弯叉车前叉到取货目标点的试验，在转弯目标点处，采集至少一百组车身角度、横向偏差、舵轮角度的数据，再在转弯目标点和取货目标点之间预设的距离处，采集至少一百组车身角度、横向偏差、舵轮角度的数据，建立修正点数据库和转弯点数据库。
[0097]
所述第一误差中的车身角度的误差值大于第二误差中的车身角度的误差值；所述第一误差中的横向偏差的误差的值大于第二误差中的横向偏差的误差值。
[0098]
以上所述，以上实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围。