[发明专利]基于领航跟随法的机器人队形可变编队控制方法与控制器

摘要

本发明公开了一种基于领航跟随法的机器人队形可变编队控制方法，包括以下步骤：以期望相对距离和期望相对角度表示编队信息；领航机器人判断是否需要进行队形变换；若是，更新当前编队信息并发送给跟随机器人；若否，领航机器人将当前速度信息和位姿信息发送给跟随机器人；跟随机器人计算当前位姿误差；跟随机器人判断当前位姿误差是否在零的小邻域内；若是，则保持前一时刻的控制输出量，保持队形；若否，则跟随机器人的控制器根据当前位姿误差计算当前控制输出量，跟随机器人根据当前控制输出量运行，队形变换。本发明还提供一种用于机器人队形可变编队控制的控制器。本发明同时适用于固定队形与队形可变的机器人编队，具有优良的适用性。

主权项

1.一种基于领航跟随法的机器人队形可变编队控制方法，其特征在于：包括以下步骤：步骤1：预先规划领航机器人的运动轨迹；领航机器人将初始编队信息发送给跟随机器人；其中，初始编队信息包括领航机器人与跟随机器人之间的初始期望相对距离和初始期望相对角度；步骤2：领航机器人根据预先规划的运动轨迹确定自身的当前位姿信息和当前速度信息；其中，当前位姿信息包括机器人的方向角与其在全局坐标系中的中心点坐标；当前速度信息包括角速度与线速度；步骤3：领航机器人判断是否需要进行队形变换，即判断当前编队信息中领航机器人与跟随机器人之间的当前期望相对距离与当前期望相对角度是否需要改变；若是，则更新当前编队信息，并进入步骤4；若否，进入步骤5；步骤4：领航机器人将当前编队信息、自身的当前位姿信息和当前速度信息发送给跟随机器人；步骤5：领航机器人将自身的当前速度信息与当前位姿信息发送给跟随机器人；步骤6：跟随机器人确定自身的当前位姿信息和当前速度信息，并根据自身的当前位姿信息和当前速度信息、当前编队信息、领航机器人的当前位姿信息和当前速度信息，计算跟随机器人与领航机器人之间的当前位姿误差，所述当前位姿误差包括距离误差与角度误差；步骤7：跟随机器人判断当前位姿误差的是否在零的小邻域内；若是，则保持前一时刻的控制输出量，从而使得队形能够保持，并回到步骤2；若否，则进入步骤8：步骤8：跟随机器人的控制器根据当前位姿误差计算能够使得当前位姿误差的落入零的小邻域内的当前控制输出量，并且跟随机器人根据当前控制输出量进行运行，从而使得队形得到变换，并回到步骤2。