多移动机器人的最小步编队方法与流程

本发明涉及多移动机器人编队控制技术领域，具体地说，是一种利用机器人行进过程中记录的路径信息来预测编队集结地点,从而加速机器人编队的方法。

背景技术：

随着技术的革新和机器人应用领域的不断拓展，单机器人在某些方面已经不能够满足人类生产生活的需要。采用多个结构和传感能力相对简单的单个机器人组成多机器人协作系统在节省机器人结构设计成本的同时也增加了安全性。多机器人协作作为多机器人系统研究的热点问题之一，在经历了20余年的发展历程后，无论在军事、国防，还是工业、生活等领域都展现了其广泛的应用空间。多机器人协作系统研究的基本问题包括编队控制、地图构建、无人救援、目标围捕以及协同搬运等，其中多机器人编队控制作为多机器人系统研究领域中一类广泛存在的基础问题，是研究其它问题的前提。

编队控制指的是多个可移动设备(例如机器人，人造卫星，无人飞行器，自主式水下探测器等)所组成的团队，在向目标地点移动或者在指定位置集结的过程中，相互之间保持特定的空间位置关系，即队形，同时又要适应环境约束的控制问题。通常而言，编队控制借助机器人之间的局部交互实现多机器人系统的群体行为，从而解决全局性的任务。

