1.一种基于仿射编队与轨迹优化的多机器人路径规划方法，其特征在于，所述方法包括以下步骤：步骤一，使用多机器人路径规划器CL‑CBS为每个机器人生成一条从起点到达终点的初始路径，用于生成编队路径以及作为轨迹优化问题的初始解；CL‑CBS有两层搜索框架，在上层搜索中，构建车身冲突树来解决检测到的冲突，从而上层搜索树中只需要考虑规划方案的路径间是否存在冲突，而不必考虑路径规划问题；而在下层搜索中，使用时空混合A*搜索算法来对为各个机器人规划满足自身约束的最佳路径；

步骤二，将多机器人目标编队构建为无向图，并计算出对应的应力矩阵Ω；

步骤三，先生成领导者的路径，再使用编队控制律求出跟随者的路径来作为编队路径，仿射编队控制律基于领航‑跟随法，而且在二维空间下至少需要3个领导者，选择至少3个机器人作为领导者，其余机器人作为跟随者；

步骤四：先根据初始路径和障碍物信息构建安全走廊，再设计轨迹优化问题并进行求解，得到目标路径。

2.如权利要求1所述的基于仿射编队与轨迹优化的多机器人路径规划方法，其特征在于，所述步骤一的过程如下：步骤1.1：将每个机器人的起始状态和终止状态输入到下层搜索中，机器人状态表示为Tz＝[x,y,θ] ，(x,y)表示机器人的位置，θ表示机器人的偏航角；；下层搜索第一次使用时会忽略机器人之间的影响为每一个机器人规划出一条最优路径，每个机器人的路径为一系列离散的带有时间戳的路径点z(t)，其中路径点之间都应满足式(1)的运动学模型，每个机器人的运动学模型定义为，T

其中机器人的控制输入表示为u＝[v,ω] ，v表示机器人的线速度，ω表示机器人的角速度，z(t)表示机器人在时间t的状态，u(t)表示机器人在时间t的控制输入，表示每个机器人都受到其最大速度的限制，即机器人的线max max

速度不能超过v ，角速度不能超过ω ；

步骤1.2：上层搜索会按时间顺序检查是否出现两个机器人发生碰撞的情况，假设最先发生的冲突为(ai,aj,t)，它表示机器人ai和机器人aj的路径在时间t时刻发生了碰撞，车身冲突树会生成两个子节点来解决此冲突，，这两个节点分别会为这两个机器人添加一个约束，使其避免在时间t时刻经过该地点；

步骤1.3：对新增约束的机器人重新进行路径规划，生成满足约束的路径，其它机器人的路径保持不变；然后节点按照所有机器人的路径长度之和进行排序，长度最短的节点优先进行冲突检测，直到找到没有冲突的结果。

3.如权利要求1或2所述的基于仿射编队与轨迹优化的多机器人路径规划方法，其特征在于，所所述步骤二中，机器人之间的联系用无向图 表示，其中 为图的顶点，n为机器人的数量，ε为图的边，顶点i的邻居集合为 应力

{ωij}(i,j)∈是每条边上的标量权重，其中ωij＝ωji，通过将目标队形隐藏在平衡应力权重中，将机器人最终收敛到目标编队，平衡应力满足，T

其中pi＝[xi,yi]是机器人ai的位置，式(2)理解为所有作用在机器人ai上的力达到平衡，合力为0，应力矩阵Ω与应力ωij的关系为，计算应力矩阵一般有两种方法，动态规划求解和线性矩阵不等式求解，使用线性矩阵不等式求解法，首先构建矩阵E为，其中n为机器人的数量，d为空间维度，d＝2，m为图 的边数， 表示图 的关联矩阵， 是H的第i列， 表示为目标队形机器人之间的相对位置，增广矩阵 定义为，

T

令 是目标编队的应力向量，由Ω ＝H diag(w)H和 知

由diag(w)hi＝diag(hi)w，两者位置互

换，得到 即Ew＝0，于是定义 是Null(E)的一组基，通

过计算E的奇异值分解得到，可得 其中 是一组待定系数；

接下就是确定系数 假设 的奇异值分解为 令U＝[U1,

U2]，其中U1为U的前d+1列；因为 的秩为d+1，所以U1为 的列空间，U2为 的零空间；因为 故ΩU1＝0，所以Ω在前d+1维秩为0，于是剩下的n‑d‑1维需要满秩，即Ω在 的零空间上正定，得 将 代入得，通过求解上式的线性矩阵不等式得到 也就得到了应力矩阵，使用Matlab的LMI工具箱进行求解。

