CN107065859A - 多移动机器人的轨迹预测方法 - Google Patents

Abstract

多移动机器人的轨迹预测方法，步骤如下：1)建立复平面内的多移动机器人运动模型。2)建立多机器人系统的拓扑图。3)求取复拉普拉斯矩阵。4)求取稳定矩阵。5)计算分布式离散控制信号6)记录位移信息并计算最终位置。本发明能够使机器人在最短时间内预测自己未来的轨迹路线，该方法是完全分布式的，所利用信息仅仅是机器人自身记录的位移信息，并且若无全局坐标系，机器人利用左右轮转速，或其他局部坐标系记录的位移信息仍可计算出自身的目标位置，该方法高效的帮助机器人计算自身目标位置，为多移动机器人的高效编队提供了可行方案。

Description

多移动机器人的轨迹预测方法

技术领域

本发明涉及一种多移动机器人的轨迹预测方法，具体是一种机器人分布式编队的过程中，每个机器人通过记录自身的路径信息，在最短时间内预测未来移动轨迹，从而为个体提供更多信息以便于实时避障或保持编队稳定性。

背景技术

随着技术的革新和机器人应用领域的不断拓展，多移动机器人系统近年来受到学者们的广泛关注，其技术也获得了长足的发展。采用多个结构和传感能力相对简单的单个机器人组成多机器人协作系统在节省机器人结构设计成本的同时也增加了安全性。多机器人协作作为多机器人系统研究的热点问题之一，在经历了20余年的发展历程后，无论在军事、国防，还是工业、生活等领域都展现了其广泛的应用空间。

多机器人协作系统研究的基本问题包括编队控制、地图构建、无人救援、目标围捕以及协同搬运等。其中，多机器人编队控制是多机器人系统协同完成其它任务的前提。多移动机器人编队控制是指机器人团队依赖传感器感知周围环境和自身状态，相互协作完成编队，在存在障碍物的环境中实现面向目标的自主运动。编队控制行为在自然界中随处可见，例如大雁列队飞行，鱼群结队游行，狼群编队捕食等。在人类活动中，编队行为也被广泛采用，例如军事土的机群编队，航母、军舰混合编队等。此外，研究多机器人编队控制控制的目的是为了最终能够在实际环境中应用，多机器人协作搬运作为多机器人编队控制控制的一个典型应用实例，近年来受到了广泛的关注。现实生活中，当需要搬运大型、沉重的物体时，往往需要多人以协作搬运的方式来完成。与此类似，若用机器人代替人类完成上述搬运任务时，当被运输的物体超过单个机器人的承载能力，使用多机器人协作搬运将是完成任务的一种有效途径。多机器人协作搬运物体本质上是一个具有约束条件下的多机器人队形保持问题，该问题可以描述为:给定任意的刚性多面体，多机器人成员能够以一条无碰撞的路径将该物体从起始点运动到最终目标点。

近年来，针对多移动机器人的编队分析与研究已取得了很大的进展。Lin Z在文献Distributed formation control of multi-agent systems using complex Laplacian[J](IEEE Transactions on Automatic Control,2014,59(7):1765-1777)中利用复拉普拉斯网络通信协议使多个机器人收敛到目标图形。Lin Z在文献Leader–followerformation via complex Laplacian[J](Automatica,2013,49(6):1900-1906)通过改进复拉普拉斯网络通信协议，引入领航-跟随结构，使得多个机器人在两个领航机器人的带领下，保持目标队形在二维平面的移动。Wang L在文献Formation control of directedmulti-agent networks based on complex Laplacian[C](2012IEEE 51st IEEEConference on Decision and Control(CDC).IEEE,2012:5292-5297)中将复拉普拉斯网络通信协议从无向网络拓扑拓展到有向网络拓扑。然而上述研究都集中于移动机器人个体间的网络拓扑特性，通过改变网络拓扑参数及移动机器人间的控制协议达到控制目标，而忽略了单个机器人在机器人编队中的作用，无法从根本上进行系统设计使整个多移动机器人系统满足一定的性能指标要求，也很难灵活地实现不同的全局控制目标。

发明内容

本发明要克服现有技术对移动机器人未来估计信息不足的缺点，针对多个可移动机器人在二维平面内以固定队形移动的轨迹预测问题，在现有的复拉普拉斯编队的基础上，本发明提供一种多移动机器人的轨迹预测方法，以最小步预测算法在最短时间内帮助每个机器人预测其未来轨迹。

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

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

首先建立全局坐标系，将多机器人移动的二维平面空间用复平面表示，二维平面的任意点坐标表示为(a,b)，那么该点在复平面中表示为a+bj，其中j表示单位虚数即a和b都表示任意实数。将目标的队形表示为一列复数向量n为自然数，表示复数的集合。视机器人为无碰撞体积的质点，表示第i个机器人在平面中的位置，是一列表示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：

