一种适用于机器人集群的最优虚拟管道的规划方法

1.本发明涉及一种适用于机器人集群的最优虚拟管道的规划方法，尤其是一种在起止区域和障碍物避碰约束下的，基于路径搜索算法和最优化二次规划问题生成最优虚拟管道来解决集群轨迹规划和障碍物环境穿越的方法，属于集群轨迹规划领域。


背景技术：

2.目前，集群轨迹规划在许多领域有着广泛的应用，例如在无人机集群协同穿越障碍物密集环境时便涉及集群避碰轨迹规划问题。针对集群穿越障碍物环境的任务，需要为每个集群个体规划出避碰轨迹而每个个体的最优轨迹规划的迭代计算过程需要花费大量算力，因此研究如何以最小算力实现大规模集群最优轨迹的生成具有十分重要的意义。
3.虚拟管道在之前的相关研究中定义为一个适用于机器人集群流动的空间。在障碍物环境中，集群在已经规划好的虚拟管道中运动，因而可以在计算量较小的情况下安全流畅地穿行到目标区域。对于虚拟管道的研究可以总结为两个问题，即虚拟管道规划问题和虚拟管道穿行控制问题。本发明的内容属于虚拟管道的规划问题。
4.对于轨迹规划问题，目前的一种比较流行的解决方法是为集群中每个个体分别解一个最优化问题并通过不断迭代得到最优解，即最优轨迹的参数化表示，这种方法在集群数量较少时可以得到较为出色的效果，但是当集群数量较多，环境较为复杂时，其劣势便显现出来，总计算量随着集群数量的增加而增大。因此，为了避免计算资源的浪费，本发明申请提出了一种基于路径搜索算法和最优化二次规划问题生成最优虚拟管道来解决集群轨迹规划和障碍物环境穿越的方法，该方法通过在环境中进行远少于集群数量的有限次路径搜索和最优轨迹规划过程来生成一个最优虚拟管道，集群个体在最优虚拟管道中的任意一个位置都可以通过简单纺射运算得到与解最优化问题等价的最优轨迹，因为本方法运算速度快，效率高。在轨迹生成完成后，利用现有成熟的轨迹跟踪的控制方法对集群进行控制，使得集群中个体在互相不碰撞的情况下完成任务，到达目标点。


技术实现要素：

