一种多机吊载系统的高速避障轨迹实时规划方法及装置

本发明属于无人机的物流运输领域，尤其涉及一种多机吊载系统的高速避障轨迹实时规划方法及装置。

背景技术：

1、近年来，随着物流运输需求的不断增长，固定翼无人飞行器由于其过高的飞行速度、较大的转弯半径的限制导致无法进一步拓展应用于丛林、城市空间、建筑物内部等场景下的货物运输任务。多旋翼无人机具备体积小、低成本、可垂直起降、机动性强等优势，是近些年军事、商业应用最为广泛的一类飞行器，可以执行多样的货物运输任务。

2、具备高速机动飞行能力的多旋翼无人机可以显著提升任务的效率，但现有商业的采用单架多旋翼无人机的货物运输方案（简称单机吊载系统），仍依赖于人工操作和外部定位，吊载飞行速度往往很慢，而即使是熟练的操作手也难以在复杂环境下高效率地执行高速机动飞行任务，且末端投送能力差（需要有人员接应），仅适用于环境空旷且任务简单的慢速货物运输场景。

3、单机吊载系统存在固有缺陷：1）载重能力有限，单个无人机能适应的载重区间较小，无人机无法搬运超过载重能力上限的重物；2）重物的姿态可控性差，仅通过一条绳子无法提供力矩，因此无法控制重物的姿态；3）系统紧急避障能力弱，无人机紧急制动后，惯性的存在使得重物依然会沿运动方向继续摆动一定距离，并带动无人机一起向前摆动，存在安全隐患。

4、与单机吊载系统相比，利用多架多旋翼无人机组成的多机吊载系统方案具有以下优势：1）负载能力强，多机吊载系统可以利用无人机数量多的优势提高载重能力，提升系统对重物尺寸、重量的适应性，无需因为重物尺寸、重量的不同而设计具备不同负载能力的平台；2）重物姿态可控性强，可用于对重物姿态有特殊要求的场景，比如利用多个无人机吊载扫雷设备，通过调整无人机的位置，使得扫雷设备与非平整的地形始终平行；3）系统紧急避障能力强，多机吊载系统可以通过多条不同方向的绳子为重物提供必要的瞬时制动控制力，避免绳子出现较大的摆动，对于急停、紧急避障等场景的应对能力更强。因此，多机吊载系统方案引起了国内外的广泛关注。

5、目前，现有的多机吊载系统运动规划方案尚且存在以下问题：

6、1、现有的多机吊载系统相关研究一般是从系统整体角度选择表征系统状态的平坦输出变量，但该平坦输出选择方法，无法给系统内所有吊载无人机提供独立的平坦输出变量以直接表征其轨迹。系统内总是存在少数吊载无人机的位置依赖于其它所有吊载无人机的平坦输出变量，因此这些吊载无人机轨迹的能量无法被考虑进优化目标函数中，可能导致这些吊载无人机轨迹不光滑且状态存在高频大幅的振荡，制约系统整体的高速机动飞行能力。

7、2、现有的方案往往选择绳子力矢量的三个分量作为平坦输出变量。这类方案中，吊载无人机的位置往往由绳子力矢量的单位方向矢量来表征。但是，对于该种表征方式，当力的大小逼近于零时，会导致吊载无人机位置存在奇异点。

8、3、现有研究中，有关质点模型的快速安全轨迹搜索方案较多，缺少面向多机吊载系统复杂构型的快速安全无碰撞路径规划方案。

9、4、绝大部分的多机吊载系统运动规划仅考虑了重物的轨迹规划，通过对重物闭环控制得到重物所需要的控制力，将控制力按照力最小范数原则分配给每个无人机。对于该方法，吊载无人机的运动由重物的运动及力分配过程唯一确定，在重物运动规划及后续控制过程中，无法考虑吊载无人机的避障需求，因此无法保证吊载无人机的安全。

10、5、现有轨迹优化方案无法保证高速机动轨迹动力学可执行。无人机受到系统延时、响应时间、物理上界以及自身动力学等因素的限制，无人机的部分状态（最大推力等）无法超过其上界，且无人机无法适应部分系统状态（加速度、角速率等）的过快变化。无人机能力的限制导致系统跟不上规划的轨迹，造成系统坠毁等安全隐患。为了保证轨迹的动力学可执行，现有方案往往仅考虑最大速度、最大加速度等粗糙保守的动力学约束。但是，粗糙的动力学约束往往使得轨迹过于保守，导致优化的轨迹速度慢、机动小。因此，无法适用于多机吊载系统的高速机动飞行。