可任选复拉普拉斯矩阵L的其中两行全为0以表示其最后两个节点为领航者节点，不受其余节点的影响不失一般性，令最后复拉普拉斯矩阵L最后两排全为0。

步骤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＝μ(t),i＝n-1,n (13)

其中u_i表示第i个机器人的速度控制输入，和分别表示第i和第k个机器人的位置。μ(t)表示随时间变化，且其Z变换极点为有限个的信号。N_i表示节点i的邻居节点的集合，N_i＝{v_k|e_ik∈E}。在此控制信号输入下，全局动态响应为：

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

x(k+1)＝(I-εL)x(k)+u(k)＝Ax(k)+u(k) (15)

其中ε为采样时间，取值范围λ_max为最大特征值。

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

根据分布式离散控制信号，机器人渐进收敛至目标队形，并保持该队形在平面内移动。在此过程中，机器人记录自身位移信息，并计算最终位置。选择第i个机器人为观测节点，算法具体方法如下:

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

x(k+1)＝Ax(k)++u(k)

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

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

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

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

(3)通过下式迭代计算观测节点的未来路径

其中是一列全为1的向量。

本项技术主要目的在于帮助机器人提前预知自身的轨迹路线，以方便机器人在复杂的工作环境中，适时的根据预测信息与感知信息，调整编队策略。以工业领域为例，大量机器人组成的机器人编队在危险物品的检查、收集和搬运的过程中，所处的环境往往是危险或且复杂的。机器人群需要根据这些环境适当的调整自身的控制策略，改变编队形状，或队形大小，在这一过程中，每个机器人都需要根据自己未来的轨迹做出判断。而在编队过程中，跟随机器人往往只能接收到领航机器人的控制信号，缺乏自身对未来路径的判断。针对这种情况，本专利提供了一种分布式预测方法，使用该方法，每个跟随机器人都能独立的预测自身的未来路径。

附图说明

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

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

图3多移动机器人收敛至目标队形并平移

图4对节点3应用算法在第12步计算出未来轨迹

具体实施方式

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

假设有一个由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)，其中，两个领航节点均以每秒0.04米的速度向右移动，如图3所示。针对该案例对算法过程进行演绎：

步骤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₅₁至图中使所有节点均在同一圆环上。

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

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

由复拉普拉斯矩阵L，

得：

根据图1机器人目标的队形，为：

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

该队形可以由下式表示：

η＝c₁1_n+c₂ξ

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

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

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

得：

配置复拉普拉斯矩阵的特征值，使得复拉普拉斯矩阵的特征值只有两个在零点，剩余的特征值均在复平面的右半平面。不失一般性，令最后复拉普拉斯矩阵L最后两排全为0，表示其最后两个节点为领航者节点，不受其余节点的影响。

步骤4，求取稳定矩阵

令对角矩阵

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

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

记：

记：

并记：

记初值为

迭代计算下述算式：

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

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

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

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

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

x(k+1)＝(I-εDL)x(k)＝Ax(k)+u(k)

其中ε为采样时间，取值范围λ_max为最大特征值，即λ_max＝3。取秒。

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

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

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

x(k+1)＝Ax(k)+u(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.0264,-0.2196,0.6149,-0.7028,0.2811)

机器人共移动7步。

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

其预测轨迹如图4所示。

其余机器人亦可用同样方法求得其未来轨迹。

以上阐述的是本发明给出的实例，仿真结果体现了本发明所提出的技术方案的有效性。需要指出的是，本发明不只限于上述实施例，当系统中不存在全局坐标系统时，机器人若能根据左右轮转速计算自身位移信息，采用本发明的技术方案，亦能有效预测未来轨迹。

本发明所涉及的预测算法，主要用于帮助机器人在最小步内预知自身的未来轨迹，一方面，为避开不可达点，危险点提供了一种参考信息，另一方面，轨迹预测有利于机器人避障。

本说明书实施例所述的内容仅仅是对发明构思的实现形式的列举，本发明的保护范围不应当被视为仅限于实施例所陈述的具体形式，本发明的保护范围也及于本领域技术人员根据本发明构思所能够想到的等同技术手段。

Claims (1)

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

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

首先建立全局坐标系，将多机器人移动的二维平面空间用复平面表示，二维平面的任意点坐标表示为(a,b)，那么该点在复平面中表示为a+bj，其中j表示单位虚数即a和b都表示任意实数；将目标的队形表示为n为自然数，表示复数的集合；视机器人为无碰撞体积的质点，表示第i个机器人在平面中的位置，是一列表示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,i＝1,2,…,n为n个矩阵L的需配置特征值，配置特征值即求解下述方程组：

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}；在此控制信号输入下，全局动态响应为：

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

x(k+1)＝(I-εL)x(k)＝Ax(k) (14)

其中ε为采样时间，取值范围λ_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步；

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

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