5.本发明提供了一种适用于完整约束机器人集群的最优虚拟管道的规划方法。它解决了目前机器人集群协同穿越障碍物环境时总计算量大、易分散的问题，提供了简单易用的解决方案，具有计算量低、通用性强的特点。
6.本发明首先建立完整约束机器人集群模型，然后建立虚拟管道相关模型。
7.本发明中用二阶质点模型描述完整约束机器人集群，对于集群中任意机器人i，其运动学模型表示为：
[0008][0009][0010]
其中，表示机器人i的位置，表示机器人i的速度，u
i,c
表示输入控制
指令。如图1所示，虚拟管道是一个n维空间中的集合，由四元组表示，其中：
[0011]
1.和称为终端，是n维空间中不相交的有界凸子集。
[0012]
2.f是一个微分同胚：因此有一组有序对
[0013]
3.h是一个平滑映射：其中使得h((q0,qm),0)＝q0，h((q0,qm),1)＝qm。函数h((q0,qm),t)称为有序对(q0,qm)的轨迹。
[0014]
一个在处的虚拟管道的横截面定义为
[0015][0016]
虚拟管道的表面是的边界，定义为
[0017]
在定义最优虚拟管道之前首先需要定义最优轨迹：设g为与轨迹h((q0,qm),t)相关的目标函数，若
[0018][0019]
其中，是轨迹候选集，则关于有序对(q0,qm)的轨迹h
*
为关于罚函数g的最优轨迹。
[0020]
基于以上定义，最优虚拟管道定义为：若在虚拟管道中的每个轨迹关于目标函数g都是最优的，则该虚拟管道为最优虚拟管道，即
[0021]
基于管道的一些特定性质，线性管道被定义为：对于一个虚拟管道若f是线性的，且h关于任意有序对是线性的，则该虚拟管道是线性的。
[0022]
有凸包终端的线性虚拟管道被定义为：对于一个虚拟管道若其满足：
[0023]
1.终端分别为有限集{q
0,k
}和{q
m,k
}的凸包。
[0024]
2.该虚拟管道是线性的。
[0025]
则该虚拟管道被称为有凸包终端的线性虚拟管道。
[0026]
本发明提出了优化相关引理和理论，为规划方法提供理论基础。
[0027]
引理1：一个凸优化问题的标准形。假设该问题形式如下：
[0028][0029]
其中，fi(x)(i＝0,1,...,m)是关于x＝[x
0 x
1 ... xn]
t
的凸函数，的凸函数，关于x是线性的。当b＝bi时，可获得最优解xi。定义i＝1,2,...,q，则是可行的且最优的。
[0030]
理论1：对于一个有凸包终端的线性虚拟管道其终端分别为有限集{q
0,k
}和{q
m,k
}的凸包，若
[0031]
1.对于每个有序对，其轨迹均可以线性化表示为h((q0,qm),t)＝c(t)x，其中c(t)是仅关于t的矩阵，x是参数向量。
[0032]
2.对于所有的有序对(q
0,k
,q
m,k
)，其轨迹关于目标函数f0是最优的，且其最优化问题的约束fi(x)≤0,i＝1,...,m是凸的。
[0033]
则关于目标函数f0是最优的。
[0034]
根据理论1，本发明就可以在满足特定条件的情况下通过优化有限条轨迹生成最优虚拟管道，最优虚拟管道中存在无穷条最优轨迹。这些最优轨迹均可以通过有限条最优轨迹的凸组合的形式表示，与直接解最优化问题的解是等价的。
[0035]
本发明提出了一种基于路径搜索算法和最优化二次规划问题生成最优虚拟管道来解决集群轨迹规划和障碍物环境穿越的方法，其规划流程图如图2所示，在之前定义的基础上，其实现步骤具体如下：
[0036]
步骤一：输入终端
[0037]
输入虚拟管道的起始终端和目标终端其分别由有限集{q
0,k
},{q
m,k
}的凸包构成，用公式表示为
[0038][0039]
步骤二：通过映射f构建有序对集
[0040]
线性映射f有多种定义方法，在这里，不失一般性，我们定义映射f为
[0041]qm,k
＝f(q
0,k
),k＝1,2,...,q.
ꢀꢀꢀꢀꢀꢀ
(5)
[0042]
因而可以构建有序对集其中存在有序对(q
0,k
,q
m,k
)。
[0043]
步骤三：路径搜索
[0044]
根据步骤二的有序对集结果，分别对有序对集中的有序对进行路径搜索，并对路径进行归一参数化。
[0045]
s31、路径搜索
[0046]
对每个有序对(q
0,k
,q
m,k
)(k＝1,2,...,q)，利用现有的路径搜索算法(例如rrt*，a*)得到m+1个路径点{q
i,k
}(i＝0,1,...,m)。在这里假设所有有序对的路径点数是相同的，否则会导致优化变量的长度不同，从而不满足引理1的条件要求。
[0047]
s32、参数化路径点
[0048]
对于每个有序对(q
0,k
,q
m,k
)(k＝1,2,...,q)，为了参数化路径点{q
i,k
}(i＝0,1,...,m)，u
i,k
被分配到对应的路径点q
i,k
。因此，一个关于uk的划分被首先设计出来，被叫做节点{u
i,k
}。弦长参数化方法作为一种在工程中广泛应用的方法被在此处用于生成节点{u
i,k
}，可以表示为
[0049][0050]
值得注意的是，在这里对于每个有序对由于路径点之间的弦长不一样，从而导致生成的节点通常是不一样的。为了方便起见，所有的有序对被期望有一致的节点，受到张量积曲面参数化方法的启发，所有有序对的公共节点{ui}被设计为所有节点的算术平均值，即为
[0051][0052]
s33、参数归一化
[0053]
对公共节点{ui}进行参数变换得到时间节点{ti}，即
[0054][0055]
步骤四：优化轨迹
[0056]
在步骤三中生成的路径由于存在尖角从而不适用于机器人个体的动力学模型，因此基于生成路径，通过解优化问题得到最优轨迹。
[0057]
s41、轨迹参数化
[0058]
对于每个有序对(q
0,k
,q
m,k
)(k＝1,2,...,q)，其轨迹h(t)可以用n阶分段多项式表示为
[0059][0060]
其中，对于任意的t∈[t
k-1
,tk]，轨迹h(t)可以表示为矩阵形式
[0061]hk
(t)＝c(t)xkꢀꢀꢀꢀꢀꢀꢀ
(10)
[0062]
其中，c(t)＝[i
d ti
d t2i
d ... tnid]，
[0063][0064]
s42、构建约束
[0065]
约束包括终端约束、中间条件约束以及包括障碍物约束在内的其他约束。终端约束和中间点约束作为问题的线性等式约束，而其他约束当作不等式约束。
[0066]
s421、终端约束
[0067]
终端约束可以表示为
[0068][0069]
其中，和是在起始点和目标点的p阶条件。
[0070]
s422、中间条件约束
[0071]
中间条件约束可以表示为
[0072][0073]
其中，p＝0,1,2,...kr,i＝1,2,...,m-1，
[0074][0075]
方便起见，我们定义
[0076][0077]
因而可以将(12)转写为
[0078][0079]
基于(14)，等式约束为
[0080][0081]
其中，
[0082][0083][0084][0085]
s423、不等式约束
[0086]
为了保证轨迹不与障碍物碰撞，需要构建不等式凸约束。其他约束也可以表示为不等式凸约束的形式即
[0087]fi
(x)≤0,i＝1,...m.
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(16)
[0088]
s43、构建目标函数
[0089]
最优化问题可以将最小化能量消耗作为目标，构建二次规划问题的目标函数如下
[0090][0091]
s44、构建最优化问题
[0092]
基于步骤42和步骤43的内容，可以得到如下的最优化问题
[0093][0094]
即为引理1的形式。通过解q个最优化问题就可以得到q个有序对集{(q
0,k
,q
m,k
)}的最优轨迹h
*
((q
0,k
,q
m,k
),t)。
[0095]
步骤五：构建最优虚拟管道
[0096]
基于理论1，虚拟管道(c0,c1,f,h
*
)是最优的。
[0097]
步骤六：输出无穷多最优轨迹
[0098]
对于起始终端中的任意一个点q0(θ)，其对应的目标点被映射f分配为qm(θ)，其最优轨迹表示为
[0099][0100]
其中，给每个完整约束机器人分配相应的轨迹，将轨迹的二阶导作为参考指令，机器人跟踪参考指令，即可实现集群的轨迹规划。
[0101]
本发明一种适用于机器人集群的最优虚拟管道的规划方法，其优点在于能够不用解优化问题的情况下通过已知的最优解的仿射组合得到等价的最优轨迹，计算量大幅度减小，具有较强的通用性，提供了简单易用的机器人集群穿行障碍物环境的解决方案。
附图说明
[0102]
图1是虚拟管道示意图。
[0103]
图2是最优虚拟管道规划流程图。
[0104]
图3a是虚拟管道终端位置图。
[0105]
图3b是路径搜索示意图。
[0106]
图3c是优化轨迹示意图。
[0107]
图3d是最优虚拟管道示意图。
[0108]
图4是插值误差示意图。
[0109]
图中符号说明如下：
[0110]
图3中的符号说明：q
0,1
,q
0,2
表示凸包终端的基向量，q
m,1
,q
m,2
表示凸包终端的基向量，表示最优轨迹。
具体实施方式
[0111]
下面结合附图和实施例，对本发明的技术方案做进一步的说明。
[0112]
实施例：无人机作为典型的完整约束机器人，适合进行轨迹规划的应用。现有11架无人机在环境中等间距放置在一平面上，需要完成目标位置分配和路径规划。其实现步骤
具体如下：
[0113]
步骤一：输入终端
[0114]
输入终端在障碍物地图中的位置，如图3a所示，终端分别表示为
[0115][0116][0117]
步骤二：通过映射f构建有序对集
[0118]
定义映射f为qm(θ)＝f(q0(θ))，则构建出有序对
[0119][0120]
其中，存在有序对(q
0,1
,q
m,1
),(q
0,2
,q
m,2
)。
[0121]
步骤三：路径搜索
[0122]
根据步骤二的有序对集结果，分别对有序对集中的有序对(q
0,1
,q
m,1
),(q
0,2
,q
m,2
)进行路径搜索，并对路径进行归一参数化。
[0123]
s31、利用路径搜索算法rrt*分别为有序对(q
0,1
,q
m,1
),(q
0,2
,q
m,2
)找到6个路径点，即m＝5，如图3b所示折线，其具体位置坐标如下表1所示，单位为米：
[0124][0125]
表1
[0126]
s32、用弦长参数化方法分别对两个有序对的路径点进行参数化得到节点{u
i,1
},{u
i,2
}，如下表2所示，单位为米：
[0127][0128]
表2
[0129]
计算所有节点的算数平均值可以得到所有路径点的公共节点{ui}，其具体数值如下表3所示，单位为米：
[0130]
[0131]
表3
[0132]
s33、对公共节点{ui}进行参数归一化得到归一化节点{ti}，即表4所示：
[0133][0134]
表4
[0135]
步骤四：优化轨迹
[0136]
在步骤三中生成的路径由于存在尖角从而不适用于机器人个体的动力学模型，因此基于生成路径，通过解优化问题得到两个有序对的最优轨迹和如图3c所示。
[0137]
步骤五：构建最优虚拟管道
[0138]
基于理论1，虚拟管道(c0,c1,f,h
*
)是最优的。
[0139]
步骤六：输出无穷多最优轨迹
[0140]
已知无人机等间距放置，因此可得每架无人机的最优轨迹，如图3d所示，表示为
[0141][0142]
至此，最优管道规划完成，为每架无人机进行了路径规划，且与直接解优化问题相比较，其结果一致，与解优化问题得到的解的误差，如图4所示，极小可以忽略不计。