11、6、对于刚体模型的多机吊载系统，现有的研究没有考虑优化过程中绳子之间可能出现的交叉缠绕情况。

12、7、少数基于非线性规划的多机吊载系统全状态运动规划方案，由于通用求解器求解速度的限制，难以满足规划实时性的要求。

技术实现思路

1、针对现有技术存在的问题，本技术实施例的目的是提供一种多机吊载系统的高速避障轨迹实时规划方法及装置。

2、根据本技术实施例的第一方面，提供一种多机吊载系统的高速避障轨迹实时规划方法，包括：

3、对多机吊载系统进行动力学建模，其中所述多机吊载系统包括若干吊载无人机和通过绳子与各吊载无人机连接的重物；

4、将所述多机吊载系统的构型构建为可伸缩变化的正棱锥，基于该构型进行无碰撞路径的快速规划；

5、通过重物的位置及绳子的矢量方向，对吊载无人机位置进行间接表征；

6、将带有约束的表征绳子方向角度优化变量等价转换为无约束的虚拟变量，从而实现绳子方向角度约束的消除；

7、构建多机吊载系统高速避障轨迹优化的多目标优化问题，其中所述多目标优化问题包括表征轨迹光滑程度和所需能量的能量函数、表征轨迹执行时间的总时间函数、精细动力学约束、安全约束和耦合动力学约束；

8、基于所述多目标优化问题，以所述无碰撞路径作为初始轨迹，进行轨迹优化，其中，所述表征轨迹光滑程度和所需能量的能量函数、精细动力学约束、安全约束和耦合动力学约束采用并行方式求解。

9、进一步地，所述多机吊载系统包括n架多旋翼无人机，作为吊载无人机，系统中重物的动力学模型如下：

10、

11、其中分别为重物的质量和其绕质心的转动惯量，分别为重物在世界坐标系下的位置和速度，为重物坐标系与世界坐标系的旋转矩阵，为重物的角速率，为所对应的反对称矩阵，是第i条绳子施加在重物上的拉力大小，为绳子方向的单位矢量，由重物指向无人机，为重物质心与绳子连接点之间的矢量，由质心指向连接点，为矢量对应的反对称矩阵，；

12、单架吊载无人机的动力学模型为：

13、

14、其中下标代表其为第架吊载无人机。分别为吊载无人机的质量和其绕质心的转动惯量，分别为吊载无人机在世界坐标系下的位置和速度，为吊载无人机的机体轴系与世界坐标系的旋转矩阵，为对应的四元数，为吊载无人机的角速率，为角速率所对应的反对称矩阵，为吊载无人机的角加速度。分别为吊载无人机的总推力与姿态控制力矩。是绳子施加在机体上的拉力大小，为绳子方向的单位矢量，由重物指向无人机，由绳子的俯仰角和方位角唯一确定，

15、

16、其中，为绳子对吊载无人机的拉力。

17、进一步地，以重物的位置、描述正棱锥空间姿态的仰角、方位角以及棱锥底面正三角形外接圆的半径作为所述多机吊载系统的构型参数。

18、进一步地，所述避障轨迹的规划基于a*算法进行，其中在进行轨迹搜索时，会对正棱锥的构型参数进行离散化，将离散化的构型参数作为算法中的搜索变量，并通过重物的位置以及正棱锥的形姿参数来计算系统整体在空间中占据的位置，从而判断其是否与地图中的障碍物发生碰撞。

19、进一步地，通过重物的位置及绳子的矢量方向，对吊载无人机位置进行间接表征，具体为：

20、

21、其中为第i个吊载无人机的位置，为重物的位置，为表示第i条绳子方向的单位矢量，由重物指向吊载无人机，为重物质心与绳子连接点之间的矢量，由质心指向连接点，为绳子的长度。

22、进一步地，将带有约束的表征绳子方向角度优化变量等价转换为无约束的虚拟变量，其中，对俯仰角的等价转换过程为：

23、引入虚拟优化变量；

24、建立虚拟优化变量与绳子实际方向角度变量之间的微分同胚映射：

