本公开涉及跟随机器人路径规划技术领域，特别涉及一种具有航向约束的跟随机器人路径规划方法及系统。
背景技术：
本部分的陈述仅仅是提供了与本公开相关的背景技术，并不必然构成现有技术。
近些年，随着机器人和人工智能的快速发展，跟随机器人已经成为研究的热点。与传统机器人相比，跟随机器人能与人自然交互，可在复杂环境下完成动态、灵活、精细的作业。目前，跟随机器人已经广泛应用于人们的日常生活中。例如，在机场，跟随机器人可用于帮助老年人搬运行李；在仓库，工作人员可引导跟随机器人将货物搬运到指定地点；在商场，人们可将商品放在机器人上进行愉快的购物。由此可见，跟随机器人有着广泛的应用前景和重要的研究意义。
跟随机器人在跟随行走过程中需要根据自身传感器对环境的感知，自行规划出一条安全的运行路线，同时高效完成作业任务。机器人常用的路径规划算法有人工势场法、图搜索算法(a*算法、d*lite算法、ara*算法等)、遗传算法、人工神经网络、模糊逻辑等。
本公开发明人发现，由于跟随机器人所处环境复杂多变，跟随的目标也处于运动状态，同时环境遮挡或障碍物移动容易导致机器人定位信息以及环境先验信息较难准确获取，此时为保证机器人运动的安全性，其路径规划只能‘看’一下，更新一下，再‘看’一下，再更新一下，需要频繁的路径重规划，该种局部路径规划方式有利于提升机器人应对突发情况的反应能力，但如何消除传统路径规划算法只对路径长度进行约束而忽略机器人航向，从而导致路径重规划时机器人航向易出现波动的问题，现有路径规划算法尚未给出明确的处理方案。
技术实现要素：
为了解决现有技术的不足，本公开提供了一种具有航向约束的跟随机器人路径规划方法及系统，在优先保证机器人航向稳定的前提下对路径进行重规划，极大的提高了机器人运动的稳定性。
为了实现上述目的，本公开采用如下技术方案：
本公开第一方面提供了一种具有航向约束的跟随机器人路径规划方法。
一种具有航向约束的跟随机器人路径规划方法，包括以下步骤：
构建环境栅格地图，机器人置于最下行栅格的中间位置，然后根据环境信息，将障碍物及目标位置投影到栅格地图中；
通过a*算法进行初始路径规划，确定当前时刻机器人的第一最短路径和第一期望航向，在目标和机器人运动过程中，通过a*算法进行路径重规划，获得更新后的机器人第二最短路径和第二期望航向；
如果第二期望航向与第一期望航向的差值小于第一预设阈值，则机器人按照第二最短路径行走；否则，认为更新后的路径尚不满足机器人航向稳定要求，利用栅格地图提取连接机器人与目标点的可通行栅格骨架，结合a*算法进行路径规划的优化。
作为可能的一些实现方式，所述路径规划的优化方法，具体为：
由栅格地图提取连接机器人与目标点的可通行栅格骨架，计算所有候选航向角度，从中寻找与第一期望航向具有最小差值的航向；
若与第一期望航向具有最小差值的航向大于第二设定阈值，则机器人按照第二最短路径行走；否则，将此航向对应的圆周与可通行栅格骨架的交点标记为路径关键点，利用a*算法分别规划从机器人到该路径关键点以及从该关键点到终点的第三路径；
若第三路径与第二最短路径的长度之差小于第三设定阈值，则认定第三路径为最优路径；否则，机器人按照第二最短路径行走。
作为可能的一些实现方式，采用zhang-suen算法对可通行区域进行骨架提取，将路径起点与终点定位到栅格地图，并设计其八邻域栅格状态，使路径起点与终点所处栅格在可通行区域细化过程中能够作为骨架点进行保留。
作为进一步的限定，若起点或终点所处栅格位于栅格地图的左上、右上、坐下或者右下的顶点位置，将三角形所处的对角栅格设为不可通行栅格，则起点或终点所处栅格作为骨架点进行保留。
作为进一步的限定，若起点或终点位于栅格地图的上、下、左或右的边缘位置，将三角形所处的边缘相反侧格设为不可通行栅格，则起点或终点所处栅格作为骨架点进行保留。
作为进一步的限定，若起点或终点位置具有完整的八邻域栅格，此时起点或终点处于可通行区域内部，将八邻域栅格中最先遍历栅格的对角线方向所处栅格置为不可通行栅格，则起点或终点所处栅格作为骨架点进行保留。
本公开第二方面提供了一种具有航向约束的跟随机器人路径规划系统。
一种具有航向约束的跟随机器人路径规划系统，包括：
栅格地图构建模块，被配置为：构建环境栅格地图，机器人置于最下行栅格的中间位置，然后根据环境信息，将障碍物及目标位置投影到栅格地图中；
路径规划模块，被配置为：通过a*算法进行初始路径规划，确定当前时刻机器人的第一最短路径和第一期望航向，在目标和机器人运动过程中，通过a*算法进行路径重规划，获得更新后的机器人第二最短路径和第二期望航向；
路径选择模块，被配置为：如果第二期望航向与第一期望航向的差值小于第一预设阈值，则机器人按照第二最短路径行走；否则，认为更新后的路径尚不满足机器人航向稳定要求，利用栅格地图提取连接机器人与目标点的可通行栅格骨架，结合a*算法进行路径规划的优化。
作为可能一些实现方式，所述系统还包括：
路径优化模块，被配置为：由栅格地图提取连接机器人与目标点的可通行栅格骨架，计算所有候选航向角度，从中寻找与第一期望航向具有最小差值的航向；
若与第一期望航向具有最小差值的航向大于第二设定阈值，则机器人按照第二最短路径行走，否则，将此航向对应的圆周与可通行栅格骨架的交点标记为路径关键点，利用a*算法分别规划从机器人到该路径关键点以及从该关键点到终点的第三路径；
若第三路径与第二最短路径的长度之差小于第三设定阈值，则认定第三路径为最优路径，否则，机器人按照第二最短路径行走。
本公开第三方面提供了一种介质，其上存储有程序，该程序被处理器执行时实现如本公开第一方面所述的具有航向约束的跟随机器人路径规划方法中的步骤。
本公开第四方面提供了一种电子设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的程序，所述处理器执行所述程序时实现如本公开第一方面所述的具有航向约束的跟随机器人路径规划方法中的步骤。
与现有技术相比，本公开的有益效果是：
1、本公开所述的路径规划方法及系统在优先保证机器人航向稳定的前提下对路径进行重规划，从而极大的提高了机器人运动的稳定性。
2、本公开所述的路径规划方法及系统将图像处理领域的骨架提取算法引入到基于栅格地图的机器人路径规划方面，并针对路径规划的特殊需要对骨架提取算法进行改进，使得骨架提取结果保留了路径起点与终点信息，极大的提高了路径规划的准确性。
附图说明
图1为本公开实施例1提供的具有航向约束的跟随机器人路径规划方法流程示意图。
图2为本公开实施例1提供的a*算法初始规划的路径示意图。
图3为本公开实施例1提供的在目标和机器人运动过程中通过a*算法重规划的路径示意图。
图4为本公开实施例1提供的栅格地图块示意图。
图5为本公开实施例1提供的骨架提取结果示意图。
图6(a)为本公开实施例1提供的顶点原始栅格状态示意图。
图6(b)为本公开实施例1提供的修改后顶点栅格状态示意图。
图7(a)为本公开实施例1提供的边缘点原始栅格状态示意图。
图7(b)为本公开实施例1提供的修改后边缘点栅格状态示意图。
图8(a)为本公开实施例1提供的内部点原始栅格状态示意图。
图8(b)为本公开实施例1提供的修改后内部点栅格状态示意图。
图9为本公开实施例1提供的优化后的可通行骨架提取结果示意图。
图10为本公开实施例1提供的候选航向提取结果示意图。
图11为本公开实施例1提供的路径优化结果示意图。
具体实施方式
应该指出，以下详细说明都是例示性的，旨在对本公开提供进一步的说明。除非另有指明，本文使用的所有技术和科学术语具有与本公开所属技术领域的普通技术人员通常理解的相同含义。
需要注意的是，这里所使用的术语仅是为了描述具体实施方式，而非意图限制根据本公开的示例性实施方式。如在这里所使用的，除非上下文另外明确指出，否则单数形式也意图包括复数形式，此外，还应当理解的是，当在本说明书中使用术语“包含”和/或“包括”时，其指明存在特征、步骤、操作、器件、组件和/或它们的组合。
在不冲突的情况下，本申请中的实施例及实施例中的特征可以相互组合。
实施例1：
如图1所示，本公开实施例1提供了一种具有航向约束的跟随机器人路径规划方法，首先通过a*算法进行初始路径规划，进而可确定当前时刻机器人的最短路径p1及期望航向d1；在后续路径重规划时，同样先采用a*算法进行路径规划并获得更新后的机器人最短路径p2及期望航向d2。
若d2与d1差值小于阈值，则认为更新后的路径航向较前一时刻波动不大，机器人即按照p2前进。否则，认为更新后的路径尚不满足机器人航向稳定要求，需对其做进一步优化。
优化方法为：首先由栅格地图提取连接机器人与目标点的可通行栅格骨架，然后计算所有候选航向角度，进而从中寻找与d1的最小差值a1。若a1超出阈值，则机器人按照p2行走。若a1在阈值范围内，则将其对应的圆周与可通行栅格骨架的交点标记为路径关键点，继而用a*算法分别规划从机器人到该路径关键点以及从该关键点到终点的路径，记为p3并计算其总长l。若l与路径p2的长度之差在阈值范围内，则认为p3为最优路径，否则机器人按照p2行走。
具体方案如下：
构建环境栅格地图，始终将机器人置于最下行栅格的中间位置，然后根据传感器感知到的环境信息，将障碍物及目标位置投影到栅格地图，如图2所示栅格地图，图中三角形所示为机器人位置，黑色栅格表示障碍物位置，五角星表示目标位置。根据栅格地图信息，机器人采用a*算法即可规划连接目标点的最短路径，如图2中黑色圆点所示。
在跟随行走过程中，由于目标处于运动状态，因而机器人当前规划的轨迹会由于目标位置的变化而失效。如图3所示机器人在沿着路径前进的同时，目标也运动到新的位置，因而机器人需按照固定的时间间隔对路径重新规划，以保证当前路径的有效性。然而a*算法在进行路径规划时只考虑了路径长度最短这一约束条件，这导致重规划的路径可能与上一时刻路径可能会有较大变化，如图3所示，黑色圆点表示重规划生产的路径，空心圆表示上一时刻路径。此种路径变化会导致机器人在短时间内对航向做出重大调整，这对于机器人的运动稳定性是非常不利的。
将图2所示当前时刻机器人的最短路径记为p1，期望航向记为d1。期望航向基于视线法导引策略获取，即将机器人当前位置指向期望点“视线”方向记为期望航向，图2及后续内容都将距离机器人4个栅格长度的路径点选为期望点，则期望航向d1如图中箭头所示。将图3所示更新后的机器人最短路径记为p2，期望航向记为d2。若d2与d1差值小于阈值，则认为更新后的路径航向较前一时刻波动不大，机器人即按照p2前进。否则，认为更新后的路径尚不满足机器人航向稳定要求，需对其做进一步优化。如选取90°作为航向波动阈值，则图2与图3中航向角变化大于该阈值，因而需进行路径优化。
首先采用现有的zhang-suen算法对可通行区域进行骨架提取，其原理及方法如下所示：
设已知3*3栅格的环境地图块如图4所示，图中网状栅格表示障碍，空白栅格表示可通行区域，为便于描述，将可通行栅格标记为1，障碍物点标记为0。
(1)记中心栅格为p1，其邻域的8个点顺时针绕中心点分别记为p2，p3，…，p9，其中p2在p1正上方。
首先标记同时满足下列条件的栅格：
(a)2≤b(p1)≤6；
(b)a(p1)＝1；
(c)p2×p4×p6＝0；
(d)p4×p6×p8＝0；
其中a(p1)是以p2，p3，…，p9为序时这些点的值从0到1变化的次数，b(p1)是p1的非零邻点的个数。当对所有边界点都检验完毕后，将所有标记了的点除去。
(2)同第(1)步，仅将前面条件(c)改为(e)p2×p4×p8＝0；条件(d)改为条件(f)p2×p6×p8＝0。同样当对所有边界点都检验完毕后，将所有标记了的点除去。
以上两步操作构成一次迭代。直至没有点再满足标记条件，这时剩下的点组成可通行区域的骨架。
图5中的黑色线条为上述可通行区域骨架提取算法作用于图2所示的栅格地图得到的结果，从图5可以清楚地看出通过上述zhang-suen算法能够提取可通行区域的连通骨架.
然而图5依靠zhang-suen算法仅对可通行区域做了骨架连通，却没有包含路径起点以及终点信息。为此需结合路径规划的需要，对上述算法做进一步完善，操作如下：
将路径起点与终点定位到栅格地图，并设计其八邻域栅格状态，使路径起点与终点所处栅格在可通行区域细化过程中能作为骨架点进行保留。
由zhang-suen算法操作过程可知，当任一栅格同时满足上述4个条件时，该栅格会被消掉。因而路径起点与终点所在的栅格只要不满足4个条件中的任意一个，其即可作为骨架点进行保留。由此，按照如下步骤对起点及终点的八邻域栅格状态进行修改，方法如下：
第一步：将起点与终点定位到栅格地图，并置其八邻域全为可通行栅格；
第二步：若起点或终点所处栅格(如图6(a)中三角形所示)位于栅格地图的顶点(左上、右上、坐下、右下)位置，则其八邻域栅格中有5个位于地图范围外面，如虚线所示，为此将这5个邻域栅格视不可通行栅格。因而仅需将三角形所处的对角栅格设为不可通行栅格(如图6(b)所示)，则公式(b)将不满足，此时可将三角形栅格作为骨架点进行保留。
若起点或终点位于栅格地图的边缘(上、下、左、右)位置，则其八邻域栅格中将有3个位于地图范围外面(如图7(a)中三角形所示，此时处于栅格地图右侧边缘)，将这3个栅格视不可通行栅格。因而仅需将三角形所处的边缘相反侧格设为不可通行栅格(如图7(b)所示)，则公式(b)将不满足，此时可将三角形栅格作为骨架点进行保留。
若起点或终点位置具有完整的八邻域栅格，如图8(a)中三角形所示，此时其处于可通行区域内部。虽然此时其本身不满足删除点要求，然而在骨架提取过程中，其邻域栅格状态会随着算法对栅格的遍历作用而改变。为保证该点仍作为骨架点保留，在骨架提取算法对栅格遍历顺序固定后，将其八邻域栅格中最先遍历栅格的对角线方向所处栅格置为不可通行栅格(如图8(b)所示,如算法从栅格地图左上角开始，则将三角形栅格的右下角栅格置为不可通行栅格)，则公式(b)将不满足，此时可将三角形栅格作为骨架点进行保留。
采用上述变化后，由图5获得的连通路径起点与终点的可通行区域骨架如图9所示，进一步，由可通行骨架可确定出4个候选航向，如图10中箭头所示，并可计算出黑色实线箭头所示的航向与d1完全相同，因而可将其所处栅格确定为路径关键点并进行如下路径规划。
采用a*算法分别规划从机器人到该路径关键点以及从该关键点到终点的路径，记为p3，如图11所示。然后计算p3总长l，若l与路径p2的长度之差在阈值范围内，则认为p3为最优路径，否则机器人仍然按照p2行走。
实施例2：
本公开实施例2提供了一种具有航向约束的跟随机器人路径规划系统，包括：
栅格地图构建模块，被配置为：构建环境栅格地图，机器人置于最下行栅格的中间位置，然后根据环境信息，将障碍物及目标位置投影到栅格地图中；
路径规划模块，被配置为：通过a*算法进行初始路径规划，确定当前时刻机器人的第一最短路径和第一期望航向，在目标和机器人运动过程中，通过a*算法进行路径重规划，获得更新后的机器人第二最短路径和第二期望航向；
路径选择模块，被配置为：如果第二期望航向与第一期望航向的差值小于第一预设阈值，则机器人按照第二最短路径行走；否则，认为更新后的路径尚不满足机器人航向稳定要求，利用栅格地图提取连接机器人与目标点的可通行栅格骨架，结合a*算法进行路径规划的优化。
路径优化模块，被配置为：由栅格地图提取连接机器人与目标点的可通行栅格骨架，计算所有候选航向角度，从中寻找与第一期望航向具有最小差值的航向；
若与第一期望航向具有最小差值的航向大于第二设定阈值，则机器人按照第二最短路径行走，否则，将此航向对应的圆周与可通行栅格骨架的交点标记为路径关键点，利用a*算法分别规划从机器人到该路径关键点以及从该关键点到终点的第三路径；
若第三路径与第二最短路径的长度之差小于第三设定阈值，则认定第三路径为最优路径，否则，机器人按照第二最短路径行走。
实施例3：
本公开实施例3提供了一种介质，其上存储有程序，该程序被处理器执行时实现如本公开实施例1所述的具有航向约束的跟随机器人路径规划方法中的步骤。
实施例4：
本公开实施例4提供了一种电子设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的程序，所述处理器执行所述程序时实现如本公开实施例1所述的具有航向约束的跟随机器人路径规划方法中的步骤。
以上所述仅为本公开的优选实施例而已，并不用于限制本公开，对于本领域的技术人员来说，本公开可以有各种更改和变化。凡在本公开的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本公开的保护范围之内。
技术特征：
1.一种具有航向约束的跟随机器人路径规划方法，其特征在于，包括以下步骤：
构建环境栅格地图，机器人置于最下行栅格的中间位置，然后根据环境信息，将障碍物及目标位置投影到栅格地图中；
通过a*算法进行初始路径规划，确定当前时刻机器人的第一最短路径和第一期望航向，在目标和机器人运动过程中，通过a*算法进行路径重规划，获得更新后的机器人第二最短路径和第二期望航向；
如果第二期望航向与第一期望航向的差值小于第一预设阈值，则机器人按照第二最短路径行走；否则，认为更新后的路径尚不满足机器人航向稳定要求，利用栅格地图提取连接机器人与目标点的可通行栅格骨架，结合a*算法进行路径规划的优化。
2.如权利要求1所述的具有航向约束的跟随机器人路径规划方法，其特征在于，所述路径规划的优化方法，具体为：
由栅格地图提取连接机器人与目标点的可通行栅格骨架，计算所有候选航向角度，从中寻找与第一期望航向具有最小差值的航向；
若与第一期望航向具有最小差值的航向大于第二设定阈值，则机器人按照第二最短路径行走；否则，将此航向对应的圆周与可通行栅格骨架的交点标记为路径关键点，利用a*算法分别规划从机器人到该路径关键点以及从该关键点到终点的第三路径；
若第三路径与第二最短路径的长度之差小于第三设定阈值，则认定第三路径为最优路径；否则，机器人按照第二最短路径行走。
3.如权利要求1所述的具有航向约束的跟随机器人路径规划方法，其特征在于，采用zhang-suen算法对可通行区域进行骨架提取，将路径起点与终点定位到栅格地图，并设计其八邻域栅格状态，使路径起点与终点所处栅格在可通行区域细化过程中能够作为骨架点进行保留。
4.如权利要求3所述的具有航向约束的跟随机器人路径规划方法，其特征在于，若起点或终点所处栅格位于栅格地图的左上、右上、坐下或者右下的顶点位置，将三角形所处的对角栅格设为不可通行栅格，则起点或终点所处栅格作为骨架点进行保留。
5.如权利要求3所述的具有航向约束的跟随机器人路径规划方法，其特征在于，若起点或终点位于栅格地图的上、下、左或右的边缘位置，将三角形所处的边缘相反侧格设为不可通行栅格，则起点或终点所处栅格作为骨架点进行保留。
6.如权利要求3所述的具有航向约束的跟随机器人路径规划方法，其特征在于，若起点或终点位置具有完整的八邻域栅格，此时起点或终点处于可通行区域内部，将八邻域栅格中最先遍历栅格的对角线方向所处栅格置为不可通行栅格，则起点或终点所处栅格作为骨架点进行保留。
7.一种具有航向约束的跟随机器人路径规划系统，其特征在于，包括：
栅格地图构建模块，被配置为：构建环境栅格地图，机器人置于最下行栅格的中间位置，然后根据环境信息，将障碍物及目标位置投影到栅格地图中；
路径规划模块，被配置为：通过a*算法进行初始路径规划，确定当前时刻机器人的第一最短路径和第一期望航向，在目标和机器人运动过程中，通过a*算法进行路径重规划，获得更新后的机器人第二最短路径和第二期望航向；
路径选择模块，被配置为：如果第二期望航向与第一期望航向的差值小于第一预设阈值，则机器人按照第二最短路径行走；否则，认为更新后的路径尚不满足机器人航向稳定要求，利用栅格地图提取连接机器人与目标点的可通行栅格骨架，结合a*算法进行路径规划的优化。
8.如权利要求7所述的具有航向约束的跟随机器人路径规划系统，其特征在于，还包括：
路径优化模块，被配置为：由栅格地图提取连接机器人与目标点的可通行栅格骨架，计算所有候选航向角度，从中寻找与第一期望航向具有最小差值的航向；
若与第一期望航向具有最小差值的航向大于第二设定阈值，则机器人按照第二最短路径行走，否则，将此航向对应的圆周与可通行栅格骨架的交点标记为路径关键点，利用a*算法分别规划从机器人到该路径关键点以及从该关键点到终点的第三路径；
若第三路径与第二最短路径的长度之差小于第三设定阈值，则认定第三路径为最优路径，否则，机器人按照第二最短路径行走。
9.一种介质，其上存储有程序，其特征在于，该程序被处理器执行时实现如权利要求1-6任一项所述的具有航向约束的跟随机器人路径规划方法中的步骤。
10.一种电子设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的程序，其特征在于，所述处理器执行所述程序时实现如权利要求1-6任一项所述的具有航向约束的跟随机器人路径规划方法中的步骤。
技术总结
本公开提供了一种具有航向约束的跟随机器人路径规划方法及系统，通过A*算法确定初始第一最短路径和第一期望航向，然后获得更新后的第二最短路径和第二期望航向，利用可通行栅格骨架中寻找与第一期望航向具有最小差值的航向，若此航向大于第二设定阈值，则按照第二最短路径行走，否则将此航向对应的圆周与可通行栅格骨架的交点标记为路径关键点，利用A*算法分别规划从机器人到该路径关键点以及从该关键点到终点的第三路径；若第三路径与第二最短路径的长度之差小于第三设定阈值，则认定第三路径为最优路径，否则机器人按照第二最短路径行走；本公开在优先保证机器人航向稳定的前提下对路径进行重规划，极大的提高了机器人运动的稳定性。
技术研发人员：张慧;马凤英;刘海英;赵永国;张友盼
受保护的技术使用者：齐鲁工业大学
技术研发日：.11.19
技术公布日：.02.21
