一种异形机器人路径规划的方法及系统与流程

1.本发明涉及一种异形机器人路径规划的方法。


背景技术：

2.对于异形机器人，其构型与传统车辆有很大不同，传统的路径规划方法有其局限性。传统路径规划将车辆或移动机器人等效成一整个规则几何体，如立方体或球体，再进行规划。但是对于异形机器人，比如具有瞭望杆的机器人，如果直接等效为一个规则的几何体，则存在体积过大的问题。而对于高地隙机器人，传统路径规划算法将机体下部的空间等效为实体，实际中机器人原本可以跨越的物体，路径规划过程中却将其标记为不可通过障碍，降低了机器人在实际工作中的通过性。


技术实现要素：

3.为解决上述问题，本发明所采用的技术方案为：
4.一种异形机器人路径规划的方法，其特征在于，包括：
5.步骤s1：存储基于栅格单元构成的占用栅格地图；
6.步骤s2：定义机器人构型，将机器人等效为多个球体的组合体；
7.步骤s3：以机器人主体对应的球体中心为探索中心，计算下一步可移动位置；
8.a)遍历以机器人主体中心为中心点的三阶魔方的四周26个栅格，计算当机器人主体中心移动到这26个栅格中，是否发生碰撞或跌落；
9.b)碰撞判断
10.i.判断车体是否与周边障碍碰撞；
11.ii.判断轮子或其他部件是否与周边障碍碰撞；
12.c)判断轮子是否着地；
13.d)若均符合将该位置加入可合法移动位置集合；
14.步骤s4、根据策略筛选一个下一步移动位置；
15.步骤s5：重复s3-s4步骤，计算下一步位置。
16.进一步的，所述的步骤s2包括：将机器人主体对应的球体中心点定义为c，其在地图坐标系中的位置为(xc，yc，zc)，半径定义为rc；将轮子或其他部件对应球体中心点定义为pi，在地图坐标系中的位置为(xi，yi，zi)，半径定义为ri，其中i为部件的个数。
17.进一步的，所述的步骤s3包括：遍历探索以c点为中心的三阶魔方块中的四周的26个栅格；所述的碰撞判断包括：
18.(1)判断机器人主体是否碰撞，被探索的s点位置为(xs，ys，zs)；搜索以s为中心，rc为半径的空间，若不存在已知障碍，则认为机器人主体在s点没有碰撞到周边障碍物；反之则认为在s点碰到障碍；
19.(2)判断其他部件是否碰撞；当机器人主体中心点在s时，考虑到车体并不是直线移动，而是有旋转、侧移、滚转等姿态，则当车体从c位置移动到s位置时，其他部件在地图中
的位置为：
[0020][0021]
其中pi为第i个部件对应的球体中心位置在地图中的坐标，ti为各部件相对于机器人本体的偏移矩阵，由各部件安装位置确定；ri为旋转矩阵，表示机器人当前的姿态，由机器人的运动模型确定；
[0022]
(3)以各部件的新位置pi为中心，ri为半径，搜索所有的部件周边是否存在障碍，若不存在已知障碍，则认为各个部件在s点没有碰撞到周边障碍物；反之则认为在s点碰到障碍；
[0023]
(4)若s点所在位置，机器人本体和所有部件均未存在碰撞情况，则将s点计为非碰撞点。
[0024]
进一步的，所述的c)判断轮子是否着地，包括：
[0025]
判断车体运动机构是否着地；查看所有应接触地面的部件边缘是否接触地面，如某个轮子部件对应的球体中心点为ps，则检测位置se：
[0026][0027]
若se点在地图中存在，则认为此部件接触地面；
[0028]
其中ri表示轮子部件等效球体的半径；e为可允许的误差，若s点所在位置，机器人所有应接触地面的部件均能着地，则将s点计为着地点；
[0029]
进一步的，遍历26个栅格位置，若有位置同时属于非碰撞点和着地点，则判定此位置为合法移动点，计为vsi；从所有合法移动点中筛选下一步移动目标位置，筛选采用一个代价函数：
[0030]
cost(c，vsi，p
t
)
[0031]
＝α*dist(vsi，p
t
)+β*loss(c，vsi)+γ*safe(vsi)
[0032]
其中c为机器人主体初始中心点，p
t
为路径规划最终目标点。
[0033]
dist(vsi，p
t
)为移动点到目标点的距离；
[0034]
loss(c，vsi)为机器人从c点移动到vsi点的代价函数；
[0035]
safe(vsi)计算vsi到周边障碍物的平均距离；
[0036]
α，β，γ分别为三个代价函数的权重值；
[0037]
计算得到最小的cost的vsi的点，选取为下一步移动点，计为ts点。
[0038]
一种异形机器人路径规划系统，用于实现所述的权利要求1-x的方法。
[0039]
有益效果
[0040]
对于高地隙机器人，本发明的路径规划方法不会将机体下部的空间等效为实体，实际中高地隙机器人可以跨越的物体，路径规划过程中可以标记为可通过障碍，提高了机器人在实际工作中的通过性。
附图说明
[0041]
图1，传统机器人示意图；
[0042]
图2，本发明中高地隙机器人示意图；
[0043]
图3，本发明中占用栅格地图示意图；
[0044]
图4，本发明中机器人等效为多个球体的组合体的示意图；
[0045]
图5，本发明中机器人等效为多个球体的组合体的示意图；
[0046]
图6，本发明中三阶魔方块示意图；
具体实施方式
[0047]
为使本发明实现的技术手段、创作特征、达成目的与功效易于明白了解，下面结合具体实施方式，进一步阐述本发明。
[0048]
如图1，传统的移动机器人示意图，其底部离地面较近，在进行路径规划时，可以直接将车体等效为一个规则的几何体。
[0049]
对于异形的移动机器人，具有更为复杂、特别结构，直接等效为规则的几何体，则会降低移动机器人的通过率。
[0050]
如图2，高地隙机器人，传统路径规划算法将机体下部的空间等效为实体，实际中机器人原本可以跨越的物体，路径规划过程中却将其标记为不可通过障碍，降低了机器人在实际工作中的通过性。
[0051]
本发明提出一种针对异形机器人路径规划的方法，能够针对此类机器人进行路径规划，该方法充分考虑到机器人构型，相比传统路径规划算法，能够很大程度上提升机器人的通过性。
[0052]
如图3，在路径规划系统中已经存储了基于栅格单元构成的占用栅格地图，后续简称地图，地图生成方法不再本专利中过多描述，具体可以是通过激光传感器构占据栅格地图。
[0053]
如图4-图5，定义机器人构型。将机器人等效为多个球体的组合体。
[0054]
以机器人主体对应的球体中心为探索中心，计算下一步可移动位置：
[0055]
a)遍历以机器人主体中心为中心点的三阶魔方的四周26个栅格，计算当机器人主体中心移动到这26个栅格中，是否发生碰撞或跌落。
[0056]
b)碰撞判断
[0057]
i.判断车体是否与周边障碍碰撞；
[0058]
ii.判断轮子或其他部件是否与周边障碍碰撞；
[0059]
c)判断轮子是否着地；
[0060]
d)若均符合将该位置加入可合法移动位置集合；
[0061]
2、根据策略筛选一个下一步移动位置；
[0062]
3、重复3-4步骤，计算下一步位置。
[0063]
优选的，进一步描述上述方法：
[0064]
定义机器人构型，将机器人等效为多个球体的组合体，对于不同构型的机器人，可以定义不同的组合。
[0065]
将机器人主体对应的球体中心点定义为c，其在地图坐标系中的位置为(xc，yc，
zc)，半径定义为rc；
[0066]
将轮子或其他部件对应球体中心点定义为pi，在地图坐标系中的位置为(xi，yi，zi)，半径定义为ri。其中i为部件的个数。
[0067]
以c点进行可移动位置探索。
[0068]
a)遍历探索以c点为中心的三阶魔方块中的四周的26个栅格。如图6。进行b碰撞判断和c判断轮子是否着地步骤的计算。
[0069]
碰撞判断包括：
[0070]
i.判断机器人主体是否碰撞。被探索的s点位置为(xs，ys，zs)。搜索以s为中心，rc为半径的空间，若不存在已知障碍，则认为机器人主体在s点没有碰撞到周边障碍物。反之则认为在s点碰到障碍。
[0071]
ii.判断其他部件是否碰撞。当机器人主体中心点在s时，考虑到车体并不是直线移动，而是有旋转、侧移、滚转等姿态，则当车体从c位置移动到s位置时，其他部件在地图中的位置为
[0072][0073]
其中pi为第i个部件对应的球体中心位置在地图中的坐标，ti为各部件相对于机器人本体的偏移矩阵，由各部件安装位置确定。ri为旋转矩阵，表示机器人当前的姿态，由机器人的运动模型确定。
[0074]
iii.以各部件的新位置pi为中心，ri为半径，搜索所有的部件周边是否存在障碍，若不存在已知障碍，则认为各个部件在s点没有碰撞到周边障碍物。反之则认为在s点碰到障碍。
[0075]
iv.若s点所在位置，机器人本体和所有部件均未存在碰撞情况，则将s点计为非碰撞点。
[0076]
v.进一步的，在第ii步计算时，不需要计算所有机器人可能的姿态，这将增加巨大的计算量。由于机器人的结构及运动方式的限制，机器人在c点时的姿态和运动模型决定了在s点的姿态只可能存在有限的情况，即t，r的集合是有限的。
[0077]
着地判断包括：
[0078]
判断车体运动机构是否着地，比如四个轮子是否接触地面。
[0079]
查看所有应接触地面的部件边缘是否接触地面。如某个轮子部件对应的球体中心点为ps，则检测位置se：
[0080][0081]
若se点在地图中存在，则认为此部件接触地面。
[0082]
其中ri表示轮子部件等效球体的半径。e为可允许的误差，在实际中表示可以允许轮子存在少量悬空或下沉。
[0083]
若s点所在位置，机器人所有应接触地面的部件均能着地，则将s点计为着地点。
[0084]
b)以上述方法遍历26个栅格位置，若有位置同时属于非碰撞点和着地点，则判定
此位置为合法移动点，计为vsi[0085]
进一步，根据一定策略从所有合法移动点中筛选下一步移动目标位置：采用一个代价函数：
[0086]
cost(c，vsi，p
t
)
[0087]
＝α*dist(vsi，p
t
)+β*loss(c，vsi)+γ*safe(vsi)
[0088]
其中c为机器人主体初始中心点，p
t
为路径规划最终目标点。dist(vsi，p
t
)为移动点到目标点的距离，此距离根据实际场景可以以欧几里得距离计算，也可以以曼哈顿距离计算。距离越短函数值越小，表示规划的距离更近。
[0089]
loss(c，vsi)为机器人从c点移动到vsi点的代价函数，此函数取决于机器人的运动模型、运行能耗、速度和姿态等多种条件。
[0090]
此函数值越小表示机器人运动的代价越小，更少的做动或耗能。safe(vsi)计算vsi到周边障碍物的平均距离。平均距离越大，则函数值越小，表示运行时更安全。
[0091]
α，β，γ分别为三个代价函数的权重值，可以根据实际场景的要求进行调整。
[0092]
计算得到最小的cost的vsi的点，选取为下一步移动点，计为ts点。
[0093]
将c点移动到ts点，重复2-3步，计算下一个移动点。当路径规划达到目标点，或规划步骤数超过一定数量时，停止规划。
[0094]
以上显示和描述了本发明的基本原理和主要特征和本发明的优点。本行业的技术人员应该了解，本发明不受上述实施例的限制，上述实施例和说明书中描述的只是说明本发明的原理，在不脱离本发明精神和范围的前提下，本发明还会有各种变化和改进，这些变化和改进都落入要求保护的本发明范围内。本发明要求保护范围由所附的权利要求书及其等效物界定。