第5期 屈云豪，等：行军启发的多机器人紧密队形保持策略 ·675· 了C机器人原来的位置，紧密编队得以继续。 人C是否处于中间位置，如果处于中间位置，则 当失效的机器人C位于非边缘位置时，如图2 将任意一边的机器人向内收拢，其队形紧密程度 所示。首先作为处于同一列且是C机器人的追 相等。否则，就将该机器人向内收拢。假设图4 随者的机器人F向前替补，同时原来C机器人的 中，机器人C偏向右侧，则将机器人C右侧的机 参考机器人A和B变为F机器人的参考机器人， 器人E向内收拢。因为机器人C向前替补，机器 C机器人的追随者机器人D变为机器人F的追随 人A也就不再是机器人C的参考机器人，此时机 者。最后在机器人运动模型的控制下机器人F和 器人E前来替补机器人C,则机器人A就变成了 机器人H向前替补了机器人C和机器人F原来 的位置，紧密编队得以继续。 机器人E的参考机器人。随后在机器人运动模型 的控制下机器人E完全替补了机器人C原来的 ④ @ 位置。紧密编队得以继续。 ⑧©①⑧-g-①⑧ F 2.2多机器人紧密队形保持策略算法实现 本文专注于研究多机器人紧密队形保持策 ⑨ ⑦ 略，对于失效机器人的检测、两轮差动机器人的 (a)初始状态 (b)C失效 (c)F替补 (d最终状态 运动模型、机器人的控制模型等问题不做过多阐 图2同列依次替补原则非边缘C机器人失效示意图 述，只是借用当前已有的成熟模型。下面阐述多 Fig.2 Non-edge C robot failure with substitution in the 机器人紧密队形保持策略的算法实现。 same column diagram 1)当检测到编队中有机器人失效时，首先触 当同一列的机器人依次替补完成后，就需要 发同列依次替补原则。将失效机器人所在列的机 检查是否满足紧密队形约束。如果满足就结束队 器人依次向前替补。如果失效机器人或替补后的 形重组，如果不满足就需要根据图3和图4所示 的不同情况进一步调整。 机器人处于该列的末尾，则同列依次替补原则结束。 2)随后触发末排向内收拢原则，首先判断失 ⑧+① ⑧+D ⑧① ⑧① 效机器人所在列所处的位置，将失效机器人偏向 @©@@-⊙-@@ ①④@ 的方向的机器人向内收拢。如果失效机器人或收 (a)初始状态(b)C失效(c)E替补(d最终状态 拢后的机器人处于末排的边缘，则末排向内收拢 图3未排向内收拢原则C机器人失效示意图 原则结束。 Fig.3 C robot failure with inward convergence diagram 3)最后机器人个数减1。 ⑧ 2.2.1同列依次替补算法实现 C( 1)首先判断失效或需要替补的机器人是否处 于该列的末尾。如果位于列尾，则同列依次替补 (a)初始状态(b)C替补B(c)E替补C(d)最终状态 算法结束。 图4末排向内收拢原则C机器人替补示意图 2)否则就将失效或需要替补的机器人的所有 Fig.4 C robot substitute with convergence diagram 参考关系都转移到替补机器人上面。以领航者追 当末排的机器人C失效时，如图3所示，先判 随者控制方法为例，即将失效或需要替补的机器 断机器人C是否处于中间位置，如果处于中间位 人的领航者变为替补机器人的领航者，同时失效 置，则将任意一边的机器人向内收拢，其队形紧 或需要替补的机器人的所有追随者（排除替补机 密程度相等。否则，就将该机器人向内收拢。假 器人)都变为追随替补机器人。 设图3中，机器人C偏向右侧，则将机器人C右 侧的机器人E向内收拢。机器人E首先替补了机 3)因为替补机器人前去替补失效的或需要替 器人C的位置，同时机器人C原来的参考机器人 补的机器人，替补机器人原来的位置无机器人。 A和B变成了机器人E的参考机器人，随后在机 接下来将替补机器人原来的位置作为需要替补的 器人运动模型的控制下机器人E完全代替了机器 机器人位置，将该列下一个机器人作为替补机器 人C。紧密编队得以继续。 人，重新调用步骤1)，继续依次替补。直到失效 当末排的机器人C向前替补时，如图4所示， 或需要替补的机器人处于该列的末尾。 当最后一排中机器人C向前替补后，先判断机器 同列依次替补算法的流程图如图5所示。了 C 机器人原来的位置，紧密编队得以继续。 当失效的机器人 C 位于非边缘位置时，如图 2 所示。首先作为处于同一列且是 C 机器人的追 随者的机器人 F 向前替补，同时原来 C 机器人的 参考机器人 A 和 B 变为 F 机器人的参考机器人， C 机器人的追随者机器人 D 变为机器人 F 的追随 者。最后在机器人运动模型的控制下机器人 F 和 机器人 H 向前替补了机器人 C 和机器人 F 原来 的位置，紧密编队得以继续。 B A C D E F H G B A C D E F H G B A D E F H G B A F D E H G (a) 初始状态 (b) C 失效 (c) F 替补 (d) 最终状态 图 2 同列依次替补原则非边缘 C 机器人失效示意图 Fig. 2 Non-edge C robot failure with substitution in the same column diagram 当同一列的机器人依次替补完成后，就需要 检查是否满足紧密队形约束。如果满足就结束队 形重组，如果不满足就需要根据图 3 和图 4 所示 的不同情况进一步调整。 B D A C E B D A C E B D A E B D A E (a) 初始状态 (b) C 失效 (c) E 替补 (d) 最终状态 图 3 末排向内收拢原则 C 机器人失效示意图 Fig. 3 C robot failure with inward convergence diagram B D A C E B D A C E B D A C E C D A E B (a) 初始状态 (b) C 替补 B (c) E 替补 C (d) 最终状态 图 4 末排向内收拢原则 C 机器人替补示意图 Fig. 4 C robot substitute with convergence diagram 当末排的机器人 C 失效时，如图 3 所示，先判 断机器人 C 是否处于中间位置，如果处于中间位 置，则将任意一边的机器人向内收拢，其队形紧 密程度相等。否则，就将该机器人向内收拢。假 设图 3 中，机器人 C 偏向右侧，则将机器人 C 右 侧的机器人 E 向内收拢。机器人 E 首先替补了机 器人 C 的位置，同时机器人 C 原来的参考机器人 A 和 B 变成了机器人 E 的参考机器人，随后在机 器人运动模型的控制下机器人 E 完全代替了机器 人 C。紧密编队得以继续。 当末排的机器人 C 向前替补时，如图 4 所示， 当最后一排中机器人 C 向前替补后，先判断机器 人 C 是否处于中间位置，如果处于中间位置，则 将任意一边的机器人向内收拢，其队形紧密程度 相等。否则，就将该机器人向内收拢。假设图 4 中，机器人 C 偏向右侧，则将机器人 C 右侧的机 器人 E 向内收拢。因为机器人 C 向前替补，机器 人 A 也就不再是机器人 C 的参考机器人，此时机 器人 E 前来替补机器人 C，则机器人 A 就变成了 机器人 E 的参考机器人。随后在机器人运动模型 的控制下机器人 E 完全替补了机器人 C 原来的 位置。紧密编队得以继续。 2.2 多机器人紧密队形保持策略算法实现 本文专注于研究多机器人紧密队形保持策 略，对于失效机器人的检测、两轮差动机器人的 运动模型、机器人的控制模型等问题不做过多阐 述，只是借用当前已有的成熟模型。下面阐述多 机器人紧密队形保持策略的算法实现。 1) 当检测到编队中有机器人失效时，首先触 发同列依次替补原则。将失效机器人所在列的机 器人依次向前替补。如果失效机器人或替补后的 机器人处于该列的末尾，则同列依次替补原则结束。 2) 随后触发末排向内收拢原则，首先判断失 效机器人所在列所处的位置，将失效机器人偏向 的方向的机器人向内收拢。如果失效机器人或收 拢后的机器人处于末排的边缘，则末排向内收拢 原则结束。 3) 最后机器人个数减 1。 2.2.1 同列依次替补算法实现 1) 首先判断失效或需要替补的机器人是否处 于该列的末尾。如果位于列尾，则同列依次替补 算法结束。 2) 否则就将失效或需要替补的机器人的所有 参考关系都转移到替补机器人上面。以领航者追 随者控制方法为例，即将失效或需要替补的机器 人的领航者变为替补机器人的领航者，同时失效 或需要替补的机器人的所有追随者 (排除替补机 器人) 都变为追随替补机器人。 3) 因为替补机器人前去替补失效的或需要替 补的机器人，替补机器人原来的位置无机器人。 接下来将替补机器人原来的位置作为需要替补的 机器人位置，将该列下一个机器人作为替补机器 人，重新调用步骤 1)，继续依次替补。直到失效 或需要替补的机器人处于该列的末尾。 同列依次替补算法的流程图如图 5 所示。 第 5 期 屈云豪，等：行军启发的多机器人紧密队形保持策略 ·675·