1、SHIP ENGINEERING 船 舶 工 程 Vol.45 No.2 2023 总第 45 卷，2023 年第 2 期 140 多旋翼无人机航迹规划算法多旋翼无人机航迹规划算法 荆学东，杜黎童，王旭飞（上海应用技术大学 机械工程学院，上海 201400）摘 要：摘 要：无人机（UAV）能实现航迹跟踪、定位、遥控、航拍、数据传输等功能，其中航迹规划是实现所有功能的前提条件。结合多旋翼无人机在恒速飞行条件下的最大转弯半径，设计一种在一定曲率约束范围内的航迹规划算法，采用 Dubins 曲线和 B 样条曲线，分别对局部障碍物和区域障碍物进行分类避障规划，实现障碍物下不规则作业区域的航迹规划，并通
2、过仿真试验验证了算法的可行性。关键词：关键词：无人机；多旋翼；航迹规划；Dubins 曲线；B 样条曲线 中图分类号：中图分类号：U671.99 文献标志码：文献标志码：A 【DOI】10.13788/ki.cbgc.2023.02.19 Planning Algorithm for Multi-Rotor UAV Track JING Xuedong,DU Litong,WANG Xufei(Mechanical Engineering College,Shanghai Institute of Technology University,Shanghai 201400,China)Abst
3、ract:Unmanned aerial vehicle(UAV)can realize the functions of track tracking,positioning,remote control,aerial photography,data transmission,etc.,among which the UAV track planning is the prerequisite for all functions.Combined with the maximum turning radius of plant protection multi-rotor UAV in c
4、onstant speed flight,a flight path planning algorithm with a certain curvature constraint is designed.The classification and obstacle avoidance planning are made for local and regional obstacles based on Dubins curve and B-spline curve.The track planning in any irregular area with obstacles is reali
5、zed,and the feasibility of the algorithm is verified by the simulation method.Key words:unmanned aerial vehicle(UAV);multi-rotor;track planning;bubins curve;B-spline curve 0 引言引言 无人机（Unmanned Aerial Vehicle,UAV）是利用无线电遥控设备和自备的程序控制装置操纵的不载人飞机，分为固定翼无人机、无人直升机和多旋翼无人机三大类。其中多旋翼无人机具有成本低、灵活性好和效率高等特点，被广泛用于海洋船舶
6、中，如海事巡航监管、船舶引航、搜救救援、船舶油污泄漏监测和检查等。无人机在海事监管中的应用成果较多，如固定翼无人机利用无人机遥感技术来实现海域与海岛、石油、海洋环境等的监视监测1。ROYEST 等2使用TSP（Traveling Salesman Problem，也就是旅行商问题，是最基本的路线问题，该问题是在寻求单一旅行者由起点出发，通过所有给定的需求点之后，最后再回到原点的最小路径成本）模型来规划无人机任务，目的是找到爆炸装置和放置简易爆炸装置的恐怖分子。国内由于需求的增加和政府的支持，涌现了一批无人机研发生产企业，如中国航天科工集团、青岛天骄无人机遥感技术有限公司和桂林鑫鹰电子科技有限公
7、司等，已经生产出了多个无人机机型3。无人机的航迹规划是实现多旋翼无人机在海洋船舶应用的关键技术之一。多旋翼无人机操作灵活，可实现悬停、俯仰、偏航、横滚等动作，无人机在转弯处或者曲线型航线处进行航迹规划时，传统航迹规划方法会降低速度，依靠多旋翼无人机多自由度的特点来绕过障碍物，但不断改变无人机的飞行速度不仅会影响工作效率，而且也会使无人机的续航能力降低。因此多旋翼无人机的航迹规划不仅需要满足避障要求，也应尽量满足无人机飞行速度稳定的条件4。因此，本文结合多旋翼无人机飞行速度，在指定飞行速度条件下计算其转弯半径，并使用曲率约束的方法规划多旋翼无人机在海洋航行中的曲线航迹。收稿日期：2022-04-
8、08；修回日期：2022-06-07 基金项目：上海市科学技术委员会研究计划项目（16090503700）作者简介：荆学东（1968），男，教授、硕士生导师。研究方向：机器人技术、智能检测技术。荆学东等，多旋翼无人机航迹规划算法 141 1 多旋翼无人机航迹规划多旋翼无人机航迹规划 1.1 在无障碍任意不规则区域的航迹规划在无障碍任意不规则区域的航迹规划 当多旋翼无人机在无障碍任意不规则海洋区域内进行航迹规划时，做出如下模型假设。假设1：不规则区域可视为多边形区域。假设2：无人机的航线都是平行的。假设3：航线间距均为无人机作业宽度 W。在一个不规则多边形基础上进行航迹规划，需要确定多边形各点之
9、间的位置坐标，在无人机作业前需要使用定位装置对作业区域进行测绘，标记出多边形的各个顶点位置，以及无人机的起飞位置和姿态。经过坐标转换即可使多旋翼无人机坐标与地理坐标重合，以此来实现多旋翼无人机的航迹跟踪5，如图 1所示，A1、A2、A3、An为使用定位仪器的多边形区域顶点坐标，在航迹规划时以 A1为起始点，并建立相应的坐标原点和坐标轴，按照规定顺序记录每个顶点的坐标。图1 中：A1An为多边形顶点；a1an为航线与多边形边线的交点；W 为多旋翼无人机的作业幅度。多旋翼无人机航迹的规划首先需要确定起始边，然后确定到起始边距离最远的顶点，并求解出其相应的距离，最后求解出航线与多边形的交点即多旋翼无
10、人机需要掉头的航点6。图 1 无人机航迹规划简图 以 A1A2边为起始边，设坐标为(x1,y1)和(x2,y2)，则起始边表达式为 212122()()()()yyxxyyxx-=-（1）由式（1）可求出该直线与y 轴的交点 C0。假设剩余(n2)个点到起始边的距离分别为d1、d2、dn，则距离起始边最大值为dmax=d1,d2,dn，设到起始边距离最远的点的坐标为(,)A x y，则经过该点并平行于起始边的直线方程为 12122()()()()yyxxyyxx-=-（2）由式（2）可求出该直线与y轴的交点C。多旋翼无人机的工作宽度为W，在多边形工作区域内分割等宽度平行的航线，设第i条航线与y
11、轴的交点为C，则：)_max002dCCwCCi-=+-|（3）因为每条航线都与起始边平行，故第i条航线的斜率为 1212()()yykxx-=-（4）通过上述解析可得到第i条航线的方程为 iykxC=+（5）经过上述方法可求解出多旋翼无人机需要转弯的航点，这些航点按照从左到右、从下到上呈“S”型的方式排序，即可得到多旋翼无人机在无障碍多边形区域的航点和航迹7。航迹规划过程中必然伴随着避障功能，在进行有障碍的不规则区域内进行航迹规划时还需对上述航迹做进一步的“约束”。1.2 在有障碍的不规则区域航迹规划在有障碍的不规则区域航迹规划 1.2.1 局部障碍物避障规划 多旋翼无人机的海洋作业区域一般
12、会存在不确定的障碍物，本文把障碍物分为局部障碍物和区域障碍物2种类型，并根据2种障碍物的特点分类进行障碍规划。局部障碍物的特点是影响作业范围较小，一般只影响一条航线，根据此特点在确定局部障碍物位置坐标的条件下，可自动生成障碍物影响的区域范围。以圆形区域代表局部障碍物所产生的影响范围，且此区域直径d小于多旋翼无人机航迹规划的间距宽度W（即dW），局部障碍物航迹简单规划见图2。图2 局部障碍物航迹规划简图 图2中：Q为局部障碍物的圆形区域；C0为局部障碍物所影响的圆形区域的圆心坐标；直线MN为受到影响的一条航线；点A、点D为航线与局部障碍物区域相交的点。以点A、点D为切点，绘制圆心为C1和C3、直
13、径相等的2个圆，且满足多旋翼无人机作业时在不改变航速情况下大于最小转弯半径。C2为与C1、C3外切的圆，其半径不小于多旋翼正常工船舶电气、探通导设备及自动控制 142 作速度下的最小转弯半径，点B和点C是圆心C2的圆与圆心C1圆和圆心C3圆的切点，则曲线A-B-C-D为所规划的航线，其航线可根据Dubins曲线来确定。Dubins曲线是在满足曲率约束和规定的始端和末端切线条件下，连接2个二维平面的最短路径，但限制于只能方向向前8。由此可见，Dubins曲线很符合本文设计的多旋翼无人机航迹规划方式。Dubins曲线是在无障碍情况下，提供一条最优的前向路径，与A*、RRT等搜索算法相比，其最大的优
14、势是所规划的路径满足物体运动学要求9。Dubins曲线通常用于机器人和控制领域，常用于轮式机器人、飞行器和水下机器人等的路径规划，Dubins曲线规划的最佳路径方式用右转（R）、左转（L）、直线（S）来描述机器人的运动，可简单概括为6种类型，分别为LSL、RSR、LSR、RSL、RLR和LRL10，见图3，图中：pi为起点；pf为终点。图 3 Dubins 曲线路径规划 本文所使用的Dubins曲线为RLR或LRL这2种类型。在保证多旋翼无人机作业速度不变的情况下（俯仰角度不变），可计算出多旋翼无人机的最小转弯半径，设多旋翼无人机作业速度为V、偏航最大角速度为max、多旋翼无人机的整体质量为m
15、、转弯半径为r，则由向心力公式可得到：22maxvmrmr=（6）化简式（6）可得到：maxvr=（7）所以多旋翼无人机的最小转弯半径为rmin=v/max，最大曲率K=max/v。多旋翼无人机在绕局部障碍物避障作业时，如图4所示，P1、P2、P3为已知半径的3个相切圆，其圆心C1、C2、C3构成的向量分别为V12、V13、V32，向量V12、V13的夹角为，V12的方向角为，在航迹规划时C1(x1,y1)、C2(x2,y2)的坐标已知，求解C3(x3,y3)的坐标即可得到多旋翼无人机绕过局部障碍物的航线。图4 Dubins 曲线RLR（LRL）简图 用d12、d13、d23代表3个圆心间的距
16、离，则：221212122213131322232323()()()()()()dxxyydxxyydxxyy=-+-=-+-=-+-（8）根据余弦定理可得到：222121323121321213131arccos2arctanarctandddd dyyxxyyxx+-=|-=|-=|-（9）联立式（8）和式（9）可得到C3坐标(x3,y3)。求得C3坐标后，可从起点pi沿V13经过P1圆弧后得到第1个切点A1，再沿V32方向经过P3圆弧得到第2个切点A2，以上是按照RLR曲线解算的；同理可得LRL曲线，进而得到局部障碍物所影响区域的2种航线避障策略。1.2.2 区域障碍物避障规划 区域障碍物的特点是影响范围广，一般会影响2条以上的航线，见图5。图5中：A1A2A3A4为多旋翼无人机海洋作业区域局部图；H1I1G1K1L1为多旋翼无人机海洋作业区域多边形的区域障碍物；H2I2G2K2L2为多边形障碍物各边经平移后得到的多边形区域。设各边平移距离为h，平移距离小于多旋翼无人机作业宽度W，即hW，多边形H1I1G1K1L1的各顶点坐标可通过定位仪器得到。假设多边形H2I2G2K2L2的顶