CN113485418B - 一种柔性绳系约束多机器人的轨迹生成方法 - Google Patents

Abstract

本发明涉及一种柔性绳系约束多机器人的轨迹生成方法，属于机器人轨迹规划研究领域。将整个飞行器协同搬运***看做一个球体，以绳索长度为半径；对于每一个飞行器，设定RRT*的随机扩展向量，设定好起点和终止点，进行路径搜索，得到一组离散的向量，作为初始轨迹；对每个维度的连续数据进行反解，求得对应的B样条曲线控制点参数作为优化的初始参数；选择所有的B样条曲线控制点作为优化变量，将飞行器的轨迹优化定义为一个非线性优化问题，对该优化问题进行非线性优化即可解出优化向量中每个维度的B样条曲线的最优控制点值，从而得到每个维度的多项式表达式，实时解出整个飞行器编队的飞行路径。

Description

一种柔性绳系约束多机器人的轨迹生成方法

技术领域

本发明属于机器人轨迹规划研究领域，具体涉及一种用于柔性绳系约束的多机器人的轨迹生成方法。

背景技术

近年来，多机器人协同工作变得越来越常见，常见的应用环境有多无人机编队表演、多智能小车编队表演、无人机与智能小车空地协同作业、多机械臂联合维修等等。使用多个机器人相互配合，协同作业可以增加***的稳定性、提高***的工作强度并且能够使***完成更多复杂的任务。同样多机器人协作带来的就是***的复杂度上升。多机器人协同作业一个常见的问题就是机器人的轨迹规划问题，尤其是涉及多个机器人后，机器人相互之间在时空上要避免碰撞，完成任务的同时也要保证机器人自身的安全。所以对于机器人的轨迹规划成为了一种必然的需求。

如今，一种多飞行器协同搬运的物资的任务逐渐展现出来，相比较于单飞行器，多飞行器协同作业能够提供更好的***通用性、安全性和可部署性，并且能够降低***的整体成本。在飞行器运输负载这一应用场景上，单飞行器的负载能力有限或过于昂贵，能量消耗速度快。而使用多飞行器协同搬运负载可以减少***的整体成本，增加***的运输能力和鲁棒性。代价是需要复杂的轨迹规划算法来生成各个飞行器的轨迹，要在保证***整体负载能力的前提下还能够达到各飞行器与负载避障的效果。

目前面向多飞行器编队飞行任务的轨迹规划方法一般有两种：一种是先生成一架飞行器的离线飞行轨迹，然后通过编队阵型仿射变换推导出其余飞行器的离线飞行轨迹，然后在线飞行过程中通过局部轨迹优化实时调整各飞行器的位置以达到避障的目的。例如，申请号为CN201910173841.5的中国专利提出了一种基于模型预测控制的多飞行器编队协同控制方法，他在首先根据多飞行器编队控制相关约束，初始化任务要求和相关控制参数，然后仅仅对领航飞行器进行初步航迹规划，之后就直接进入了在线的轨迹实施优化过程。这种方式生成的离线轨迹可能会使队形中的某些飞行器不满足避障约束，然而多飞行器绳索连接协同搬运负载的情况要考虑更多的约束并且约束的精度要求更高，此时将更多的轨迹优化压力放在了在线飞行过程中，对低性能的嵌入式平台要求更高，增加了这种协同运输任务的难度。另一种是先使用一种不考虑运动学的路径搜索方法生成一条飞行器的初始飞行轨迹，然后扩展出多条飞行器的路径，或者不指定初始轨迹，接下来再进行多飞行器的协同路径规划。例如申请号为CN201910395051.1的中国专利提出了一种多飞行器多蚁群协同搜索目标方法，他并没有生成一个初始的轨迹就直接使用蚁群算法进行多飞行器协同轨迹优化。这种方法使得非优化算法容易陷入局部极小值，并且在有狭窄障碍物时会降低优化成功率。

发明内容

要解决的技术问题

为了避免现有技术的不足之处，本发明提出一种针对多飞行器用柔性绳索连接负载并协同飞行下生成飞行器轨迹的方法。旨在实现在已知栅格地图的情况下，给出多飞行器协同运输负载的飞行轨迹。

技术方案

一种柔性绳系约束多机器人的轨迹生成方法，其特征在于步骤如下：

步骤1：生成整体队形初始路径

1.1)获取地图信息：通过计算机视觉获取实际环境的图像信息，然后生成一个ESDF全局栅格地图，所述的ESDF全局栅格地图可以提供每一个栅格到最近的障碍物的距离以及远离障碍物的梯度值；

