1.一种基于分层路径规划的集群机器人协同作业方法,其特征在于,包括如下步骤:S1、采用障碍物检测方法和有界单元划分法对集群机器人作业的环境地图进行空间分区,得到多个无障碍的单元格;集群机器人包括若干个跟踪机器人和多个测量机器人;
S2、采用改进型RRT*算法对集群机器人进行路径初步规划,得到初始路径集合,其中改进型RRT*算法的改进在于采用边界约束引导自适应采样策略进行节点采样,边界约束引导自适应采样策略是根据单元格拓扑、目标位置、障碍物分布动态调整采样的节点分布,且在节点生成中节点仅在单元格边界上生成的策略;
S3、对所有测量机器人生成的路径进行全局冲突检测,并利用CBS算法协调多个测量机器人之间的路径冲突,形成无碰撞路径集合;
S4、以测量机器人无碰撞路径集合为依据,采用CBS算法为一个或多个跟踪机器人统一规划跟踪路径,同时在每个跟踪机器人路径优化过程中,通过构建的路径代价函数使得当前跟踪机器人与负责跟踪的多个测量机器人之间保持至少设定的安全跟踪距离;
S5、当某个跟踪机器人无法感知其所负责的任一测量机器人位置时,进入感知恢复模式,若感知恢复失败,则该跟踪机器人自动终止感知恢复状态并回退至S4,重新规划跟踪机器人跟踪路径;
S6、对所有测量机器人的路径以及各个跟踪机器人的跟踪路径分别进行路径平滑处理,得到最终规划的无碰撞路径;
所述S1具体包括如下步骤:
S11、结合集群机器人本体半径及设定的安全距离,对环境地图中障碍物所在区域进行形态膨胀处理,并更新环境地图中障碍物边界;其中集群机器人中的测量机器人负责执行特定的测量任务,跟踪机器人负责实时获取一个或多个测量机器人的位姿状态;
S12、采用有界单元划分法自左向右依次扫描环境地图中的每列单元格,通过障碍物检测将每一列中相邻的可通行的单元格合并到一个单元格,得到多个无障碍的单元格;
所述S2具体包括如下步骤:
S21、将所有测量机器人的起点与目标作为改进型RRT*算法的输入,并基于单元格的空间拓扑关系,并结合启发式函数引导采样方向,在多个单元格的边界上随机采样生成节点;
S22、确定距离节点最近的节点,连接节点和节点并找到连线上与多个单元格边界交点中与节点最近的节点;
S23、判断节点与节点上的连线之间有无障碍物,如果没有,进入S24中,否则进入S25;
S24、将节点作为结构树上的一个新节点,并将新节点作为节点,然后判断当前迭代次数达到设定次数,如果是,则结束路径初步规划,得到初始路径集合,否则进入S22中;
S25、放弃节点,然后判断当前迭代次数达到设定次数,如果是,则结束路径初步规划,得到初始路径集合,否则返回S21;
所述S3具体包括如下步骤:
S31、对生成的测量机器人路径进行全局冲突检测,并依据初始路径集合构建时空占用约束表;
S32、CBS算法内的高层搜索器参考时空占用约束表并以“无约束”情形构建约束树的初始根节点;
S33、高层搜索器参考时空占用约束表对初始路径集合进行冲突检测,判断是否存在多个测量机器人在相同时刻占据同一位置或相距小于设定安全距离阈值的情况;
S34、一旦检测到冲突,高层搜索器将该冲突作为分裂点在约束树上生成两个子节点,每个子节点分别向冲突的测量机器人添加一个排除性约束,随后仅对被加排除性约束的测量机器人中调用底层路径规划器从起点重新规划一条完整路径,以满足新的约束条件;而所有未受影响的测量机器人的路径和历史约束则原样继承到生成的这两个子节点;
S35、高层搜索器再次进行冲突检测,如果仍存在路径冲突,则返回S34,直至得到全局无冲突的路径,形成无碰撞路径集合。
2.根据权利要求1所述的基于分层路径规划的集群机器人协同作业方法,其特征在于,所述S21中的启发式函数具体如下:;
其中,表示启发式函数,表示当前采样点的位置,表示路径规划的终点。
3.根据权利要求2所述的基于分层路径规划的集群机器人协同作业方法,其特征在于,所述S4中的路径代价函数具体如下:;
其中,表示当前跟踪路径的代价值;为调节因子;表示跟踪机器人在时刻t的位置,表示跟踪机器人跟踪的第i个测量机器人在时刻t的位置,为第个测量机器人的跟踪权重,N表示当前跟踪机器人所负责跟踪的测量机器人的总数;T表示总的跟踪时间。
4.根据权利要求3所述的基于分层路径规划的集群机器人协同作业方法,其特征在于,所述S5具体包括如下步骤:S51、如果当前跟踪机器人无法感知其所负责的任一测量机器人位置时,进入感知恢复模式;
S52、进入到感知恢复模式之后,当前跟踪机器人首先以其目标集合中最近一个测量机器人最后可观测位置作为圆心,构建圆形绕行轨迹;
S53、当前跟踪机器人围绕圆心并依据圆形绕行轨迹绕行到指定位置,并尝试恢复对其所负责的任一测量机器人的感知;若达到最长感知恢复时间或者当前跟踪机器人绕行半径已经达到最大绕行半径,则认定感知恢复失败,将该跟踪机器人自动感知恢复终止并回退至S4,重新执行当前跟踪机器人的路径规划。
5.根据权利要求4所述的基于分层路径规划的集群机器人协同作业方法,其特征在于,所述S52中的圆形绕行轨迹的表达式具体如下:;
其中,表示跟踪机器人在t时刻的位置;表示圆心坐标;表示动态半径;表示第t时刻的绕行角速度;表示即时侧向偏移分量。
6.根据权利要求5所述的基于分层路径规划的集群机器人协同作业方法,其特征在于,所述S53中当前跟踪机器人围绕圆心并依据圆形绕行轨迹绕行到指定位置的实现方式为:当前跟踪机器人每次围绕圆心并依据圆形绕行轨迹旋转绕行设定角度后,便计算当前跟踪机器人与其他所有集群机器人之间的最小距离;当小于设定安全阈值时,立即通过调整动态半径实现扩圈,即,其中表示当前扩圈的半径差值;若连续三个采样周期均大于,则以的方式收缩半径,其中表示初始绕行半径,始绕行半径设定为跟踪机器人机体半径与期望安全距离之和;跟踪机器人机体半径指的是以跟踪机器人外轮廓所拟合得到的最小半径,用于衡量跟踪机器人占据的空间大小;
绕行角速度以比例形式随半径变化,满足,其中表示初始绕行角速度;
即时侧向偏移分量由距离当前跟踪机器人最近的集群机器人连线方向的单位垂向向量乘以偏移量生成;当时触发一次侧向偏移,随后在三个采样周期内指数衰减至零,避免持续振荡,指数衰减公式具体如下:, ;
其中,,分别表示x、y轴上的即时侧向偏移分量,表示衰减系数;表示自然常数;
同时为保证多目标同时观测,若当前跟踪机器人负责的测量机器人集合中任一目标位置超出视场,先执行强制扩圈,再按减缓角速度,直至全部目标重新进入视场。
7.一种采用权利要求1至6任一项所述的集群机器人协同作业方法进行作业的集群机器人协同作业系统,包括集群机器人,其特征在于,还包括以下模块:分区模块,用于对环境地图进行空间分区,采用有界单元划分方法BCD将自由空间划分为多个无障碍的单元格;
采样模块,用于基于所述单元格边界,利用改进型RRT*算法进行采样,拓展的子节点只在单元格的边界上,并结合启发式函数引导路径扩展至目标单元格边界;
测量机器人路径规划与协调模块,用于对多个测量机器人并行进行路径规划,并通过CBS算法对其初始路径进行冲突检测与消解,生成一组所有测量机器人的无碰撞路径集合;
跟踪机器人路径规划模块,用于以已生成的测量机器人无碰撞路径集合为约束输入,采用CBS算法为一个或多个跟踪机器人生成无冲突的跟踪路径;
优化模块,用于在跟踪机器人路径的路径代价函数中引入多目标距离保持项,以使跟踪机器人在运行过程中保持与负责跟踪的多个测量机器人之间的至少设定的安全跟踪距离;
感知恢复模块,用于当任一跟踪机器人无法观测到其所负责的任一测量机器人时,控制跟踪机器人进入感知恢复模式;
路径平滑模块,采用B样条方法对路径进行平滑处理,从而得到最终规划的无碰撞的路径。