25、

26、其中为俯仰角 的上界和下界；

27、从而将带有约束的俯仰角等价转换为无约束的虚拟变量。

28、进一步地，所述多目标优化问题中，

29、所述表征轨迹光滑程度和所需能量的能量函数，其中，为重物位置，令， 则为与第i个吊载无人机相连绳子的力矢优化变量的轨迹，为调整两个能量函数比重的系数，t为轨迹的总时长，为吊载无人机数量；

30、所述表征轨迹执行时间的总时间函数，其中，为多段多项式轨迹中一段轨迹的时间；

31、所述精细动力学约束包括吊载无人机的最大速度约束违背函数、最大/最小推力约束违背函数、最大轴角约束违背函数、角速率约束违背函数、角加速度约束违背函数，其中i代表第i个吊载无人机，j代表轨迹的第j段，k代表第j段轨迹上的第k个采样点，为吊载无人机在世界坐标系下的速度，为容许的吊载无人机最大速度，为吊载无人机的总推力，分别为容许的最大推力、最小推力，为吊载无人机的机体轴系的z轴与重力加速度方向的夹角，为容许的最大轴角，为吊载无人机的角速率，为容许的最大角速率，为吊载无人机的角加速度，为容许的最大角加速度；

32、所述安全约束包括系统避障安全约束违背函数和多机吊载系统内部相互避碰约束违背函数，其中为绳子上采样点的数量，为与障碍物之间最大的容许安全距离，为重物第j段轨迹上的第k个采样点；

33、所述耦合动力学约束包括重物平移动力学约束违背函数和重物转动动力学约束违背函数，其中分别为重物的质量和其绕质心的转动惯量，为重物在世界坐标系下的速度，是第i条绳子施加在重物上的拉力大小，为绳子方向的单位矢量，，为重物的角速率，为重物的角加速度，为重物质心与绳子连接点之间的矢量对应的反对称矩阵，为重物坐标系与世界坐标系的旋转矩阵，为吊载无人机的数量。

34、根据本技术实施例的第二方面，提供一种多机吊载系统的高速避障轨迹实时规划装置，包括：

35、动力学建模模块，用于对多机吊载系统进行动力学建模，其中所述多机吊载系统包括若干吊载无人机和通过绳子与各吊载无人机连接的重物；

36、轨迹规划模块，用于将所述多机吊载系统的构型构建为可伸缩变化的正棱锥，基于该构型进行无碰撞路径的快速规划；

37、位置表征模块，用于通过重物的位置及绳子的矢量方向，对吊载无人机位置进行间接表征；

38、约束消除模块，用于将带有约束的表征绳子方向角度优化变量等价转换为无约束的虚拟变量，从而实现绳子方向角度约束的消除；

39、优化问题构建模块，用于构建多机吊载系统高速避障轨迹优化的多目标优化问题，其中所述多目标优化问题包括表征轨迹光滑程度和所需能量的能量函数、表征轨迹执行时间的总时间函数、精细动力学约束、安全约束和耦合动力学约束；

40、轨迹优化模块，用于基于所述多目标优化问题，以所述无碰撞路径作为初始轨迹，进行轨迹优化，其中，所述表征轨迹光滑程度和所需能量的能量函数、精细动力学约束、安全约束和耦合动力学约束采用并行方式求解。

41、根据本技术实施例的第三方面，提供一种电子设备，包括：

42、一个或多个处理器；

43、存储器，用于存储一个或多个程序；

44、当所述一个或多个程序被所述一个或多个处理器执行，使得所述一个或多个处理器实现如第一方面所述的方法。

45、根据本技术实施例的第四方面，提供一种计算机可读存储介质，其上存储有计算机指令，该指令被处理器执行时实现如第一方面所述方法的步骤。

46、本技术的实施例提供的技术方案可以包括以下有益效果：

47、1、本发明从系统内单个无人机视角选择表征每个吊载无人机状态的平坦输出变量，第个无人机的平坦输出变量为重物的位置、姿态四元数、航向角、以及表征绳子力矢的俯仰角、方位角、力的大小以及无人机的航向角，在此基础上选择系统整体的拓展平坦输出空间为，拓展平坦输出空间可以保证系统内每个吊载无人机均有独立的平坦输出变量直接表征其位置，可以保证每架吊载无人机轨迹的光滑性，提高多机吊载系统的高速避障飞行能力，解决现有从系统整体角度出发的平坦输出选择方式无法保证部分无人机轨迹的光滑性而限制系统高速避障飞行能力的问题；