1.2)定义RRT*路径搜索算法参数：将整个阵型视为一个以载荷位置为球心，以绳索长度为半径的球体；对于每一个飞行器，分别在竖直方向和水平方向上定义一个偏转角；每一个飞行器的位置都可以通过载荷位置以及两个偏转角求出；这种球体的优化参数只需要包括载荷的绝对位置和飞行器相对于载荷的角度值即可求解出所有飞行器的绝对位置；从而设定一个3+2n维的向量作为RRT*的随机扩展向量，3+2n维即载荷三维位置以及n个飞行器的两个偏转角；

1.3)生成初始飞行轨迹：设定好起点和终止点，进行路径搜索，得到一组离散的向量，作为初始轨迹；

步骤2：求解轨迹优化初始解参数

2.1)对相邻的两个向量间的每个维度分别进行插值，在每个维度上得到一组近似连续的值；

2.2)对每个维度的连续数据进行反解，求得对应的B样条曲线控制点参数作为优化的初始参数；

步骤3：优化出满足约束条件的飞行轨迹

3.1)选择所有的B样条曲线控制点作为优化变量，输入步骤2中得到的B样条曲线控制点参数作为初始解；

3.2)将飞行器的轨迹优化定义为一个非线性优化问题，优化问题代价包括轨迹平滑度代价、终点约束、动力学可行性约束、飞行器间防碰撞约束、飞行器阵型避障约束以及推力约束；

3.3)对步骤3.2)的优化问题进行非线性优化即可解出优化向量中每个维度的B样条曲线的最优控制点值，从而得到每个维度的多项式表达式，实时解出整个飞行器编队的飞行路径。

优选地：步骤1.2)中的n为2。

优选地：步骤3.2)中优化问题代价可表示为下式：

f_total＝λ₁f_s+λ₂f_i+λ₃f_c+λ₄f_e+(λ₅f_v+λ₆f_a+λ₇f_w+λ₈f_wa)+λ₉f_f

其中q为9维向量，a_max为最大加速度，v_max为最大速度，w_max为最大角速度，wa_max为最大角加速度，d_min为飞行器间最小距离，d_max为飞行器间最大距离，obs_min为避障允许最小距离，q_end为飞行终点q_end，d_near为从ESDF地图中可以获得每个点到距离最近的障碍物的距离，λ₁、λ₂、λ₃、λ₄、λ₅、λ₆、λ₇、λ₈、λ₉为各个代价的权值。

有益效果

本发明提出的一种柔性绳系约束多机器人的轨迹生成方法，具有以下益处：

1、使用了两阶段轨迹规划算法，首先用RRT*寻找到一个初始轨迹解，然后在使用非线性优化对初始轨迹解进行优化，这样可以增快优化的速度并且更容易找到全局最优解而不是陷入局部最优解；

2、将整个飞行器协同搬运***看做一个球体，以载荷位置为圆心，以绳长为半径。将绳长约束转化为位置解算参数，简化了优化所需的参数数量和优化复杂性；

3、使用ESDF地图代替普通的栅格地图，容易获取到每个飞行器到最近障碍物的距离，并且可以获得该点处远离障碍物的梯度值，利于避障代价的计算。

4、考虑到了每个飞行器的推力约束，使得优化的轨迹尽可能的减少整个***的能量消耗。

5、通过调节每一次的优化的距离以及优化的迭代次数，使得该优化算法既可以用于离线的全局规划，也可以用于在线的局部规划。

附图说明

附图仅用于示出具体实施例的目的，而并不认为是对本发明的限制，在整个附图中，相同的参考符号表示相同的部件。

图1多飞行器协同搬运负载***整体图；

图2***整体算法流程图；

图3仿真结果图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图和实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅用以解释本发明，并不用于限定本发明。此外，下面描述的本发明各个实施方式中所涉及到的技术特征只要彼此之间未构成冲突就可以相互组合。

本发明所采用的技术方案包括以下步骤：

1)生成整体队形初始路径；

2)求解轨迹优化初始解参数；

3)优化出满足约束条件的飞行轨迹；

所述的步骤1)生成整体队形初始路径包含以下子步骤：

1.1)获取地图信息：通过计算机视觉获取实际环境的图像信息，然后生成一个ESDF全局栅格地图。ESDF地图可以提供每一个栅格到最近的障碍物的距离以及远离障碍物的梯度值；

1.2)定义RRT*路径搜索算法参数：将整个阵型视为一个以载荷位置为球心，以绳索长度为半径的球体。对于每一个飞行器，分别在竖直方向和水平方向上定义一个偏转角。每一个飞行器的位置都可以通过载荷位置以及两个偏转角求出。这种球体的优化参数只需要包括载荷的绝对位置和飞行器相对于载荷的角度值即可求解出所有飞行器的绝对位置。从而设定一个3+2n维的向量(载荷三维位置以及n个飞行器的两个偏转角)作为RRT*的随机扩展向量；

