基于自组织拓扑与模型预测的多机器人编队控制方法

专利名称：基于自组织拓扑与模型预测的多机器人编队控制方法

专利类型：发明专利

专利申请号：CN202210275617.9

专利申请（专利权）人：广东工业大学
权利人地址：广东省广州市越秀区东风东路729号

专利发明（设计）人：肖涵臻,杨洋,林泽瀚,揭英儒,庞业臻,林泽派

专利摘要：本发明公开了一种基于自组织拓扑与模型预测的多机器人编队控制方法，包括：针对机器人编队，基于拓扑学与RIPF算法生成自组织形成拓扑网络；在所述自组织形成拓扑网络的基础上，规划机器人编队的系统路径；对机器人编队进行系统轨迹跟踪。本发明将RIPF生成方法和移动机器人形成方法相结合，建立了一个自组织的蜂群系统；采用人工场势法辅助系统路径进行规划，并应用PDNN组合MPC的方法，重构并解决了预测模型的约束QP问题，大大降低了控制算法的计算复杂度。

主权利要求：
1.一种基于自组织拓扑与模型预测的多机器人编队控制方法，其特征在于，包括：针对机器人编队，基于拓扑学与RIPF算法生成自组织形成拓扑网络，包括：利用有向图和机器人位置序列集合来搭建机器人编队控制框架，通过设置机器人编队构成RIPF所满足的条件，来自组织生成拓扑网络；
在所述自组织形成拓扑网络的基础上，规划机器人编队的系统路径，包括：构建每个机器人的期望路径序列，使用梯度下降法得到t时刻的控制输入；通过设置机器人的安全距离、允许距离，用以引导机器人避免与路径上的障碍物碰撞；利用人工场势法，结合所定义的安全距离和允许距离，对机器人行走过程进行避障；基于机器人期望路径的位置向量及控制输入，构建第一离散化仿射系统；
对机器人编队进行系统轨迹跟踪，包括：
构建机器人当前状态与参照状态的跟踪误差，并基于此建立轨迹跟踪系统的运动学模型；定义输入控制向量，令所述跟踪误差趋于零并对其进行逆变换，构建第二离散化仿射系统并设置该系统的约束条件；基于预测输出量构建代价函数，并将代价函数转换为求解优化二次规划问题；利用神经动态优化方法解决各个机器人的二次规划问题，并用下一时刻的输入增量来计算机器人编队的最优化输入，从而控制多机器人编队；
结合所定义的安全距离和允许距离，对机器人行走过程进行避障，包括：先将机器人所扫描到障碍物进行排序，再利用障碍物对机器人i的排斥力，定义潜在函数 和控制输入其中 表示t时刻机器人i的期望路径的位置向量， 表示距离机器人i最近的第c个障碍物的位置，e为自然对数底数，l为整数是随环境变化而变化的障碍数，为避障权重系数；
表示t时刻障碍物进入了机器人的安全范围，人工势场 开始排斥机器人使机器人避障，若 则表明机器人避障成功；
→表示趋近；dsafe为机器人的安全距离，dallow为允许距离；将 与 两个控制输入相加，得到t时刻机器人i期望路径的控制输入 其中， 为t时刻下的控制输入；
所述构建机器人当前状态与参照状态的跟踪误差，并基于此建立轨迹跟踪系统的运动学模型，包括：T
根据机器人运动学方程，构建机器人当前状态pic＝[xicyicθic] 与参照状态T的跟踪误差pie＝[xieyieθie]，目标将pie→0；其中，当前状态、参照状态、跟踪误差中的三个参数分别表示对应的x、y轴位置以及方向角；
轨迹跟踪系统的运动学模型可以表示为：
上式中，参数上标圆点表示该参数的一阶导数，ωic、 分别表示当前状态、参照状态下的角速度，vic、 vie分别表示当前状态、跟踪误差、参照状态下的线速度，θie为跟踪误差的方向角，xie为跟踪误差的x轴位置；
所述定义输入控制向量，令所述跟踪误差趋于零并对其进行逆变换，构建第二离散化仿射系统并设置该系统的约束条件，包括：定义输入控制向量uie：
令pie→0并将其进行逆变换：
构建第二离散化仿射系统：
pi(k+1)＝h(pi(k))+si·ui(k)其中：
该系统的约束条件为：
uimin≤ui(k)≤uimax、Δuimin≤Δui(k)≤Δuimax、pimin≤pi(k)≤pimax其中ui(k)表示k时刻机器人i的广义输入向量；Δui(k)＝ui(k)‑ui(k‑1)表示为k时刻机器人i的输入增量；pi(k)表示为k时刻机器人i的广义离散状态误差向量；uimin、Δuimin、pimin分别为广义输入向量、输入增量、广义离散状态误差向量的最小值，uimax、Δuimax、pimax分别为广义输入向量、输入增量、广义离散状态误差向量的最大值，Δt为采样周期。
2.根据权利要求1所述的基于自组织拓扑与模型预测的多机器人编队控制方法，其特征在于，所述利用有向图和机器人位置序列集合来搭建机器人编队控制框架，通过设置机器人编队构成RIPF所满足的条件，来自组织生成拓扑网络，包括：定义F＝(G,P)来表示多机器人编队的M个机器人编队控制框架；其中G＝(V,E,A)是一个有向图，表示机器人之间通信拓扑结构；V＝1,2,...,M表示共有M个机器人；E＝{(i,j),i≠j,i、j∈V}为有向边集合，(i,j)∈E表示机器人i可以接收到机器人j的信息；A表示由相
2M
关加权邻接系数aij构成的相关加权邻接矩阵；P＝(pixy)∈R ，pixy＝(xi,yi)是机器人i的位置序列，R代表实数集；
机器人编队构成RIPF所满足的条件：
Num(E)＝2Num(V)‑3
其中Num(·)表示数量，d(i)为机器人i的度，表示机器人i与其他机器人之间的信息传递关系；dout(i)为机器人i的出度，出度表示接收信息。
3.根据权利要求2所述的基于自组织拓扑与模型预测的多机器人编队控制方法，其特征在于，所述自组织生成拓扑网络，包括：a.获取机器人在t时刻的位置信息，将M个机器人在t时刻的位置定义为：pixy(t)＝[xi(t),yi(t)]，i＝1,2,…,M其中，xi(t),yi(t)表示t时刻的位置坐标；
机器人i与j之间存在一个最小距离dmin：
pixy(0)、pjxy(0)表示初始时刻机器人i、j的位置；
b.将编队中的各机器人视为点，利用DelaunayTriangulation算法绘制散点图作为机器人编队的位置图像；在所获的t时刻位置图像上进行三角剖分得到三角剖分图像，并满足d(i)≤6；
c.在所获的三角剖分图像上进行边界删减，使图像的边数满足Num(E)＝2Num(V)‑3，此时图像为无向图刚性图；
d.从编队中选择一个机器人RL作为领导者，该RL只能接收主机命令，不能从其他机器人接收命令，该机器人出度为0；其他机器人也要满足 条件，给无向图刚性图的边添加方向，得到一个RIPF图像即为自组织形成拓扑网络。
4.根据权利要求3所述的基于自组织拓扑与模型预测的多机器人编队控制方法，其特征在于，所述使用梯度下降法得到t时刻的控制输入，表示为：其中 为t时刻下的控制输入，aij为相关加权邻接系数，ki为机器人i期望路径的输入权值， 是梯度下降法的表示符号；设置一个距离值函数 为两个相邻机器人i和j之间的期望距离，i≠ji、j∈V； 表示t时刻机器人i的期望路径的位置向量。
5.根据权利要求4所述的基于自组织拓扑与模型预测的多机器人编队控制方法，其特征在于，所述基于机器人期望路径的位置向量及控制输入，构建第一离散化仿射系统，具体表示为：其中，Δt为采样周期，k为时间状态，k+1为k的下一个状态， 为离散时间下的
为平滑机器人的运动路径，需要定义一个参考路径：
定义 为机器人i在参考路径上移动的参考状态向量，
分别表示x、y轴位置以及方向角；
为机器人进行避障时的参考速度向量，其中 分别
为线速度、角速度；
其中γ是正的常数，k0是一个表示避障过程结束的时间实例；pic(k)＝[xic(k)yic(k)T Tθic(k)]表示k时刻机器人i的当前路径状态向量，pir(k)＝[xir(k)yir(k)θir(k)]表示k时刻机器人i的期望路径状态向量；这两个状态向量中的三个参数分别表示对应的x、y轴位置以及方向角。
6.根据权利要求5所述的基于自组织拓扑与模型预测的多机器人编队控制方法，其特征在于，所述代价函数表示为：其中：
其中，pi(k+1|k)表示机器人i用k时刻的pi(k)对pi(k+1)时刻进行预测的结果，而ui(k|k)、Δui(k|k)表示k时刻ui(k)、Δui(k)的最优预测值，N表示预测域的范围；Nu表示控制时域的范围，而且1≤Nu≤N，Qi和Ri表示权重矩阵。
7.根据权利要求6所述的基于自组织拓扑与模型预测的多机器人编队控制方法，其特征在于，所述二次规划问题表示为：约束条件为：
其中：
其中 表示k时刻机器人i的广义预测输入向量； 表示为k时刻机器人i的预测输入增量； 表示为k时刻机器人i的广义离散状态预测误差向量，分别为广义预测输入向量、预测输入增量、广义离散状态预测误差向量的最小值， 分别为广义预测输入向量、预测输入增量、广义离散状态预测误差向量的最大值，I表示单位矩阵，上标T表示矩阵转置。 说明书 : 基于自组织拓扑与模型预测的多机器人编队控制方法技术领域[0001] 本发明涉及机器人控制领域，具体涉及一种基于自组织拓扑与模型预测控制的多机器人编队控制方法。背景技术[0002] 近年来，多机器人蜂群系统引起了人们的广泛兴趣，多机器人的合作在完成个别机器人难以处理的任务。多机器人蜂群系统一般由智能单元、通信拓扑和移动规律组成，其中轮式移动机器人是构建蜂群系统最常用的单元之一。[0003] 由于机器人的感知和计算能力有限，通信带宽受限，在多机器人编队中机器人只能接收和发送相邻信息。在一些现有的编队系统中，采用了分布式构造并基于图论来描述机器人的通信拓扑。但是大多通信拓扑结构都是人为提前设计好的，存在一些缺点。例如：拓扑结构不适合实际情况；某个机器人出现损坏，拓扑结构受损，信息无法正常传递；编队运动后，先前设定的拓扑结构不是最优的。因此，自动生成适当的通信拓扑结构来控制机器人编队是必要的。[0004] 多机器人编队仍需要控制算法来驱动机器人，现在已有许多控制算法，如：模型预测控制(MPC)、图论、非线性控制、LQ方法、自适应控制、滑模和人工势场法。对于多机器人蜂群的形成，需要考虑系统的约束和避免碰撞两个问题。在本发明中，采用MPC对多机器人编队进行控制。[0005] 现有技术主要存在的缺点：[0006] 1.拓扑结构不适合实际情况；2.某个机器人出现损坏，拓扑结构受损，信息无法正常传递；3.编队运动后，先前设定的拓扑结构不是最优的；4.控制算法计算量复杂。发明内容[0007] 本发明的目的是提供一种基于自组织拓扑与模型预测的多机器人编队控制方法，使通信网络在机器人编队运动中可以保持整体的刚性。[0008] 为了实现上述任务，本发明采用以下技术方案：[0009] 一种基于自组织拓扑与模型预测的多机器人编队控制方法，包括：[0010] 针对机器人编队，基于拓扑学与RIPF算法生成自组织形成拓扑网络，包括：[0011] 利用有向图和机器人位置序列集合来搭建机器人编队控制框架，通过设置机器人编队构成RIPF所满足的条件，来自组织生成拓扑网络；[0012] 在所述自组织形成拓扑网络的基础上，规划机器人编队的系统路径，包括：[0013] 构建每个机器人的期望路径序列，使用梯度下降法得到t时刻的控制输入；通过设置机器人的安全距离、允许距离，用以引导机器人避免与路径上的障碍物碰撞；利用人工场势法，结合所定义的安全距离和允许距离，对机器人行走过程进行避障；基于机器人期望路径的位置向量及控制输入，构建第一离散化仿射系统；[0014] 对机器人编队进行系统轨迹跟踪，包括：[0015] 构建机器人当前状态与参照状态的跟踪误差，并基于此建立轨迹跟踪系统的运动学模型；定义输入控制向量，令所述跟踪误差趋于零并对其进行逆变换，构建第二离散化仿射系统并设置该系统的约束条件；基于预测输出量构建代价函数，并将代价函数转换为求解优化二次规划问题；利用神经动态优化方法解决各个机器人的二次规划问题，并用下一时刻的输入增量来计算机器人编队的最优化输入，从而控制多机器人编队。[0016] 进一步地，所述利用有向图和机器人位置序列集合来搭建机器人编队控制框架，通过设置机器人编队构成RIPF所满足的条件，来自组织生成拓扑网络，包括：[0017] 定义F＝(G,P)来表示多机器人编队的M个机器人编队控制框架；其中G＝(V,E,A)是一个有向图，表示机器人之间通信拓扑结构；V＝1,2,...,M表示共有M个机器人；E＝{(i,j),i≠j,i、j∈V}为有向边集合，(i,j)∈E表示机器人i可以接收到机器人j的信息；A表示2M由相关加权邻接系数aij构成的相关加权邻接矩阵；P＝(pixy)∈R pixy＝(xi,yi)是机器人i的位置序列，R代表实数集；[0018] 机器人编队构成RIPF所满足的条件：[0019] Num(E)＝2Num(V)‑3[0020][0021][0022][0023][0024] 其中Num(·)表示数量，d(i)为机器人i的度，表示机器人i与其他机器人之间的信息传递关系；dout(i)为机器人i的出度，出度表示接收信息。[0025] 进一步地，所述自组织生成拓扑网络，包括：[0026] a.获取机器人在t时刻的位置信息，将M个机器人在t时刻的位置定义为：[0027] pixy(t)＝[xi(t),yi(t)](i＝1、2、...、M)[0028] 其中，xi(t),yi(t)表示t时刻的位置坐标。[0029] 机器人i与j之间存在一个最小距离dmin：[0030][0031] pixy(0)、pjxy(0)表示初始时刻机器人i、j的位置；[0032] b.将编队中的各机器人视为点，利用DelaunayTriangulation算法绘制散点图作为机器人编队的位置图像；在所获的t时刻位置图像上进行三角剖分得到三角剖分图像，并满足d(i)≤6；[0033] c.在所获的三角剖分图像上进行边界删减，使图像的边数满足Num(E)＝2Num(V)‑3，此时图像为无向图刚性图；[0034] d.从编队中选择一个机器人RL作为领导者，该RL只能接收主机命令，不能从其他机器人接收命令，该机器人出度为0；其他机器人也要满足 条件，给无向图刚性图的边添加方向，得到一个RIPF图像即为自组织形成拓扑网络。[0035] 进一步地，所述使用梯度下降法得到t时刻的控制输入，表示为：[0036][0037] 其中 为t时刻下的控制输入，aij为相关加权邻接系数，ki为机器人i期望路径的输入权值， 是梯度下降法的表示符号；设置一个距离值函数为两个相邻机器人i和j之间的期望距离，i≠ji、j∈V； 表示t时刻机器人i的期望路径的位置向量。[0038] 进一步地，结合所定义的安全距离和允许距离，对机器人行走过程进行避障，包括：[0039] 先将机器人所扫描到障碍物进行排序，再利用障碍物对机器人i的排斥力，定义潜在函数 和控制输入[0040][0041][0042] 其中 表示距离机器人i最近的第c个障碍物的位置，e为自然对数底数，l为整数是随环境变化而变化的障碍数， 为避障权重系数；[0043] 表示t时刻障碍物进入了机器人的安全范围，人工势场开始排斥机器人使机器人避障，若 则表明机器人避障成功；→表示趋近；dsafe为机器人的安全距离，dallow为允许距离；将 与 两个控制输入相加，得到t时刻机器人i期望路径的控制输入[0044] 进一步地，所述基于机器人期望路径的位置向量及控制输入，构建第一离散化仿射系统，具体表示为：[0045][0046] 其中，Δt为采样周期，k为时间状态，k+1为k的下一个状态， 为离散时间下的[0047] 为平滑机器人的运动路径，需要定义一个参考路径：[0048] 定义 为机器人i在参考路径上移动的参考状态向量， 分别表示x、y轴位置以及方向角；[0049] 为机器人进行避障时的参考速度向量，其中分别为线速度、角速度；[0050][0051][0052][0053] 其中γ是正的常数，k0是一个表示避障过程结束的时间实例；pic(k)＝[xic(k)yicT T(k)θic(k)]表示k时刻机器人i的当前路径状态向量，pir(k)＝[xir(k)yir(k)θir(k)] 表示k时刻机器人i的期望路径状态向量；这两个状态向量中的三个参数分别表示对应的x、y轴位置以及方向角。[0054] 进一步地，所述构建机器人当前状态与参照状态的跟踪误差，并基于此建立轨迹跟踪系统的运动学模型，包括：[0055] 根据机器人运动学方程，构建机器人当前状态pic＝[xicyicθic]T与参照状态T的跟踪误差pie＝[xieyieθie] ，目标将pie→0；其中，当前状态、参照状态、跟踪误差中的三个参数分别表示对应的x、y轴位置以及方向角；[0056][0057] 轨迹跟踪系统的运动学模型可以表示为：[0058][0059] 上式中，参数上标圆点表示该参数的一阶导数，ωic、 分别表示当前状态、参照状态下的角速度，vic、 vie分别表示当前状态、跟踪误差、参照状态下的线速度，θie为跟踪误差的方向角，xie为跟踪误差的x轴位置。[0060] 进一步地，所述定义输入控制向量，令所述跟踪误差趋于零并对其进行逆变换，构建第二离散化仿射系统并设置该系统的约束条件，包括：[0061] 定义输入控制向量uie：[0062][0063] 令pie→0并将其进行逆变换：[0064][0065] 构建第二离散化仿射系统：[0066] pi(k+1)＝h(pi(k))+si·ui(k)[0067] 其中：[0068][0069] 该系统的约束条件为：[0070] uimin≤ui(k)≤uimax、Δuimin≤Δui(k)≤Δuimax、pimin≤pi(k)≤pimax[0071] 其中ui(k)表示k时刻机器人i的广义输入向量；Δui(k)＝ui(k)‑ui(k‑1)表示为k时刻机器人i的输入增量；pi(k)表示为k时刻机器人i的广义离散状态误差向量；uimin、Δuimin、pimin分别为广义输入向量、输入增量、广义离散状态误差向量的最小值，uimax、Δuimax、pimax分别为广义输入向量、输入增量、广义离散状态误差向量的最大值。[0072] 进一步地，所述代价函数表示为：[0073][0074] 其中：[0075][0076][0077][0078][0079] 其中，pi(k+1|k)表示机器人i用k时刻的pi(k)对pi(k+1)时刻进行预测的结果，而ui(k|k)、Δui(k|k)表示k时刻ui(k)、Δui(k)的最优预测值，N表示预测域的范围；Nu表示控制时域的范围，而且1≤Nu≤N，Qi和Ri表示权重矩阵。[0080] 进一步地，所述二次规划问题表示为：[0081][0082] 约束条件为：[0083][0084] 其中：[0085][0086][0087] 其中 表示k时刻机器人i的广义预测输入向量； 表示为k时刻机器人i的预测输入增量； 表示为k时刻机器人i的广义离散状态预测误差向量， 分别为广义预测输入向量、预测输入增量、广义离散状态预测误差向量的最小值， 分别为广义预测输入向量、预测输入增量、广义离散状态预测误差向量的最大值，I表示单位矩阵，上标T表示矩阵转置。[0088] 与现有技术相比，本发明具有以下技术特点：[0089] 1.本发明将RIPF生成方法和移动机器人形成方法相结合，建立了一个自组织的蜂群系统。基于机器人的t时刻位置，利用RIPF生成方法得到最有持久通信拓扑结构。该方法避免了人为设定拓扑结构，解决人为设定拓扑结构的缺陷。[0090] 2.本发明采用人工场势法辅助系统路径进行规划，通过设置逆梯度和Tp(k)，使得机器人在避障完成后可以平滑回到所需路径。[0091] 3.本发明应用PDNN组合MPC的方法，重构并解决了预测模型的约束QP问题，大大降低了控制算法的计算复杂度。通过该方法不仅得到了控制机器人的最优输入，还考虑到了机器人速度约束条件和系统的状态，该方法适用于系统的实时QP问题。附图说明[0092] 图1的(a)至(d)为自组织形成拓扑网络的在不同阶段的示意图；[0093] 图2为本发明控制方法的流程示意图；[0094] 图3为自组织形成拓扑网络的具体流程示意图；[0095] 图4为运用本发明控制方法的仿真示意图。具体实施方式[0096] 本发明提出一种自组织生成关系不变持久形成控制算法(Relation‑invariablepersistentformation,RIPF)，RIPF可以保持刚性和边缘关系，减少运动过程中的突变和能量损失。构建通信网络后，运用人工场势法使机器人避障，采用神经动力学组合MPC对机器人编队进行控制，并采用原对偶神经网络(Primal‑DualNeuralNetworks,PDNN)的神经动态方法对编队进行优化。[0097] 参见附图，本发明的基于自组织拓扑与模型预测的多机器人编队控制方法，包含以下内容：[0098] 1.基于拓扑学与RIPF算法生成自组织形成拓扑网络[0099] 利用有向图和机器人位置序列集合来搭建机器人编队控制框架，通过设置机器人编队构成RIPF所满足的条件，来自组织生成拓扑网络。[0100] 1.1定义F＝(G,P)来表示多机器人编队的M个机器人编队控制框架，由有向图和机器人位置序列集合构成；其中G＝(V,E,A)是一个有向图，表示机器人之间通信拓扑结构；V＝1,2,...,M表示共有M个机器人；E＝{(i,j),i≠j,i、j∈V}为有向边集合，(i,j)∈E表示机器人i可以接收到机器人j的信息，相关加权邻接系数aij＜0；如果(j,i)∈E，则aij＞0；如果 或者i＝j，则aij＝0；A表示由aij构成的相关加权邻接矩阵。[0101] 在t时刻时，如果(i,j)∈E，则aij(t)＜0；如果(j,i)∈E,则aij(t)＞0；如果或者i＝j，则aij(t)＝0；A(t)表示由aij(t)构成的相关加权邻接矩阵。P＝(pixy)∈2MR pixy＝(xi,yi)是机器人i的位置序列，R代表实数集。[0102] 1.2构建机器人编队构成RIPF所满足的条件：[0103] Num(E)＝2Num(V)‑3[0104][0105][0106][0107][0108] 其中Num(·)表示数量，d(i)为机器人i的度，表示机器人i与其他机器人之间的信息传递关系；dout(i)为机器人i的出度，出度表示接收信息；入度则表示发送信息。[0109] 1.3自组织生成拓扑网络：[0110] a.获取机器人在t时刻的位置信息，将M个机器人在t时刻的位置定义为：[0111] pixy(t)＝[xi(t),yi(t)](i＝1、2、…、M)[0112] 其中，xi(t),yi(t)表示t时刻的位置坐标。[0113] 为防止机器人间的碰撞，所以认为机器人i与j之间存在一个最小距离dmin：[0114][0115] pixy(0)、pjxy(0)表示0时刻(初始时刻)机器人i、j的位置(初始位置)。[0116] b.将编队中的各机器人视为点，利用DelaunayTriangulation算法绘制散点图(二维)作为机器人编队的位置图像；在所获的t时刻位置图像上进行三角剖分得到三角剖分图像，并满足d(i)≤6机器人度不超过6。[0117] c.在所获的三角剖分图像上进行边界删减，使图像的边数满足Num(E)＝2Num(V)‑3，此时图像为无向图刚性图(RIMRG)。[0118] d.从编队中选择一个机器人RL作为领导者，该RL只能接收主机命令，不能从其他机器人接收命令，所以该机器人出度为0，dout(RL)＝0。其他机器人也要满足条件，给RIMRG图像的边添加方向，得到一个RIPF图像即为自组织形成拓扑网络。自组织形成拓扑网络示意如图1所示，流程图如图3所示。[0119] 2.在所述自组织形成拓扑网络的基础上，规划机器人编队的系统路径[0120] 2.1每个机器人在运动的过程中都希望达到并保持期望的距离，构建t时刻的期望路径序列：[0121][0122] 其中 表示机器人i的期望路径的位置向量，xir、yir表示为机器人i的期望路径的x轴、y轴的输入量，i∈V；构建其一阶模型为 表示机器人i的期望路径的坐标控制输入向量。[0123] 本方案中参数上标圆点是指一阶导数，上标T表示转置，参数后的(t)表示t时刻的该参数，例如 表示t时刻的 即机器人i的期望路径的位置向量，以下不再赘述。[0124] 2.2为使机器人i回到其期望路径上达到并保持期望距离，所以使用梯度下降法得到t时刻的控制输入[0125][0126] 其中 为t时刻下的控制输入，aij为相关加权邻接系数，ki为机器人i期望路径的输入权值， 是梯度下降法的表示符号；设置一个距离值函数为两个相邻机器人i和j之间的期望距离，i≠ji、j∈V。[0127] 2.3通过设置机器人的安全距离、允许距离，用以引导机器人避免与路径上的障碍物碰撞：[0128] 定义dsafe为机器人的安全距离，dallow为允许距离，且dsafe≥dallow。如果机器人i在t时刻其安全距离内出现第c个障碍物，将机器人与障碍物的最近距离点设为并定义序列 表示机器人i遇到的所有障碍物，其中正整数l为随环境变化的障碍物数量。[0129] 2.4利用人工场势法(APF)，结合所定义的安全距离和允许距离，对机器人行走过程进行避障。[0130] 先将机器人所扫描到障碍物进行排序，再利用障碍物对机器人i的排斥力，定义潜在函数 和控制输入[0131][0132][0133] 其中 表示距离机器人i最近的第c个障碍物的位置，e为自然对数底数，l为整数是随环境变化而变化的障碍数， 为避障权重系数。[0134] 表示t时刻障碍物进入了机器人的安全范围，人工势场开始排斥机器人使机器人避障，若 则表明机器人避障成功；→表示趋近；将 与 两个控制输入相加，得到t时刻机器人i期望路径的控制输入[0135] 2.5基于机器人期望路径的位置向量及控制输入，构建第一离散化仿射系统：[0136][0137] 为了满足时间的连续性，所以上文的公式都是连续的。但是在算法中无法无时无刻都将信息进行反馈，所以要设置采样时间，将上述公式进行离散化，当采样时间足够小便可近似认为是连续的。[0138] 其中，Δt为采样周期，k为时间状态，k+1为k的下一个状态， 为离散时间下的 下文中参数后(k)均表示该参数的离散时间下的表示形式，不再赘述。[0139] 为平滑机器人的运动路径，需要定义一个参考路径。一般情况下，机器人因为避障会离开所需路径，避障后仍要返回所需路径，需要利用Tp(k)函数来提高轨迹跟踪的收敛性；Tp(k)是一个关于时间状态k的函数。[0140] 定义 为机器人i在参考路径上移动的参考状态向量， 分别表示x、y轴位置以及方向角。[0141] 为机器人进行避障时的参考速度向量，其中分别为线速度、角速度。[0142][0143][0144][0145] 其中γ是正的常数，k0是一个表示避障过程结束的时间实例。pic(k)＝[xic(k)yicT T(k)θic(k)]表示k时刻机器人i的当前路径状态向量，pir(k)＝[xir(k)yir(k)θir(k)] 表示k时刻机器人i的期望路径状态向量；这两个状态向量中的三个参数分别表示对应的x、y轴位置以及方向角。[0146] 3.对机器人编队进行系统轨迹跟踪[0147] 3.1构建机器人当前状态与参照状态的跟踪误差，并基于此建立轨迹跟踪系统的运动学模型。[0148] 根据机器人运动学方程，构建机器人当前状态pic＝[xicyicθic]T与参照状态T的跟踪误差pie＝[xieyieθie] ，目标将pie→0；其中，当前状态、参照状态、跟踪误差中的三个参数分别表示对应的x、y轴位置以及方向角。[0149][0150] 轨迹跟踪系统的运动学模型可以表示为：[0151][0152] 上式中，参数上标圆点表示该参数的一阶导数，ωic、 分别表示当前状态、参照状态下的角速度，vic、 vie分别表示当前状态、跟踪误差、参照状态下的线速度，θie为跟踪误差的方向角，xie为跟踪误差的x轴位置。[0153] 3.2定义输入控制向量，令所述跟踪误差趋于零并对其进行逆变换，构建第二离散化仿射系统并设置该系统的约束条件。[0154] 定义输入控制向量uie：[0155][0156] 令pie→0并将其进行逆变换：[0157][0158] 构建第二离散化仿射系统：[0159] pi(k+1)＝h(pi(k))+si·ui(k)[0160] 其中：[0161][0162] 该系统的约束条件为：[0163] uimin≤ui(k)≤uimax、Δuimin≤Δui(k)≤Δuimax、pimin≤pi(k)≤pimax[0164] 其中ui(k)表示k时刻机器人i的广义输入向量；Δui(k)＝ui(k)‑ui(k‑1)表示为k时刻机器人i的输入增量；pi(k)表示为k时刻机器人i的广义离散状态误差向量；uimin、Δuimin、pimin分别为广义输入向量、输入增量、广义离散状态误差向量的最小值，uimax、Δuimax、pimax分别为广义输入向量、输入增量、广义离散状态误差向量的最大值。[0165] 3.3基于预测输出量构建代价函数，并将代价函数转换为求解优化二次规划问题。[0166] 预测输出量可以表示为：[0167][0168] 其中：[0169][0170] 并定义向量：[0171][0172][0173][0174] 其中，pi(k+1|k)表示机器人i用k时刻的pi(k)对pi(k+1)时刻进行预测的结果，其他参数的含义按照这种形式进行类比；即符号|表示预测，参数中含有|即该参数|之后的时刻对|之前时刻的预测；而ui(k|k)、Δui(k|k)表示k时刻ui(k)、Δui(k)的最优预测值，N表示预测域的范围，Nu表示控制时域的范围，而且1≤Nu≤N。[0175] 构建代价函数：[0176][0177] 其中Qi和Ri表示权重矩阵。[0178] 考虑到神经动态优化分布式预测模型的分布式特性，对于每个机器人i的代价函数可以看作是求解最优化二次规划(QP)问题[0179][0180] 约束条件为：[0181][0182] 其中：[0183][0184][0185] 其中 表示k时刻机器人i的广义预测输入向量； 表示为k时刻机器人i的预测输入增量； 表示为k时刻机器人i的广义离散状态预测误差向量， 分别为广义预测输入向量、预测输入增量、广义离散状态预测误差向量的最小值， 分别为广义预测输入向量、预测输入增量、广义离散状态预测误差向量的最大值，I表示单位矩阵，上标T表示矩阵转置。[0186] 3.4利用神经动态优化方法解决各个机器人的二次规划问题，并用下一时刻的输入增量来计算机器人编队的最优化输入，从而控制多机器人编队。据分析可得：[0187][0188] 其中， 表示对 关于t求导，K^为投影算子，γ为一个正数，W(·)N(·)为两个连续可微函数：[0189][0190][0191] 其中， 矩阵的右上角+号代表此矩阵的伪逆； 即为最优解，这样便优化了多机器人编队。[0192] 本发明整体流程如图2所示；本发明方法的应用过程如下：[0193] 步骤1，对于具有M个移动机器人的机器人编队，获取机器人编队中各机器人的初2M始位置P(0)＝(pixy(0))∈R ；根据前述第1部分的基于拓扑学与RIPF算法生成自组织形成拓扑网络，基于初始位置P(0)生成拓扑网络。[0194] 步骤2，适当地设定时间k(令k＝1)，设置采样周期Δt，选定预测域的范围N和控制时的范围Nu，设置γ的值、输入权值ki与权重矩阵Qi和Ri。[0195] 步骤3，计算机器人i的期望路径的坐标控制输入向量[0196] 步骤4，通过构建离散化仿射系统，计算出机器人i所需达到的位置 从而得到该机器人在参考路径上移动的参考状态向量 和机器人进行避障时的参考速度向量[0197] 步骤5，根据当前状态与参照状态的跟踪误差pie，构建轨迹跟踪系统的运动学模型并形成QP问题。[0198] 步骤6，求解步骤5中的QP问题，得到第i个机器人最优的预测输入增量令pie→0通过逆变换计算出该机器人的线速度输入量vic(k+1)和角速度输入量ωic(k+1)，从而通过其对机器人进行控制。[0199] 步骤7，对每个机器人都进行QP问题求解，应用当前时刻所计算出的线速度输入量、角速度输入量作为下一时刻对机器人进行控制的输入向量，驱动机器人在这段时间内移动，并更新机器人的当前状态向量[0200] pic(k+1)＝[xic(k+1)yic(k+1)θic(k+1)]T。[0201] 步骤8，若运动继续，令k＝k+1并返回步骤3；否则机器人群停止运动。[0202] 本发明将RIPF生成方法和移动机器人形成方法相结合，建立了一个自组织的蜂群系统。基于机器人的t时刻位置，利用RIPF生成方法得到最有持久通信拓扑结构。该方法避免了人为设定拓扑结构，解决人为设定拓扑结构的缺陷；采用人工场势法辅助系统路径进行规划，通过设置逆梯度和Tp(k)函数，使得机器人在避障完成后可以平滑回到所需路径；应用PDNN结合MPC的方法，重构并解决了预测模型的约束QP问题，大大降低了控制算法的计算复杂度。本发明通过机器人群在运动中自组织生成最优的拓扑结构，维持了机器人间的良好通信，并通过模型预测控制对机器人的运动进行约束，从而实现了良好的多机器人编队运动。图4为本发明控制机器人编队的仿真示意图，通过仿真可以看出，机器人在避障完成后可以平滑回到所需路径，该方法的控制效果显著。[0203] 以上实施例仅用以说明本申请的技术方案，而非对其限制；尽管参照前述实施例对本申请进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本申请各实施例技术方案的精神和范围，均应包含在本申请的保护范围之内。

专利地区：广东

专利申请日期：2022-03-21

专利公开日期：2024-07-26

专利公告号：CN114647244B