48、2、本发明提出了一种新型的间接表征多机吊载系统无人机位置的方法，利用无人机的位置以及绳子的俯仰角和方位角的唯一地表征无人机的轨迹，从而消除无人机与重物之间的运动学约束，且避免其它表征方法可能导致的无人机位置奇异点问题。

49、3、本发明提出一种面向多机吊载系统安全无碰撞路径规划方法。将多机吊载系统的复杂构型空间简化为仅6个自由度的构型空间，实现复杂障碍环境下的安全无碰撞路径快速规划。

50、4、本发明提出一种适用于多机吊载系统高速机动轨迹优化的轻量级多目标优化框架。考虑吊载无人机的精细动力学约束，保证优化的高速机动轨迹动力学可执行，进一步考虑系统避障/避碰约束，保证包括重物以及无人机在内的整个多机吊载系统的安全。

51、5、本发明提出一种消除绳子优化变量拓扑约束的方法。将表征绳子优化变量的进行微分同胚映射，消除绳子方向角度的拓扑约束，同时避免绳子可能出现的交叉缠绕情况。

52、6、并行式地评估各个无人机的约束违背以及求解多项式轨迹参数梯度，使得轨迹优化时间不随系统内无人机数量的增加而线性地增长，保证了轨迹优化的实时性，同时提高无人机数量的拓展性。

53、应当理解的是，以上的一般描述和后文的细节描述仅是示例性和解释性的，并不能限制本技术。

技术特征：

1.一种多机吊载系统的高速避障轨迹实时规划方法，其特征在于，包括：

2.根据权利要求1所述的方法，其特征在于，所述多机吊载系统包括n架多旋翼无人机，作为吊载无人机，系统中重物的动力学模型如下：

3.根据权利要求1所述的方法，其特征在于，以重物的位置、描述正棱锥空间姿态的仰角、方位角以及棱锥底面正三角形外接圆的半径作为所述多机吊载系统的构型参数。

4.根据权利要求3所述的方法，其特征在于，所述无碰撞路径的规划基于a*算法进行，其中在进行路径搜索时，会对正棱锥的构型参数进行离散化，将离散化的构型参数作为算法中的一个搜索变量，并通过重物的位置以及正棱锥的形姿参数来计算系统整体在空间中占据的位置，从而判断其是否与地图中的障碍物发生碰撞。

5.根据权利要求1所述的方法，其特征在于，通过重物的位置及绳子的矢量方向，对吊载无人机位置进行间接表征，具体为：

6.根据权利要求1所述的方法，其特征在于，将带有约束的表征绳子方向角度优化变量等价转换为无约束的虚拟变量，其中，对俯仰角的等价转换过程为：

7.根据权利要求1所述的方法，其特征在于，所述多目标优化问题中，

8.一种多机吊载系统的高速避障轨迹实时规划装置，其特征在于，包括：

9.一种电子设备，其特征在于，包括：

10.一种计算机可读存储介质，其上存储有计算机指令，其特征在于，该指令被处理器执行时实现如权利要求1-7中任一项所述方法的步骤。

技术总结
本发明公开了一种多机吊载系统的高速避障轨迹实时规划方法及装置，该方法包括：对多机吊载系统进行动力学建模；将多机吊载系统构建为可伸缩变化的正棱锥，基于该构型进行初始无碰撞路径的规划；通过重物的位置及绳子的矢量方向，对吊载无人机位置进行间接表征；将带有约束的表征绳子方向角度优化变量等价转换为无约束的虚拟变量，从而实现约束消除；构建多机吊载系统高速避障轨迹优化的多目标优化问题，包括表征轨迹光滑程度和所需能量的能量函数、表征轨迹执行时间的总时间函数、精细动力学约束、安全约束和耦合动力学约束；基于多目标优化问题，采用并行式框架进行轨迹优化，使得轨迹优化时间不随系统内无人机数量的增加而线性地增长。

技术研发人员：王永超,高飞
受保护的技术使用者：浙江大学
技术研发日：
技术公布日：2024/9/23