一种全向AGV路径和姿态综合规划方法

一种全向agv路径和姿态综合规划方法
技术领域
1.本发明涉及的是一种路径和姿态综合规划方法，具体的说是一种全向agv的路径和姿态综合规划方法，旨在解决全向agv小车在行驶过程中的车身姿态调整问题。


背景技术：

2.近年来，随着智能技术的发展与进步，自动引导小车(automated guided vehicle)作为一种运输工具，在餐饮行业得到了广泛的应用，是实现智能餐厅的重要手段之一。
3.而全向agv作为agv小车的一种，它的优点就在于其灵活性，可以支持小车在行驶的过程中根据实际工作需求自由的前进、后退、左移、右移以及原地自旋等，可以解决普通车轮的传菜小车在工作过程中出现的一些问题。
4.比如当传菜小车上菜时，需要以客户最方便的姿态停靠在餐桌旁，使得用户随手就可以取到小车上的菜品。假设客户需要从小车两侧取菜，如果传菜小车使用的是普通车轮，那么小车只能以车头进站，如图1右图所示，此时小车车头正对餐桌，车身距离餐桌尚有一段距离，客户需要走下座位才可以取到菜品，这不仅没有方便到客户，还可能会影响到客户的体验。然而如果小车是全向agv，小车可以以图1左图所示的姿势，侧身驶向餐桌，假设菜品均匀的摆放在传菜小车上，那么坐在餐桌上的客户甚至不需要离开座位，随手就可以取到传菜小车上的菜品。
5.又如图2所示，图2是本方法设计的一个简单的餐厅地图模型，假设此时需要小车从取餐处送餐至餐桌1，也就是从站点1行驶至站点25，如果小车当前处于12站点处，且检测到12-18这一段路线发生堵塞，那么传菜小车将在12站点等待道路通畅后再继续行驶，但是如果此时恰巧后方道路7-12也发生拥塞，且有障碍物从前方靠近，前后道路都被堵死，那么此时普通车轮的传菜小车就可能会无法移动，从而陷入僵局，需要现场工作人员手动处理。而如果传菜小车是全向agv，那么此时小车就可以侧移或者原地自旋驶入站点11或者站点13躲避障碍。
6.全向agv虽然在使用过程中带来了许多便利，可以根据需求自由的改变车身姿态，但是由于小车在行驶过程中部分站点或者路段存在车身姿态限制，只允许小车以某种姿态通过，比如狭窄通道只允许小车以车身最窄的一侧平行于道路通过，这为全向agv的行驶带来了许多不便。
7.为此，本发明提出一种全向agv的路径和姿态综合规划方法，能够在路径规划的同时，根据小车在行驶过程中站点或者路段对车身姿态的限制，规划好agv小车在行驶过程中的姿态调整节点，从而帮助小车调整姿态，顺利的抵达终点。


技术实现要素：