技术特征：
1.一种适用于机器人集群的最优虚拟管道的规划方法，其特征在于：包括如下步骤：步骤一：输入终端输入虚拟管道的起始终端和目标终端其分别由有限集{q
0,k
},{q
m,k
}的凸包构成，用公式表示为步骤二：通过映射f构建有序对集线性映射f有多种定义方法，在这里，不失一般性，定义映射f为q
m,k
＝f(q
0,k
),k＝1,2,...,q；
ꢀꢀꢀꢀ
(2)构建有序对集其中存在有序对(q
0,k
,q
m,k
)；步骤三：路径搜索根据有序对集结果，分别对有序对集中的有序对进行路径搜索，并对路径进行归一参数化；步骤四：优化轨迹在生成的路径由于存在尖角从而不适用于机器人个体的动力学模型，因此基于生成路径，通过解优化问题得到最优轨迹；步骤五：构建最优虚拟管道虚拟管道(c0,c1,f,h
*
)是最优的；步骤六：输出无穷多最优轨迹对于起始终端中的任意一个点q0(θ)，其对应的目标点被映射f分配为q
m
(θ)，其最优轨迹表示为其中，给每个完整约束机器人分配相应的轨迹，将轨迹的二阶导作为参考指令，机器人跟踪参考指令，即实现集群的轨迹规划。2.根据权利要求1所述的一种适用于机器人集群的最优虚拟管道的规划方法，其特征在于：在步骤三中，路径搜索为：对每个有序对(q
0,k
,q
m,k
)(k＝1,2,...,q)，利用现有的路径搜索算法得到m+1个路径点{q
i,k
}(i＝0,1,...,m)；设所有有序对的路径点数是相同的，否则会导致优化变量的长度不同。3.根据权利要求1所述的一种适用于机器人集群的最优虚拟管道的规划方法，其特征在于：在步骤三中，参数化路径点为：对于每个有序对(q
0,k
,q
m,k
)(k＝1,2,...,q)，为了参数化路径点{q
i,k
}(i＝0,1,...,m)，u
i,k
被分配到对应的路径点q
i,k
；u
k
的划分被设计出来，叫做节点{u
i,k
}；弦长参数化方法用于生成节点{u
i,k
}，表示为：
对于每个有序对由于路径点之间的弦长不一样，从而导致生成的节点是不一样的；为了方便起见，所有的有序对被期望有一致的节点，所有有序对的公共节点{u
i
}被设计为所有节点的算术平均值，即为：4.根据权利要求3所述的一种适用于机器人集群的最优虚拟管道的规划方法，其特征在于：在步骤三中，参数归一化为：对公共节点{u
i
}进行参数变换得到时间节点{t
i
}，即5.根据权利要求1所述的一种适用于机器人集群的最优虚拟管道的规划方法，其特征在于：在步骤四中，轨迹参数化为：对于每个有序对(q
0,k
,q
m,k
)(k＝1,2,...,q)，其轨迹h(t)用n阶分段多项式表示为：其中，对于任意的t∈[t
k-1
,t
k
]，轨迹h(t)表示为矩阵形式：h
k
(t)＝c(t)x
k
ꢀꢀꢀꢀꢀꢀ
(8)其中，c(t)＝[i
d ti
d t2i
d ... t
n
i
d
]，6.根据权利要求5所述的一种适用于机器人集群的最优虚拟管道的规划方法，其特征在于：在步骤四中，终端约束表示为：
其中，和是在起始点和目标点的p阶条件。7.根据权利要求6所述的一种适用于机器人集群的最优虚拟管道的规划方法，其特征在于：在步骤四中，中间条件约束表示为：其中，p＝0,1,2,...k
r
,i＝1,2,...,m-1，1，方便起见，定义：因而将(10)转写为：基于(12)，等式约束为：其中，其中，
8.根据权利要求7所述的一种适用于机器人集群的最优虚拟管道的规划方法，其特征在于：在步骤四中，不等式约束为：为了保证轨迹不与障碍物碰撞，需要构建不等式凸约束；其他约束表示为不等式凸约束的形式即：f
i
(x)≤0,i＝1,...m(14)最优化问题可以将最小化能量消耗作为目标，构建二次规划问题的目标函数如下：得到如下的最优化问题：通过解q个最优化问题得到q个有序对集{(q
0,k
,q
m,k
)}的最优轨迹h
*
((q
0,k
,q
m,k
),t)。9.根据权利要求1所述的一种适用于机器人集群的最优虚拟管道的规划方法，其特征在于：用二阶质点模型描述完整约束机器人集群，对于集群中任意机器人i，其运动学模型表示为：表示为：其中，表示机器人i的位置，表示机器人i的速度，u
i,c
表示输入控制指令；虚拟管道是一个n维空间中的集合，由四元组表示，其中：1.和称为终端，是n维空间中不相交的有界凸子集；2.f是一个微分同胚：因此有一组有序对3.h是一个平滑映射：其中使得h((q0,q
m
),0)＝q0，h((q0,q
m
),1)＝q
m
；函数h((q0,q
m
),t)称为有序对(q0,q
m
)的轨迹；一个在处的虚拟管道的横截面定义为虚拟管道的表面是的边界，定义为
在定义最优虚拟管道之前首先需要定义最优轨迹：设g为与轨迹h((q0,q
m
),t)相关的目标函数，若其中，是轨迹候选集，则关于有序对(q0,q
m
)的轨迹h
*
为关于罚函数g的最优轨迹；最优虚拟管道定义为：若在虚拟管道中的每个轨迹关于目标函数g都是最优的，则该虚拟管道为最优虚拟管道，即基于管道的一些特定性质，线性管道被定义为：对于一个虚拟管道若f是线性的，且h关于任意有序对是线性的，则该虚拟管道是线性的；有凸包终端的线性虚拟管道被定义为：对于一个虚拟管道若其满足：1.终端分别为有限集{q
0,k
}和{q
m,k
}的凸包；2.该虚拟管道是线性的；则该虚拟管道被称为有凸包终端的线性虚拟管道。10.根据权利要求9所述的一种适用于机器人集群的最优虚拟管道的规划方法，其特征在于：提出设置如下：设置1：一个凸优化问题的标准形；设该问题形式如下：其中，f
i
(x)(i＝0,1,...,m)是关于x＝[x
0 x
1 ... x
n
]
t
的凸函数，的凸函数，关于x是线性的；当b＝b
i
时，获得最优解x
i
；定义；定义则是可行的且最优的；设置2：对于一个有凸包终端的线性虚拟管道其终端分别为有限集{q
0,k
}和{q
m,k
}的凸包，若1.对于每个有序对，其轨迹均能线性化表示为h((q0,q
m
),t)＝c(t)x，其中c(t)是仅关于t的矩阵，x是参数向量；2.对于所有的有序对(q
0,k
,q
m,k
)，其轨迹关于目标函数f0是最优的，且其最优化问题的约束f
i
(x)≤0,i＝1,...,m是凸的；则关于目标函数f0是最优的。

技术总结
本发明提出一种适用于机器人集群的最优虚拟管道的规划方法，包括如下步骤：步骤一：输入终端输入虚拟管道的起始终端和目标终端其分别由有限集{q


技术研发人员：全权 毛鹏达
受保护的技术使用者：北京航空航天大学
技术研发日：2023.04.18
技术公布日：2023/8/5