一种多机器人队形变换方法

摘要

本发明公开了一种多机器人队形变换方法，包括以下步骤：在坐标系中求得得到各机器人的坐标，将各个机器人所在的点依次连接形成一多边形，计算求出该多边形重心的坐标，队形变换时分为静态变换和动态变换两种模式：在静态变换模式下，以所有机器人的行走距离之和最短、队列收敛时间最少作为衡量指标，求出所要变换队形中各个机器人的坐标，根据坐标对应关系实现队形变换；在动态变换模式下，以所有机器人的行走距离之和最短、队列收敛时间最少以及队形变换约束条件作为衡量指标，求出所要变换队形中各个机器人的坐标，最后根据坐标对应关系实现队形变换。本发明方法不受机器人数量的限制，而且对单个机器人性能没有要求，实现较为简单。

权利要求

1.一种多机器人队形变换方法，包括以下步骤：

将所有机器人置于一坐标系中，得到各机器人所在位置的坐标，同时将各个机器人所在的点依次连接形成一多边形，计算求出该多边形重心的坐标，队形变换时分为静态变换和动态变换两种模式：

在静态变换模式下，以所述的多边形重心作为所要变换队形的重心，以所有机器人的行走距离之和最短、队列收敛时间最少作为衡量指标，求出所要变换队形中各个机器人的坐标，最后根据坐标对应关系实现队形变换；

在动态变换模式下，以所有机器人的行走距离之和最短、队列收敛时间最少以及所述的多边形重心与所要变换队形的重心之间的约束条件作为衡量指标，求出所要变换队形中各个机器人的坐标，最后根据坐标对应关系实现队形变换；

所述的多边形重心与所要变换队形的重心之间的约束条件为：