8.本发明的目的是针对全向agv在行驶过程中遇到的车身姿态调整问题，提供一种全向agv的路径和姿态综合规划方法，可以针对在行驶过程中可能遇到的车身姿态限制条
件，规划好agv小车在行驶过程中的姿态调整节点，从而帮助小车调整姿态，顺利的抵达终点。
9.本发明的技术方案如下：
10.一种全向agv的路径和姿态综合规划方法，包括以下步骤：
11.步骤1：使用拓扑地图法构建室内地图模型。拓扑地图法就是将环境中一些重要的区域用一个个节点表示，节点之间用线段连接表示路径。其中，节点除了代表区域地理位置以外，还包含agv进站姿态、最大旋转角度等限制条件。。
12.步骤1.1：构建地图站点模型。在餐厅所有可以允许小车通过的路径上划分若干站点，通过这些站点可以将路径划分为若干个基本路段。这些站点模型至少需要包含以下信息：站点的编号，站点的位置信息，邻近站点的编号，允许小车旋转的角度，该站点对agv小车的进站车身姿态限制等要素。本方法中划分站点的依据一般是道路的分岔口或转折点以及路况发生变化路段的起点和终点。
13.步骤1.2：构建道路地图模型，将所有可以通行的道路划分为若干基本路段，用以描述餐厅道路的位置信息，车体经过该道路的车身姿态限制信息，道路狭窄情况等。假设本方法中通道宽度为width
road
，所述狭窄通道一般指公式(1)所述情况：
14.width
road
《width
car
2*width
safe
ꢀꢀꢀꢀꢀꢀ
(1)
15.其中width
car
指小车行驶过程中垂直于行驶方向的车体最宽处距离，width
safe
指小车距离道路边界的安全距离，在本发明中设置为0.5m。之所以要考虑狭窄通道，不妨假设有一段宽1.2m道路，而小车长1m，宽0.5m，在这种情况下，如果小车选择侧移，也就是车身垂直于道路方向移动，那么小车距离道路边界仅有10cm间隙，这会影响小车的安全通行，因此需要注明狭窄通道，从而限制车身姿态。
16.步骤2：使用深度优先搜索算法在各路段车身姿态约束条件下进行路径规划。由于本方法使用拓扑地图法构建地图模型，可以将拓扑地图中的站点视为图论中图的顶点，站点之间的连线也就是路线可以视为图中的边，深度优先搜索算法作为图论中常用的算法之一，可以找出两个节点之间符合要求的路径，然后计算组成该路径的各条边的距离之和，可以获得每条路径的距离，符合本方法的需要。本发明中所述的车身姿态约束条件一般指小车通过某一路段所允许的车身方向。
17.步骤2.1：计算距离矩阵，如果地图中存在n个站点，那么就事先定义一个n
·
n的二维矩阵e，e[i,j]表示站点i到站点j之间的距离，如果站点i到站点j可以不经过任何其他站点，直接抵达，那么e[i,j]就等于站点i到站点j之间的距离数值，如果站点i到站点j不可以直接抵达或者i等于j，那么e[i,j]可以等于一个极大的固定值。
[0018]
步骤2.2：输入起始站点和目标站点，从起始站点开始，沿当前站点遍历未访问过的站点，在访问前，首先判断当前站点是否允许调整车身姿态，若当前站点允许调整车身姿态就说明无论之前车身姿态如何，都可以使小车在当前站点得到调整，使得小车可以从当前站点抵达即将需要访问的站点；若不可调整车身姿态，则将之前路段允许的车身姿态集合与即将访问路段的车身姿态集合求交集，若交集为空说明小车无法抵达，放弃该站点，若交集不为空，说明小车可以抵达，访问该站点。当没有未访问过的站点时，则回到上一个站点，继续试探访问别的站点，直到所有的站点都被访问过。在访问过程中，若访问到目标站点，则将经过的所有节点记录并存入后备路径，并记录所有距离之和。访问结束后，选取距
离最短的路径作为当前路径，其余路径根据距离远近依次存入后备队列，在当前路径不可达时，依次从后备队列切换路径。
[0019]
步骤3：在道路的相关约束条件下，计算在规划路径中小车的车身姿态调整站点。本方法中所述的车身姿态只考虑直行和侧移两种，暂不考虑斜行的情况。本方法中所述直行即车身方向平行于道路方向，如图1左图所示，所示侧移即车身方向垂直于道路方向，如图1右图所示。
[0020]
步骤3.1：首先结合餐厅地图模型与规划路径，遍历途经的每一个站点，找出路径中的狭窄通道，计算传菜agv小车通过各基本路段的限制条件。其中限制条件包括：传菜小车经过该路段所允许的车身姿态，小车的进站姿态，小车在站点所能旋转的最大角度等。
[0021]
步骤3.2：然后计算在规划路径中小车的车身姿态调整站点。
[0022]
首先做一种假设，小车在不改变车身姿态的情况下，最远可以行驶多远。如图2所示，假设需要从站点2驶向充电区充电，其规划路径为2-3-4-26，从站点2开始推导，通过步骤3.1可以获得允许小车通过各个基本路段的车身姿态的集合p
end
(end表示该基本路段结束站点的标识)，将路线段2-3的车身姿态集合p3与路线段3-4的车身姿态集合p4求交集，结果为s，如果s非空，那么就说明小车能够从站点2不改变车身姿态驶向站点4，此时，再将s与路线段4-26的集合p
26
求交集，如果结果为空，就说明小车从站点2驶向站点26需要至少改变1次车身姿态，如果站点4允许小车调整车身姿态，那么允许小车调整车身姿态的站点就在站点4，如果不允许调整车身姿态，那么需要反向遍历规划路径中，站点4到站点2中的每一个站点，其中第一个允许调整车身姿态的站点就是小车在当前路段需要的调整站点。然后再以此调整车身姿态站点为当前新的起点，重复上述过程，直到遍历完所有站点，就可以获得所有调整车身姿态站点，如果所有站点均不可调整车身姿态，就说明该路径不可达，跳出步骤3，切换路线。本实施例中所述车身姿态集合，如图2所示，以地图正上方所对应实际方向为基准，设为v，假设此时小车需要从7站点驶向24站点，这是一条在地图中自下向上的道路，假设这段道路允许小车纵移与侧移，那么这段道路在地图中允许的车身姿态集合为{v,u,-u,-v}，其中v表示允许小车车头在地图中向上，在这段道路中也就是向前直行，u表示允许小车车头在地图中向右，在这段道路中也就是侧移，-u表示允许小车车头在地图中向左，在这段道路中也是侧移，-v表示允许小车车头在地图中向下，在这段道路中也就是倒车行驶，其中v,u,-u,-v，并非要求小车车头始终保持该角度，而是允许小车在不偏离道路的情况下，以该方向为基准，调整方向。
[0023]
步骤4：根据车身姿态调整站点划分路段，计算各路段车身姿态。
[0024]
以每一个车身姿态调整站点作为分割点，将整个路径划分成若干段，每一段路线记为roadi(i为每一段路线的序号)；每一段路线roadi又由若干基本路段组成，将roadi中每个基本路段允许的车身姿态集合求交集，得到集合statusi，statusi就是允许小车通过路线roadi的车身姿态集合，如果允许的车身姿态不唯一，可以从statusi中选择一个最适合当前行进方向的车身姿态，若statusi为说明当前路径不可达，切换路线，若无其他路线可以切换，则跳出步骤4，该方法结束。
[0025]
步骤5：依次控制全向agv小车以规划好的车身姿态通过各路段，并在对应节点实现车身姿态的调整，直至小车抵达终点。
[0026]
本方法的有益效果在于：
[0027]
本发明能够针对在行驶过程中可能遇到的车身姿态限制条件，规划好小车的车身姿态调整节点，避免在行驶过程中出现因为车身姿态限制原因而无法顺利通行的情况，从而帮助小车调整车身姿态，顺利抵达终点。
附图说明
[0028]
图1是是本发明车身姿态示意图。
[0029]
图2是本发明实施例的餐厅模型示意图。
[0030]
图3是本发明的流程图。
[0031]
图4是本发明各站点之间的距离关系图。
[0032]
图5是本发明计算调整车身姿态节点与各路段运行姿态流程图。
具体实施方式
[0033]
下面结合附图和实施例对本发明做进一步详细说明。
[0034]
如图1-5所示。
[0035]
本发明针对全向agv小车在行驶过程中遇到的车身姿态调整问题，提供一种全向agv的路径和姿态综合规划方法，可以针对在行驶过程中可能遇到的车身姿态限制条件，提前规划好agv小车在行驶过程中的姿态调整节点，从而帮助小车调整姿态，顺利的抵达终点。
[0036]
图3是本发明的流程图，具体实施过程如下：
[0037]
步骤1：使用拓扑地图法构建室内地图模型。拓扑地图法就是将环境中一些重要的区域用一个个节点表示，节点之间用线段连接表示路径。其中，节点除了代表区域地理位置以外，还包含agv进站姿态、最大旋转角度等限制条件。
[0038]
步骤1.1构建地图站点模型。在餐厅所有可以允许小车通过的路径上划分若干站点，通过这些站点可以将路径划分为若干个基本路段。这些站点模型至少需要包含以下信息：站点标识，站点位置，邻近站点编号，允许小车旋转的角度，进站车身姿态限制等要素等。本方法中设置站点的依据一般是道路的分岔口或转折点以及路况发生变化路段的起点和终点。
[0039]
步骤1.2构建道路地图模型，将所有可以通行的道路划分为若干基本路段，用以描述餐厅道路的位置信息，车体经过该道路的车身姿态限制信息，道路狭窄情况等。假设本方法中通道宽度为width
road
，所述狭窄通道一般指符合公式(1)的情况：
[0040]
其中width
car
指小车行驶过程中垂直于行驶方向的车体最宽处距离，width
safe
指小车距离道路边界的安全距离，本实施例中安全距离设置为为0.5m。
[0041]
步骤2：使用深度优先搜索算法在各路段车身姿态约束条件下进行路径规划。由于本实施例使用拓扑地图法构建地图模型，可以将拓扑地图中的站点视为图论中图的顶点，站点之间的连线也就是路线可以视为图中的边，深度优先搜索算法作为图论中常用的算法之一，可以找出两个节点之间符合要求的路径，然后计算组成该路径的各条边的距离之和，可以获得每条路径的距离，符合本实施例的需要。本实施例中所述的车身姿态约束条件一般指小车通过某一路段所允许的车身方向。
[0042]
步骤2.1：计算距离矩阵，如果地图中存在n个站点，那么就事先定义一个n
·
n的二
维矩阵e，e[i,j]表示站点i到站点j之间的距离，如果站点i到站点j可以不经过任何其他站点，直接抵达，那么e[i,j]等于站点i到站点j之间的距离，如果站点i到站点j不可以直接抵达或者i等于j，那么e[i,j]可以等于一个极大的固定值，图2是一个6
×
6的距离矩阵。
[0043]
步骤2.2：输入起始站点和目标站点，从起始站点开始，沿当前站点遍历未访问过的站点，在访问前，首先判断当前站点是否允许调整车身姿态，若当前站点允许调整车身姿态就说明无论之前车身姿态如何，都可以使小车在当前站点得到调整，使得小车可以抵达即将需要访问的站点；若不可调整车身姿态，则将之前路段允许的车身姿态集合与即将访问路段的车身姿态集合求交集，若交集为空说明小车无法抵达，放弃该站点，若交集不为空，说明小车可以抵达，访问该站点。当没有未访问过的站点时，则回到上一个站点，继续试探访问别的站点，直到所有的站点都被访问过。在访问过程中，若访问到目标站点，则将经过的所有节点记录并存入后备路径，并记录所有距离之和。访问结束后，选取距离最短的路径作为当前路径，其余路径根据距离远近依次存入后备队列，在当前路径不可达时，依次从后备队列切换路径。
[0044]
步骤3：在道路的相关约束条件下，计算在规划路径中小车的车身姿态调整站点。本实施例中所述的车身姿态只考虑直行和侧移两种，暂不考虑斜行的情况。本实施例中所述直行即车身方向平行于道路方向，如图1左图所示。所述侧移即车身方向垂直于道路方向，如图1右图所示。
[0045]
步骤3.1：结合餐厅地图模型与规划路径，遍历途经的每一个站点，计算传菜agv小车通过各基本路段的限制条件，找出并记录路径中的狭窄通道。其中限制条件包括：传菜小车经过该路段所允许的车身姿态，小车的进站姿态，小车在站点所能旋转的最大角度等。如图2所示，以路段7-12为例，小车从站点7开向站点12，那么该段路线的限制条件包括了站点模型中，12站点的进站约束条件，旋转约束条件，以及道路模型中，7-12路段的是否狭窄通道等条件。
[0046]
步骤3.2：计算在规划路径中小车的调整车身姿态站点，其流程图如图5所示。
[0047]
从起点开始，遍历规划路径每一个站点，将途经各路段所允许的车身姿态逐一求交集，若交集不为空，就说明小车在不改变车身姿态的情况下可以一直行驶；若交集为空，就说明小车需要调整车身姿态，再反向遍历途经站点，寻找其中允许调整车身姿态的站点。然后再以调整车身姿态站点作为新的起点重复上述过程，直至遍历至终点。
[0048]
首先做一种假设，小车在不改变车身姿态的情况下，最远可以行驶多远。如图2所示，假设需要从站点2驶向充电区充电，其规划路径为2-3-4-26，从站点2开始推导，通过步骤3.1可以获得允许小车通过各个基本路段的车身姿态的集合p
end
(end表示该基本路段结束站点的标识)，将路线段2-3的车身姿态集合p3与路线段3-4的车身姿态集合p4求交集，结果为s，如果s非空，那么就说明小车能够从站点2不改变车身姿态驶向站点4，此时，再将s与路线段4-26的集合p
26
求交集，如果结果为空，就说明小车从站点2驶向站点26需要至少改变1次车身姿态，如果站点4允许小车调整车身姿态，那么允许小车调整车身姿态的站点就在站点4，如果不允许调整车身姿态，那么需要反向遍历规划路径中，站点4到站点2中的每一个站点，其中第一个允许调整车身姿态的站点就是小车在当前路段的调整站点。然后再以此调整车身姿态站点为当前新的起点，重复上述过程，直到遍历完所有站点，就可以获得所有调整车身姿态站点，如果所有站点均不可调整车身姿态，就说明该路径不可达，跳出步骤
3，切换路线。本实施例中所述车身姿态集合，如图2所示，以地图正上方所对应实际方向为基准，设为v，假设此时小车需要从7站点驶向24站点，这是一条在地图中自下向上的道路，假设这段道路允许小车纵移与侧移，那么这段道路在地图中允许的车身姿态集合为{v,u,-u,-v}，其中v表示允许小车车头在地图中向上，在这段道路中也就是向前直行，u表示允许小车车头在地图中向右，在这段道路中也就是侧移，-u表示允许小车车头在地图中向左，也就是侧移，-v表示允许小车车头在地图中向下，在这段道路中也就是倒车行驶，其中v,u,-u,-v，并非要求小车车头始终保持该角度，而是允许小车在不偏离道路的情况下，以该方向为基准，调整方向。
[0049]
步骤4：根据调整车身姿态站点与狭窄道路划分路段，计算各路段车身姿态。
[0050]
以每一个调整车身姿态站点与狭窄道路的起点与终点作为分割点，将整个路径划分成若干段，每一段路线记为roadi(i为每一段路线的序号)；每一段路线roadi又由若干基本路段组成，将roadi中每个基本路段允许的车身姿态集合求交集，得到集合statusi，statusi就是允许小车通过路线roadi的车身姿态集合，如果允许的车身姿态不唯一，可以从statusi中选择一个最适合当前行进方向的车身姿态，若statusi为说明当前路径不可达，则切换路线，若无其他路线可以切换，则跳出步骤4，该方法结束。
[0051]
以地图中的正上方为u，如图2所示，小车从厨房驶向餐厅，假设roadn包含路线段2-3-4，其中2-3在地图中为自下向上的路段，允许小车前进，后退，左移，右移，所允许的车身姿态为{v,u,-v,-u}，路线段3-4在地图中也为自下向上的路段，但是通常餐厅与厨房连接处有大门连接，道路相对狭窄，因此只允许小车前进和后退，所允许的车身姿态为{v,-v}，那么两者的交集status就是{v,-v}，这说明小车可以以status中的车身姿态通过roadn。
[0052]
步骤5：依次控制全向agv小车以规划好的车身姿态通过各路段，并在对应节点实现车身姿态的调整，直至小车抵达终点。
[0053]
本发明未涉及部分与现有技术相同或可采用现有技术加以实现。