1.3)生成初始飞行轨迹：设定好起点和终止点，进行路径搜索，得到一组离散的向量，作为初始轨迹。

所述的步骤2)求解轨迹优化初始解参数包含以下子步骤：

2.1)步骤1)中得到的向量每一个维度都是解耦的，对相邻的两个向量间的每个维度分别进行插值，在每个维度上得到一组近似连续的值；

2.2)对每个维度的连续数据进行反解，求得对应的B样条曲线控制点参数作为优化的初始参数。

所述的步骤3)优化出满足约束条件的轨迹包含以下子步骤

3.1)选择所有的B样条曲线控制点作为优化变量，输入步骤2)中得到的B样条曲线控制点参数作为初始解；

3.2)将飞行器的轨迹优化定义为一个非线性优化问题，优化问题代价包括轨迹平滑度代价、终点约束、动力学可行性约束、飞行器间防碰撞约束、飞行器阵型避障约束以及推力约束；

3.3)通过步骤3.1)、3.2)、3.3)将整个路径规划问题转换为一个非线性优化问题，并确定了优化的一组良好的初始解、优化问题以及优化约束，对该优化问题进行非线性优化即可解出优化向量中每个维度的B样条曲线的最优控制点值，从而得到每个维度的多项式表达式，实时解出整个飞行器编队的飞行路径。

上述步骤具体如下：

1)生成载荷的飞行轨迹包含以下子步骤：

1.1)获取地图信息：通过计算机视觉获取实际环境的图像信息，然后生成一个ESDF全局栅格地图。ESDF地图可以提供每一个栅格到最近的障碍物的距离以及远离障碍物的梯度值；

1.2)定义RRT*路径搜索算法参数：如图1所示，整个飞行器协同搬运阵型被视作一个球体。定义负载的三维坐标为[x y z]，每个无人机的两个偏转角分别为

在本发明中以三架无人机为例，则整个阵型的状态可以用一个9维向量

表示。设绳子长度为r，对于任意一个飞行器，其三维坐标可以由公式(1)解出，

1.3)生成初始飞行轨迹：将这个9维向量作为RRT*轨迹搜索算法的输入向量，每一个维上每次随机生成一个偏移值，然后以负载的位置作为扩展方向，每一次扩展后判断阵型是否碰撞到障碍物，直到负载的位置到达终点，最终得到m个离散的向量q(m)，表示了阵型飞行的路径以及每个时刻的阵型状态；

2)求解轨迹优化初始解参数包含以下子步骤：

2.1)在步骤1)中得到的向量每一个维度都是解耦的，对相邻的两个向量间的每个维度分别进行插值，在每个维度上得到一组近似连续的值；

2.2)对每个维度的连续数据进行拟合成多项式，然后对多项式进行反解，求得对应的B样条曲线控制点，将这些控制点作为优化的初始参数；

3)优化出满足约束条件的轨迹包含以下子步骤：

3.1)选择所有的B样条曲线控制点作为优化变量，输入步骤2)中得到的B样条曲线控制点参数作为初始解，定义优化变量为i个9维向量q(i)最大加速度为a_max，最大速度为v_max，最大角速度为w_max，最大角加速度为wa_max，飞行器间最小距离为d_min，飞行器间最大距离为d_max，避障允许最小距离obs_min，飞行终点q_end，从ESDF地图中可以获得每个点到距离最近的障碍物的距离为d_near，由飞行器的当前阵型排布可以反解出每个飞行器给绳子的拉力F；

3.2)定义轨迹优化问题包括轨迹平滑度代价、动力学可行性代价、飞机间防碰撞代价、飞机阵型避障代价、飞行终点到达度代价以及推力代价，如公式(2)所示

f_total＝λ₁f_s+λ₂f_i+λ₃f_c+λ₄f_e+(λ₅f_v+λ₆f_a+λ₇f_w+λ₈f_wa)+λ₉f_f。 (2)

λ₁、λ₂、λ₃、λ₄、λ₅、λ₆、λ₇、λ₈、λ₉为各个代价的权值；

其中，轨迹平滑度代价：由于无人机***的特殊性，其多项式轨迹的三阶导数代表轨迹的无人机欧拉角的角速度，与无人机的轨迹平滑度相关，故依据B样条曲线的导数特性得到平滑度代价函数如下：

终点到达度代价：

动力学可行性代价：公式(5)、(6)中的q仅计算前三维，对应的载荷三维位置，求解载荷的速度和加速度代价