$\left\{\begin{array}{c}-{\theta }_q\le \arctan \left(\frac{y_{\mathrm{cg}}-{y_{\mathrm{cg}}}^{\prime }}{x_{\mathrm{cg}}-{x_{\mathrm{cg}}}^{\prime }}\right)\le {\theta }_q\\ a_1{x}_{\mathrm{cg}}+b_1\le y_{\mathrm{cg}}\le a_2{x}_{\mathrm{cg}}+b_2\end{array}\right.$其中(xcg、ycg)为所要变换队形的重心坐标、(xcg’、ycg’)为所述多边形重心的坐标，θq、a1、a2、b1、b2为设定的常数。

说明书

技术领域

本发明涉多机器人系统协作领域，尤其涉及一种多机器人队形变换方法。

背景技术

多机器人编队是典型的多机器人系统协作课题，要求从系统的角度探讨机器人群体的合作行为、信息交互及进化机制等。随着多机器人学的发展，目前多机器人编队控制已成为一个广受关注的问题。
编队控制，是指多机器人之间保持某种几何位置关系向目标行进，且保持队形或为适应周围环境的约束而变换队形的控制技术。很多研究者从不同角度提出了一些队形控制算法，包括：
几何法，从特定几何形状形成上研究机器人队形控制。这种方法易于实现，适合控制数量不大的机器人群体。
Leader-Follower方法，这种方法将队列中的机器人分为Leader与Follower两种角色，通过Leader运动的控制，来控制整个队列的运动。这种方法的优点是只需要给定Leader的行为或轨迹就可以控制整个机器人群体的行为。而该方法的缺点是系统中没有明确的队形反馈，例如队列保持或形变对机器人运动能力的要求超出了单个机器人的性能范围，就不能准确的执行预定动作。
行为控制法，首先为机器人规定一些期望的基本行为，一般情况下，机器人行为包括避障、保持队形和队形变换等。当机器人接受到外界环境刺激时，根据外部输入信息做出反应，并输出反应向量(比如说运动速度与航向)作为该行为的期望反应。行为选择模块通过一定的机制来综合各行为的输出，并将综合结果作为机器人对环境刺激的反应而输出。该方法中，协作是通过共享机器人之间的相对位置、状态等知识实现的。

发明内容

本发明提供了一种多机器人队形变换方法，该方法适合大群机器人的活动，而且不受单个机器人性能限制。
一种多机器人队形变换方法，包括以下步骤：
将所有机器人置于一坐标系中，得到各机器人所在位置的坐标，同时将各个机器人所在的点依次连接形成一多边形，计算求出该多边形重心的坐标，队形变换时分为静态变换和动态变换两种模式：
所述静态变换是指机器人在静止的情况下的变换，在静态变换模式下，以所述的多边形重心作为所要变换队形的重心，以所有机器人的行走距离之和最短、队列收敛时间最少作为衡量指标，求出所要变换队形中各个机器人的坐标，最后根据坐标对应关系实现队形变换；
静态模式又分为两种情况，一种是各机器人相对所要变换队形的重心的角度固定，此时所有机器人行走的距离之和最短为：
$\min \underset{i}{\overset{n}{\Sigma }}|{[(x_{\mathrm{cg}}+d_i\cos {\theta }_i)-{x_i}^{\prime }]}^2+{[(y_{\mathrm{cg}}+d_i\sin {\theta }_i)-{y_i}^{\prime }]}^2|$
队列收敛最小时间为：
$\min \underset{i\in \{1,...,n\}}{\max }|{[(x_{\mathrm{cg}}+d_i\cos {\theta }_i)-{x_i}^{\prime }]}^2+{[(y_{\mathrm{cg}}+d_i\sin {\theta }_i)-{y_i}^{\prime }]}^2|$
其中(xcg、ycg)为所要变换队形的重心坐标，(xi、yi)为第i机器人在所要变换队形中的坐标，θi为第i个机器人与所要变换队形重心的角度，n是指机器人的数量。
另一种是各机器人相对所要变换队形的重心的角度不固定，此时所有机器人行走的距离之和最短为：
$\min \underset{i}{\overset{n}{\Sigma }}|{\left[(x_{\mathrm{cg}}+M_x{d}_i\cos {\theta }_i){-x_i}^{\prime }\right]}^2+{[(y_{\mathrm{cg}}+M_y{d}_i\sin {\theta }_i)-{y_i}^{\prime }]}^2|$
队列收敛最小时间为：
$\min \underset{i\in \{1,...,N\}}{\max }|{[(x_{\mathrm{cg}}+{M_{x}d}_i\cos {\theta }_i)-{x_i}^{\prime }]}^2+{[(y_{\mathrm{cg}}+{M_{y}d}_i\sin {\theta }_i)-{y_i}^{\prime }]}^2|$
其中Mx，My分别为x，y的旋转矩阵，满足：
$\left[\begin{array}{c}{M}_x\\ M_y\end{array}\right]=\left[\begin{array}{cc}\cos \theta & \sin \theta \\ -\sin \theta & \cos \theta \end{array}\right]$
所述的动态变换是指机器人在移动过程当中的队形变换，在动态变换模式下，以所有机器人的行走距离之和最短、队列收敛时间最少以及所述的多边形重心与所要变换队形的重心之间的约束条件作为衡量指标，求出所要变换队形中各个机器人的坐标，最后根据坐标对应关系实现队形变换。
所有机器人行走的距离之和最短为：
$\underset{i}{\min }\underset{i}{\overset{n}{\Sigma }}|{[(x_{\mathrm{cg}}+d_i\cos {\theta }_i)-{x_i}^{\prime }]}^2+{[(y_{\mathrm{cg}}+d_i\sin {\theta }_i)-{y_i}^{\prime }]}^2|$
队列收敛最小时间为：
$\min \underset{i\in \{1,...,N\}}{\max }|{[(x_{\mathrm{cg}}+d_i\cos {\theta }_i)-{x_i}^{\prime }]}^2+{[(y_{\mathrm{cg}}+d_i\sin {\theta }_i)-{y_i}^{\prime }]}^2|$
多边形重心与所要变换队形的重心之间的约束条件为：
$\left\{\begin{array}{c}{-\theta }_q\le \arctan \left(\frac{y_{\mathrm{cg}}-{y_{\mathrm{cg}}}^{\prime }}{x_{\mathrm{cg}}-{x_{\mathrm{cg}}}^{\prime }}\right)\le {\theta }_q\\ a_1{x}_{\mathrm{cg}}+b_1\le y_{\mathrm{cg}}\le a_2{x}_{\mathrm{cg}}+b_2\end{array}\right.$
其中(xcg、ycg)为所要变换队形的重心坐标、(xcg’、ycg’)为所述多边形重心坐标，θq、a1、a2、b1、b2为设定的常数。其具体的值是根据机器人群组实际运动过程中所遭遇的外部环境，来计算得到。对队形变换而言，这些常数都是已知值。
本发明方法不受机器人数量的限制，而且对单个机器人性能没有要求，实现较为简单。

附图说明

图1为本发明机器人群静态变换前队形示意图；
图2为图1所示机器人群静态变换示意图；
图3为本发明机器人群动态变换前队形示意图；
图4为图3所示机器人群动态变换示意图。

具体实施方式

如图1所示，6个机器人随机分布在一平面当中，此时该6个机器人需要去完成某个任务，需要将该6个机器人编成如图2所示的正六边形。为了最有效率地把这6个机器人排成正六边形，通过如下方法来实现：
把每个机器人看作是平面上的一个点，然后将这6个点依次连接起来，可以得道一不规则的多边形，同时6个机器人所在的平面看作是一个坐标系，这里的坐标系可以是直角坐标系，也可以极坐标系，这样就可以确定每个机器人所在位置的坐标，根据现有重心计算公式，得到该多边形的重心坐标。为了计算方便，最好是将重心坐标设置为(0，0)，并将该重心坐标设为正六边形的重心坐标。
在图1和图2所示的机器人当中，所有的机器人是静止不动的，而且每个机器人的性能是一样的。机器人从图1所示的多边形变换到正六边形，其变换效率的衡量指标有两个，一个所有机器人行走距离之和，另一个是从不规则多边形到正六边形的变换时间(即队列收敛时间)。行走的距离之和越短，说明机器人消耗的能量越少，收敛时间越短，变换效率越高。
根据这两个条件可以得到两个方程，其中所有机器人行走距离之和的最小值为：
$\min \underset{i}{\overset{n}{\Sigma }}|{[(x_{\mathrm{cg}}+d_i\cos {\theta }_i)-{x_i}^{\prime }]}^2+{[(y_{\mathrm{cg}}+d_i\sin {\theta }_i)-{y_i}^{\prime }]}^2|$
n表示机器人数量，该处等于6，当机器人数量变化时，n也随着改变；(xcg、ycg)为正六边形的坐标；θi表示第i个机器人相对重心水平偏离的角度；di表示第i个机器人到重心的距离。
队列收敛的最短时间为：
$\min \underset{i\in \{1,...,n\}}{\max }|{[(x_{\mathrm{cg}}+d_i\cos {\theta }_i)-{x_i}^{\prime }]}^2+{[(y_{\mathrm{cg}}+d_i\sin {\theta }_i)-{y_i}^{\prime }]}^2|$
通过上述两个方程，就可以得到正六形中的各个机器人的坐标，机器人通过上述坐标的对应关系，可以通过直线行走，实现队形变换，也就是说通过上述的队形变化，其变换效率是最高的。
上述方程中，其实是假定正六边形中各机器人相对重心水平偏离的角度是固定的，当然其偏离角度是可以变化的，即正六边形是可以绕重心旋转的，此时所有机器人行走距离之和的最小值为：
$\min \underset{i}{\overset{n}{\Sigma }}|{\left[(x_{\mathrm{cg}}+M_x{d}_i\cos {\theta }_i){{-x}_i}^{\prime }\right]}^2+{[(y_{\mathrm{cg}}+M_y{d}_i\sin {\theta }_i)-{y_i}^{\prime }]}^2|$
队列收敛最短时间为：
$\min \underset{i\in \{1,...,N\}}{\max }|{[(x_{\mathrm{cg}}+{M_{x}d}_i\cos {\theta }_i)-{x_i}^{\prime }]}^2+{[(y_{\mathrm{cg}}+{M_{y}d}_i\sin {\theta }_i)-{y_i}^{\prime }]}^2|$
其中Mx，My分别为x，y的旋转矩阵，满足：
$\left[\begin{array}{c}{M}_x\\ M_y\end{array}\right]=\left[\begin{array}{cc}\cos \theta & \sin \theta \\ -\sin \theta & \cos \theta \end{array}\right]$
通过上述两个方程，就可以得到正六形中的各个机器人的坐标，机器人通过上述坐标的对应关系，实现最高效率的队形变换。
如图3所示，4个在运动当中的机器人，其原来的队形是呈长方形的，当它们需要去执行某个具体任务时，需要将队形变换成菱形，4个机器人从长方形队形变换到菱形队形，同样是将4个机器人置于一坐标系中，得到长方形队形中各个机器人的坐标，变换效率的指标也是一样的，一个是机器人的行走距离之和，另一个是队列收敛的时间，通过这两个条件，得到两个方程，其中所有机器人行走距离之和最小值为：
$\underset{i}{\min }\underset{i}{\overset{n}{\Sigma }}|{[(x_{\mathrm{cg}}+d_i\cos {\theta }_i)-{x_i}^{\prime }]}^2+{[(y_{\mathrm{cg}}+d_i\sin {\theta }_i)-{y_i}^{\prime }]}^2|$
队列收敛最小时间为：
$\min \underset{i\in \{1,...,N\}}{\max }|{[(x_{\mathrm{cg}}+d_i\cos {\theta }_i)-{x_i}^{\prime }]}^2+{[(y_{\mathrm{cg}}+d_i\sin {\theta }_i)-{y_i}^{\prime }]}^2|$
n表示机器人数量，该处等于4，当机器人数量变化时，n也随着改变；(xcg、ycg)为菱形的坐标；θi表示第i个机器人相对重心水平偏离的角度；di表示第i个机器人到重心的距离。
因为机器人是在运动当中的，其运动带有约束条件：
$\left\{\begin{array}{c}{-\theta }_q\le \arctan \left(\frac{y_{\mathrm{cg}}-{y_{\mathrm{cg}}}^{\prime }}{x_{\mathrm{cg}}-{x_{\mathrm{cg}}}^{\prime }}\right)\le {\theta }_q\\ a_1{x}_{\mathrm{cg}}+b_1\le y_{\mathrm{cg}}\le a_2{x}_{\mathrm{cg}}+b_2\end{array}\right.$
通过拉格郎日乘子法求解可以得到菱形队形中各个机器人的坐标，通过坐标的对应关系，实现机器人队形最优变换。