近年来，学者和工程师对多机器人系统编队进行了广泛而深入的研究。Abel R等人通过适当的选择几对机器人，并确定其在队形中的相对位置关系，通过设计反馈控制控制律，最终使得多机器人系统趋向目标队形(Coordinated fault-tolerant control of autonomous agents:Geometry and communications architecture[C]//Proc.of the IFAC Congress,Prague(Czech Republic),IFAC.2005.)。Lin Z等人通过利用复拉普拉斯矩阵设计多智能体的交互网络拓扑，从而实现了利用分布式控制形成任意图形的编队(Distributed formation control of multi-agent systems using complex laplacian[J].Automatic Control,IEEE Transactions on,2014,59(7):1765-1777.)。Olfati-Saber R等人研究了图的刚性与分布式编队的稳定性(Graph rigidity and distributed formation stabilization of multi-vehicle systems[C]//Decision and Control,2002,Proceedings of the 41st IEEE Conference on.IEEE,2002,3:2965-2971.)。中国专利文献CN102331711A是本发明最接近的现有技术。

从图形编队控制的角度看，大部分关于编队控制的现有的文献可以分为两类。一类旨在解决如何形成特定形状的编队图形。另一类则是意在解决如何保持编队图形的稳定性。如何形成特定形状的编队图形，通常也被称为集结问题，主要目的在于使多个机器人形成特定的形状。二维平面的编队集结问题已有较好的方法解决，即Lin Z等人提供的利用复拉普拉斯进行的分布式控制技术，其结果基本可以保证任意编队图形都能形成。但是此方法还存在一些不足。一方面，在实际应用中，存在这样一种情况，机器人最终达到的集结位置可能是不可到达点，如墙角，障碍物等，亦或是危险点，如陷阱。目前的方法缺乏对机器人未来目标位置的预测，可能最终使机器人落入这些点，轻则编队失败，重则整个系统崩溃。另一方面，由于分布式控制本身存在的问题，控制量随着编队的过程逐渐减小，使得整个编队的收敛速度较慢，且随着机器人个数的增多，编队的收敛速度会更慢。针对以上两个问题，本发明提出了最小步编队集结预测方法。通过该方法，每个机器人在集结过程中，在最少离散步内利用自身所具有的信息预测集结位置，为多移动机器人在各种实际应用中提高编队稳定性和编队效率提供关键技术。

技术实现要素：

针对多个可移动机器人在二维平面形成特定队形的问题，本发明要克服现有技术不能满足高效率编队任务的缺点，在现有的复拉普拉斯编队的基础上，本发明提供一种多移动机器人的最小步编队方法，旨在每个机器人在最短时间内能预测其在编队最终的集结位置。

本发明的多移动机器人的最小步编队方法，首先，将机器人在二维平面的运动建模，用复数来表示机器人在二维平面内的坐标，以一列复数向量来表示机器组人的当前位置，接着设计机器人的交互拓扑，然后用一列复数向量来表示机器人组的目标队形，并求取复拉普拉斯算子来设计分布式控制律，再求取稳定矩阵，最后通过记录机器人的位移信息，求得机器人的目标位置。具体步骤如下：

步骤1，建立运动模型

首先建立全局坐标系，用复平面表示多机器人移动的二维平面空间。比如，在二维平面的坐标表示为(a,b)，那么该点在复平面中表示为a+bj，其中j表示单位虚数即表示复数的集合。一列n维复数向量表示n个机器人的目标队形。视机器人为无碰撞体积的质点，第i个机器人在平面中的位置表示为用一列n维复数向量表示n个机器人的位置：

x＝(x₁,x₂,…,x_n)^T (1)

其中(·)^T为矩阵的转置。系统中的每个机器人都服从单积分器运动模型：

是第i个机器人的速度输入信号。

步骤2，建立多机器人系统的拓扑图

将多机器人系统及其相互之间的局部交互表示为无向拓扑图G＝(V,E)，其中V＝{v₁,v₂,…v_n}表示图中的n个节点的集合，v_i表示图中第i个节点，即第i个机器人，表示节点与节点之间的边的集合，e_ik∈E表示机器人i能测量机器人k的相对位置d＝ρj^θ，其中ρ表示两个机器人之间的距离，θ表示机器人k相对于机器人i的角度。由于G＝(V,E)是无向图，所以如果e_ik∈E，那么e_ki∈E，即机器人k也能测量机器人i的相对位置。添加边e₁₂,e₂₃,…,e_(n-1)n,e_n1至图中使所有节点均在同一圆环上。

步骤3，求取复拉普拉斯矩阵

对应图无向图G＝(V,E)的邻接矩阵W，如果存在e_ik∈E，那么w_ik≠0,反之如果那么w_ik＝0，w_ik表示矩阵W第i行第k列个元素。

定义复拉普拉斯矩阵L，

式(3)中∑(·)为求和符。

编队图形可以由下式表示：

η＝c₁1_n+c₂ξ (4)

其中，1_n表示一列含有n个元素，且元素全为1的向量，为一列含有n个复数元素的向量，表示队形基，且ξ≠1_n。c₁和c₂为任意复数。

通过求解矩阵方程组计算复拉普拉斯矩阵L：

步骤4，将复拉普拉斯矩阵的极点配置在右半平面

为保证本发明中编队系统是稳定的，复拉普拉斯矩阵的特征值必须配置在右半平面。记λ_i为n个矩阵L的需配置特征值，i＝1,2,…,n。配置特征值即求解下述方程组：

det(·)是行列式运算符，表示计算其后括号内矩阵的行列式值。其中，

由于有两个特征值已存在，不失一般性，令λ_n＝λ_n-1＝0，并可设d_n＝d_n-1＝1。记可用牛顿迭代法求解式(6)，具体如下：

记：

记：

并记：

其中，表示函数f_i对d_k求偏导数。记初值为

迭代计算下述算式：

直至‖(·)‖表示求取式(·)的二范数，δ表示计算精度，取δ＝0.0001。

重新配置极点后的复拉普拉斯矩阵：

步骤5，将连续系统转换为离散系统

机器人的控制信号由机器人与其邻居机器人位置差的加权组合决定：

其中u_i表示第i个机器人的速度控制输入，和分别表示第i和第k个机器人的位置。N_i表示节点i的邻居节点的集合，N_i＝{v_k|e_ik∈E}。在此控制信号输入下，全局动态响应为：

由于在实际应用中控制信号以离散时间信号给出，所以其对应的离散时间响应：

其中ε为采样时间，取值范围max{·}表示求取集合{·}的最大值。

步骤6，记录位移信息并计算最终位置

根据基于复拉普拉斯矩阵的离散时间分布式控制律，机器人渐进收敛至目标队形。在此过程中，每个机器人记录自身位移信息，并计算最终位置。为叙述简便，选择第i个机器人对算法加以说明，算法具体思路如下:

(1)机器人系统在下述离散时间响应下逐渐收敛至目标队形，

x(k+1)＝Ax(k)

观测第i个机器人的位移信息：

其中表示一列除第i个元素为1以外全为0的向量。

(2)第i个机器人记录位移信息x_i(k)，k＝1,2,3,…，并以此构建Hankel矩阵H：

(3)当Hankel矩阵H(x_i(k+1)-x_i(k))第一次失秩时，计算其零空间，并记为ρ，并记机器人移动2s+1步，s是正整数。

(4)通过下式计算观测节点的

其中是一列全为1的向量，x_i(∞)表示第i个节点最终位置。对一个n个机器人的系统，对任意机器人而言，均有2s+2≤2n，即机器人至多移动2n-1步即可算出最终位置。

(5)通过调整控制律加快编队速度：

每个机器人根据计算得到的最终位置调整控制律：

其中(x_i(∞)-x_i)是加速系统收敛控制项。

在实际的应用过程中，需要多移动机器人以各种各样的编队形状以满足各种任务需求，现有机器人编队技术整体收敛速度慢，影响机器人工作效率。本发明利用了机器人编队控制内在的线性关系，使得机器人在最短时间内通过分析自身的轨迹路径能够得出最终位置，加入收敛加速控制项，从而加速机器人群的编队过程。本发明是完全分布式的，不依赖全局坐标系，机器人不需要安装

附图说明

图1为本发明示例的目标队形图形

图2为本发明的拓扑示意图

图3原复拉普拉斯矩阵编队收敛过程

图4为在本发明算法控制下的机器人编队收敛过程

具体实施方式

以下结合附图和实际编队案例对本发明新型的技术方案作进一步描述。

针对有一个由5个机器人组成的多移动人系统，5个机器人分布在二维平面上，其坐标为(-1.3077,-1.3499)，(-0.4336,3.0349)，(0.3426,0.7254)，(3.5784,-0.0631)，(2.7694,0.7147)，需要组成如图1所示的特定形状队形，该形状二维平面空间中可由坐标表示为(-1,1)，(0,0)，(1,1)，(1,-1)，(-1,-1)，针对该案例对算法过程进行演绎：

步骤1，建立复平面内的多移动机器人运动模型

首先建立全局坐标系，将多机器人移动的二维平面空间用复平面表示，二维平面的任意点坐标表示为(a,b)，那么该点在复平面中表示为a+bj，其中j表示单位虚数即视机器人为无碰撞体积的质点，是一列表示5个机器人的位置的向量，表示第i个机器人在平面中的位置。每个机器人的动力学方程由下式给出：

其中表示第i个节点的速度输入信号。

步骤2，建立多机器人系统的拓扑图

将多机器人系统及其相互之间的局部交互表示为无向拓扑图G＝(V,E)，其中V＝{v₁,v₂,…v₅}表示图中的5个节点的集合，v_i表示图中第i个节点，即第i个机器人，表示节点与节点之间的边的集合，e_ik∈E表示机器人i能测量机器人k的相对位置d＝ρj^θ，其中ρ表示两个机器人之间的距离，θ表示机器人k相对于机器人i的角度。由于G＝(V,E)是无向图，所以如果e_ik∈E，那么e_ki∈E，即机器人k也能测量机器人i的相对位置。添加边e₁₂,e₂₃,e₃₄,e₄₅,e₅₁至图中使所有节点均在同一圆环上。最后得到如图2所示的多机器人系统的拓扑图。

步骤3，求取复拉普拉斯矩阵

对应图无向图G＝(V,E)的邻接矩阵W，如果存在e_ik∈E，那么W_ik≠0,反之如果那么W_ik＝0。根据步骤2设计的无向G＝(V,E)，可知

由复拉普拉斯矩阵L，

得：

根据要求，将目标的队形表示为

η＝[-1+j,0,1+j,1-j,-1-j]^T

该队形可以由下式表示：

η＝c₁1_n+c₂ξ

不失一般性，取c₁＝0，c₂＝1:

η＝c₁1_n+c₂ξ＝ξ

通过求解矩阵方程组计算复拉普拉斯矩阵L：

得：

配置复拉普拉斯矩阵的特征值，使得复拉普拉斯矩阵的特征值只有两个在零点，剩余的特征值均在复平面的右半平面。

步骤4，求取稳定矩阵

目标配置极点为λ₁＝1,λ₂＝2,λ₃＝3,λ₄＝0,λ₅＝0。配置特征值即求解下述方程组：

其中对角矩阵

由于有两个特征值已存在，可设d₄＝d₅＝1。记用牛顿迭代法求解，具体如下：

记：

记：

并记：

记初值为

迭代计算下述算式：

直至‖(·)‖表示求取式(·)的模，δ表示计算精度，取δ＝0.0001。循环10次即满足条件：

重新配置极点后的复拉普拉斯矩阵：

步骤5，计算分布式离散控制信号

每个机器人的速度控制信号由下式给出：

其中u_i表示第i个机器人的速度控制输入，和分别表示第i和第k个机器人的位置。在此控制信号输入下，全局动态响应为：

在实际应用中控制信号以离散时间信号给出，其对应的离散时间响应：

其中ε为采样时间，取

步骤6，记录位移信息并计算最终位置

根据基于复拉普拉斯矩阵的离散时间分布式控制律，机器人渐进收敛至目标队形。在此过程中，机器人记录自身位移信息，并计算最终位置。不失一般性，选择第3个机器人例说明，算法具体如下:

(1)机器人系统在下述离散时间响应下逐渐收敛至目标队形，其过程如图3所示

x(k+1)＝Ax(k)

测记录第三个机器人的位移信息：

x₃(k)＝(0,0,1,0,0)^Tx(k)

初始值

(2)第3个机器人记录位移信息x₃(k)，k＝1,2,3,…，并以此构建Hankel矩阵：

(3)当Hankel矩阵H(x₃(k+1)-x₃(k))第一次失秩时，计算其零空间，并记为ρ，

ρ＝(-0.0485,0.3559,-0.7765,0.5177)

机器人共移动7步。

(4)通过下式计算观测节点的

其中

其余机器人亦可用同样方法求得其最终位置。

(5)通过调整控制律加快编队速度

每个机器人根据计算得到的最终位置调整控制律：

其中(x_i(∞)-x_i)是加速系统收敛控制项。在调整控制律后，系统编队过程如图4所示。

以上阐述的是本发明给出的实例，仿真结果体现了本发明所提出的技术方案的有效性。需要指出的是，本发明不只限于上述实施例，当系统中不存在全局坐标系统时，机器人可通过对速度积分计算自身位移信息，采用本发明的技术方案，亦能有效预测最终位置。

本发明所涉及的预测算法，主要用于帮助机器人在最小步内预测自身的目标位置，一方面，为避开不可达点，危险点提供了一种参考信息，另一方面，也为加快分布式编队提供了关键技术。