[发明专利]一种移动机器人平滑回归给定轨迹的路径规划方法及装置

说明书

本发明提供的该移动机器人平滑回归给定轨迹的路径规划方法及装置，通过五次多项式生成回归路径，克服了给定轨迹与回归路径在连接点处曲率不连续的情况，有利于移动机器人平滑的回归到给定轨迹。其次通过找到移动机器人距离给定轨迹最小距离的索引，并在该索引到给定轨迹终点索引的区间内寻找最优回归路径，从而确保回归路径的行驶方向与给定轨迹的行驶方向一致，再而通过设置松弛变量和可行解集的方式找到最优的回归路径，保证了回归路径的曲率和最小以及路径长度最短，同时满足最大曲率限制。

技术领域

本发明涉及移动机器人路径规划技术领域，尤其涉及移动机器人基于五次多项式从给定轨迹外平滑回归到给定轨迹的路径规划方法及装置。

背景技术

当移动机器人接收指令前往目的地的过程中，需要根据预先计算的路径规划进行移动，然而在实际应用中，预先规划的路径上往往不可避免的会存在障碍物，使得移动机器人不得不暂时偏离预定路径轨迹以避障。如在智能仓储领域，该移动机器人可以是仓库中的无人仓储车，其按照给定的轨迹进行行驶是实际工作中必不可少的一部分，尤其是按照给定轨迹运送货物。

但是由于很多原因，无人仓储车并不能确保每时每刻都在给定的轨迹上，所以当仓储车不在给定轨迹上时，如何快速且最优的回归到给定轨迹，这是一个非常重要的问题，不仅关系到能否准确的将货物运送到指定点，还关系到仓储管理人员的安全问题，因为无人仓储车如果不按照给定轨迹行驶时，仓库的管理人员将不得不使用额外的精力来留意无人仓储车。这一方面，会影响工作人员的工作，同时会给工作人员带来额外的安全隐患。

目前针对于在给定轨迹外平滑回归到给定轨迹的问题，业内并没有统一的标准和较好的标杆解决方案。然而发明人认为在当前技术条件下，如果能在确保回归路径与给定轨迹在连接点曲率连续的前提下，通过对回归路径的最大曲率、曲率和以及回归路径的长度等约束来找到最优的回归点，可能是引导此类工作条件下的无人仓储车回归到给定轨迹的较优方式。

发明内容

本发明的主要目的在于提供一种移动机器人平滑回归给定轨迹的路径规划方法，以实现背景技术中发明人提出的方案。

为了实现上述目的，根据本发明的一个方面，提供了一种移动机器人平滑回归给定轨迹的路径规划方法，其步骤包括：

S1 路径规划装置接收移动机器人的位姿与给定轨迹信息，采用启发式快速随机迭代算法以生成移动机器人距离给定轨迹最近点的索引，其步骤包括：

S11 在任意一个有限区间，在起始区间内随机生成n个点，且n大于2，将自变量索引最小的点的索引值赋给Left，自变量索引最大的点的索引值赋给Right，通过比较n个随机点的代价值的大小，找出代价值最小的索引MinCostIndex，根据MinCostIndex所在的位置对区间进行收缩；

S12 循环执行步骤S11，直至整个迭代区间长度小于预设值，以生成移动机器人距离给定轨迹最近点的索引；

S2 路径规划装置从步骤S1的索引开始遍历，根据位姿信息及遍历到的给定轨迹的点以五次多项式参数方程形式生成路径；

S3路径规划装置对步骤S2生成的路径的最大曲率进行判断，当回归路径的最大曲率小于1/Rmin时，Rmin为最小曲率半径，将路径加入到可行解集中；

S4 路径规划装置计算步骤S3中各可行解集的回归路径长度，以选择最短的回归路径作为最优回归路径。

在可能的优选实施方式中，步骤S11中根据MinCostIndex所在的位置对区间进行收缩的步骤包括：