公式(7)、(8)中的q计算后六维，对应的每个无人机相对于载荷的角度偏移量，求解无人机相对运动的角速度和角加速度代价

飞行器间防碰撞代价：

飞行器阵型避障代价：由公式(1)可以得出每一架飞行器的位置以及载荷的位置，在每一架飞行器到载荷的绳上取8个点作为碰撞检测点，代价函数如下：

以及推力代价：

3.3)通过步骤3.1)、3.2)将整个路径规划问题转换为一个非线性优化问题，并确定了优化的一组良好的初始解、优化问题代价函数，对该优化问题进行非线性优化即可解出优化出每个维度的B样条曲线的最优控制点值，从而得到每个维度的多项式表达式，实时解出整个飞行器编队的飞行路径。

定义平滑度权重为10，终点到达度权重为0.01，动力学可行性权重为0.001，飞机间防碰撞权重为1，飞行器阵型避障权重为1，推力权重为0.0001。此时在CPU为i5-8250u的笔记本上运行规划算法，各个约束的cost值如下f smoothness:0.12772

f distance:0.000663906

f feasibility:9.63905e-05

f thrust:0

f obs:2.95077e-06

f_combine:0.128925

代码运算时间为0.8765s，规划出的路径如图3所示，轨迹是平滑的并且避开了所有的障碍物，也没有相互碰撞，满足了所有的约束条件，其中4条蓝色(折线)的轨迹分别指无人机和载荷在前端使用RRT*算法寻找出的良好初始轨迹，4条红色(平滑曲线)的轨迹分别代表3架无人机和载荷的最终轨迹。

以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明公开的技术范围内，可轻易想到各种等效的修改或替换，这些修改或替换都应涵盖在本发明的保护范围之内。

Claims (2)

1.一种柔性绳系约束多机器人的轨迹生成方法，其特征在于步骤如下：

步骤1：生成整体队形初始路径

1.1)获取地图信息：通过计算机视觉获取实际环境的图像信息，然后生成一个ESDF全局栅格地图，所述的ESDF全局栅格地图可以提供每一个栅格到最近的障碍物的距离以及远离障碍物的梯度值；

1.2)定义RRT*路径搜索算法参数：将整个阵型视为一个以载荷位置为球心，以绳索长度为半径的球体；对于每一个飞行器，分别在竖直方向和水平方向上定义一个偏转角；每一个飞行器的位置都可以通过载荷位置以及两个偏转角求出；这种球体的优化参数只需要包括载荷的绝对位置和飞行器相对于载荷的角度值即可求解出所有飞行器的绝对位置；从而设定一个3+2n维的向量作为RRT*的随机扩展向量，3+2n维即载荷三维位置以及n个飞行器的两个偏转角；

1.3)生成初始飞行轨迹：设定好起点和终止点，进行路径搜索，得到一组离散的向量，作为初始轨迹；

步骤2：求解轨迹优化初始解参数

2.1)对相邻的两个向量间的每个维度分别进行插值，在每个维度上得到一组近似连续的值；

2.2)对每个维度的连续数据进行反解，求得对应的B样条曲线控制点参数作为优化的初始参数；

步骤3：优化出满足约束条件的飞行轨迹

3.1)选择所有的B样条曲线控制点作为优化变量，输入步骤2中得到的B样条曲线控制点参数作为初始解；

3.2)将飞行器的轨迹优化定义为一个非线性优化问题，优化问题代价包括轨迹平滑度代价、终点约束、动力学可行性约束、飞行器间防碰撞约束、飞行器阵型避障约束以及推力约束；

3.3)对步骤3.2)的优化问题进行非线性优化即可解出优化向量中每个维度的B样条曲线的最优控制点值，从而得到每个维度的多项式表达式，实时解出整个飞行器编队的飞行路径；

优化问题代价可表示为下式：

f_total＝λ₁f_s+λ₂f_i+λ₃f_c+λ₄f_e+(λ₅f_v+λ₆f_a+λ₇f_w+λ₈f_wa)+λ₉f_f

其中q为9维向量，a_max为最大加速度，v_max为最大速度，w_max为最大角速度，wa_max为最大角加速度，d_min为飞行器间最小距离，d_max为飞行器间最大距离，obs_min为避障允许最小距离，q_end为飞行终点q_end，d_near为从ESDF地图中可以获得每个点到距离最近的障碍物的距离，λ₁、λ₂、λ₃、λ₄、λ₅、λ₆、λ₇、λ₈、λ₉为各个代价的权值。

2.根据权利要求1所述的一种柔性绳系约束多机器人的轨迹生成方法，其特征在于步骤1.2中的n为2。

Images