4.如权利要求1或2所述的基于仿射编队与轨迹优化的多机器人路径规划方法，其特征在于，所述步骤三的过程如下：步骤3.1：生成3个领导者的路径，由于领导者之间的相对位置变化会影响整体队形的变化，为了保持队形，使用l‑β法由一个领导者a1的路径计算出另外两个领导者a2和a3的路径，偏航角与a1保持一致；l是距离，β是角度，这两个的值根据编队队形的要求设定；

领导者a2的位置通过式(7)计算得出，另一个领导者a3的路径计算也类似，领导者a1的路径就是步骤一的初始路径中a1的路径，步骤3.2：使用编队控制律生成跟随者路径，得到步骤3.1生成的领导者路径后，应用式(8)得到跟随者的路径，T

其中v＝[vx,vy]表示机器人x轴和y轴上的速度， 表示代表跟随者的顶点；

步骤3.1生成的领导者路径由离散的路径点组成，每个路径点的时间间隔为1秒，由此计算出领导者的速度，反过来根据计算出的跟随者速度计算出跟随者的路径，这样就得到了所有机器人的编队路径，这个路径作为轨迹优化问题中的目标函数。

5.如权利要求1或2所述的基于仿射编队与轨迹优化的多机器人路径规划方法，其特征在于，所述步骤四中，使用路径点参数化，将[x,y,θ,v,ω]作为优化变量，过程如下：步骤4.1：构建安全走廊，将机器人位置限制在安全走廊内，走廊不与障碍物相交，那么在走廊中的机器人是绝对安全的；由CL‑CBS得到的每个机器人的初始路径表示为总共M+1个路径点，路径中第i个线段由Ii＝＜zi‑1→zi＞表示，在路径中的每一条线段周围生成一个凸多面体Si来构成一条安全走廊，为了确保安全，首先应满足以下条件，其中R(z)表示机器人所占据的区域，车身形状函数R由机器人的大小尺寸等因素共同决定，O表示障碍物空间，这些凸多面体的集合构成了安全走廊，用SC(π)＝{Si|i＝1,…,M}表示，要注意安全走廊需要保持顺序连接，因此要满足以下条件，每个安全走廊以两点之间的线段为中心，向x轴和y轴正负方向进行扩展，直到遇到障碍物或者超出边际；

步骤4.2：构建轨迹优化问题，得到路径点和安全走廊后，开始进行轨迹优化，轨迹优化的目标是为了让轨迹变得平滑，具有动力学可行性还能保持队形；设计了一个非线性优化问题，并使用非线性优化求解器IPOPT进行求解得到问题的局部最优解，IPOPT使用内点法用以解决以下形式的一般非线性规划问题，n L U

式(11)中x∈R为优化变量，下限为x，上限为x；函数f为目标函数；函数g为一般非线L U L U性约束，其下限为g，上限为g，通过设置g＝g来制定等式约束；

构建的非线性优化问题为：

其中 表示由步骤三生成的编号为j的机器人在时间i时的编队路径点、N表示机器人的j j数量、s 是编号为j的机器人的起始状态、g是编号为j的机器人的终止状态、dsafe为机器人之间的安全距离， 和 为优化变量，其初始值由步骤一得到的初始路径提供；目标函数由三部分组成：第一部分是为了让轨迹保持编队，对远离编队路径的轨迹进行惩罚；第二部分是让轨迹变得平滑，尽量为直线；第三部分使得连续两个控制输入之间的差异变小，使得速度的变化尽可能的小；另外置了5个约束，第一个为起点和终点的限制；第二个保证路径点满足运动学模型，确保运动学可行；第三个为安全走廊限制，使得轨迹点的位置在安全走廊内，确保机器人不与障碍物碰撞；第四个限制每个机器人的控制输入在允许范围内；最后一个为机器人之间的距离限制，杜绝机器人间的碰撞；

步骤4.3：处理路径数据并代入求解，生成最终的路径，删除掉由步骤一生成的路径中后面的一些路径点，即带有Reeds–Shepp路径的部分，并对删除后的路径进行补齐使得每个机器人的路径点数量相同，补齐的具体做法是重复最后的路径点直到每个路径的路径点数量相同，理解为机器人待在原地不动。