基于包络切换的四旋翼吊挂无人机避障轨迹优化研究*

康晶杰 黄政 文浩

（南京航空航天大学机械结构力学及控制国家重点实验室，南京210016）

引言

四旋翼无人机具有可垂直起降、机动性强、易维护等特点，近年来在民用、军用以及警用领域获得了极大的发展.通过四旋翼无人机吊挂载荷飞行，可以在其他运输方式难以到达的地方，完成物资运送和人员救援等任务.四旋翼吊挂无人机飞行技术已在救灾、灭火以及危险区域援助等方面呈现出突出优势，成为近年的研究热点［1，2］.开展四旋翼吊挂无人机轨迹优化研究，解决其在复杂环境下的应用问题，对推动四旋翼吊挂无人机技术发展具有重要意义.

相对于无吊挂系统，四旋翼吊挂无人机的动力学、轨迹优化与控制问题无疑具有更大难度.一方面，吊挂与无人机本体间的动力学耦合使系统复杂度大增.特别是在吊挂无人机升降和飞行过程中，可能因系绳的松弛∕拉紧切换会产生动力学描述切换，因而难以建立各个飞行阶段的统一动力学模型.另一方面，由于缺乏直接作用于吊挂体的作动力，仅能通过动力学耦合间接施加控制，无疑增大了吊挂无人机的轨迹优化和控制难度.

四旋翼吊挂无人机动力学建模是开展轨迹优化及控制研究的基础.近年来，学者们针对四旋翼吊挂无人机系统发展了多种动力学模型.例如，Zhou等［3］假设吊挂无人机上的系绳始终处在绷紧状态，建立了吊挂无人机的三维动力学模型.Sreenath等［4］考虑系绳拉紧和不拉紧两种情况，将吊挂无人机系统建模为混合微分平坦系统.Goodarzi等［5］摒弃了系绳是无质量且始终张紧的假设，将其建模为由球形节点串联的一系列连杆.Foehn等［6］采用三个关节坐标来描述有效载荷运动，其中两个为旋转关节，而另一个是柱形关节.该研究将系绳拉紧和放松状态建模为一个互补约束，从而实现了两种状态的统一建模.

轨迹优化是实现吊挂无人机自主飞行的重要环节，是指在满足特定约束条件下，确定从起始状态到目的状态的转移轨迹以及相应的控制输入，以优化任务性能指标.例如，Foehn等［6］将吊挂四旋翼无人机轨迹优化问题表述为具有互补约束的数学规划问题，通过序列二次规划（Sequential Quadratic Programming，简称 SQP）求解了最优轨迹 .Tartaglione等［7］将吊挂无人机系统避障问题视为两个圆形的不干涉问题，并基于模型预测控制（Model Predictive Control，简称MPC）方法计算了最优轨迹.Son等［8］仅考虑吊挂物避障问题，计算出了连续避障多个圆形障碍物的无碰撞轨迹，并完成了相应的实验.值得注意的是，现有吊挂无人机避障运动研究中，通常将吊挂无人机全系统包络圆半径设为定值，忽略了吊挂载荷摆幅度变化导致的包络圆切换的问题.然而，当系绳长于无人机本体包络圆半径时，吊挂载荷可能越出无人机本体包络圆的范围，造成包络圆切换.若将系统整体包络圆半径固定设置为系绳全长，则过于保守［6］；而若仅考虑无人机本体包络圆半径，则不能用于吊挂物摆出本体包络圆的情况，存在碰撞风险［7］.此外，为了简化分析，现有吊挂无人机轨迹优化研究中，通常将障碍区域近似为圆形，其避障轨迹优化方法缺乏普适性，实际工程意义欠佳［6-8］.

本文基于带有互补约束的吊挂四旋翼无人机三维动力学模型，考虑非圆避障区域以及吊挂载荷摆动导致的避撞包络圆切换问题，开展避障轨迹优化研究.文中基于R函数建立了不同包络圆下的统一避障约束，使用碰撞检测算法计算包络圆与障碍物的距离，并通过非线性最优控制方法实现吊挂无人机避障轨迹优化.最终，将开环最优控制问题离散为非线性规划问题，通过数值求解得到了吊挂无人机的最优运动轨迹.

1 吊挂无人机动力学模型

吊挂四旋翼无人机系统简图如图1所示.建模过程中采用了3套坐标系，即世界坐标系owxwywzw、机体坐标系ob-xbybzb和吊挂物坐标系osxsyszs.图中吊挂无人机单个旋翼所提供的升力为Fi，提供的绕旋转轴的反作用力矩为Mi，i=1，2，3，4分别与四个旋翼对应 .建模时近似认为Mi=kFi，其中k为常值比例系数.无人机旋翼产生的绕体轴系各轴的力矩分别为τxb，τyb和τzb.此外，本文以mQ与mq分别代表四旋翼无人机和吊挂物的质量，I=[Ixx，Iyy，Izz]T为四旋翼无人机相对于相对连体坐标系轴的转动惯量.吊挂无人机系统的整体动力学方程为［6］：

图1 吊挂无人机系统结构简图Fig.1 A schematic diagram of the quadrotor with slung payload

其中，通过互补约束式（2）-式（4）对拉紧-松弛系绳进行统一描述，q=[xQ，yQ，zQ，ψ，θ，φ，αL，βL，ρ]T表示系统的状态向量，pQ=[xQ，yQ，zQ]T表示四旋翼无人机在惯性系下的位置坐标，η=[ψ，θ，φ]T表示用于无人机本体姿态描述的Z-Y-X形式欧拉角.此外，αL和βL分别表示系绳绕坐标系os-xsyszs中xs轴和ys轴的转动的角度.式中，l0和ρ分别表示系绳原长和当前长度；Jλ(q)和λ分别表示约束方程映射在广义坐标下的雅可比矩阵和系绳张力；B(q)和U分别表示无人机的输入矩阵以及力和力矩输入向量，其中 U=[F，τψ，τθ，τφ]的四个分量分别是四个旋翼推力的合力大小以及对应于欧拉角ψ，θ和φ的广义力矩.为简化分析，考虑欧拉角为较小值，将欧拉角广义力矩τψ，τθ和τφ分别近似为体轴力矩τzb，τyb和τxb，导出近似表达式U=Eu，其中

2 吊挂无人机轨迹优化

本节将建立四旋翼吊挂无人机轨迹优化问题的非线性最优控制模型，基于R函数描述不同包络圆下的统一避障约束［9］，使用碰撞检测算法计算包络圆与障碍物的距离［10，11］.并通过Legendre-Gauss-Radau（LGR）伪谱法将非线性最优控制问题离散，以便通过非线性规划方法求解［12］.

2.1 轨迹优化模型

为便于建立轨迹优化模型，将式（1）改写为：

引入状态向量 x=[q，q̇]T，将方程 6 转化化为状态空间形式吊挂无人机的轨迹优化问题可表述为如下非线性最优控制问题

其中，J(x，u，t)性能指标函数；Θ为不等式路径约束；Φ为系统初始时刻和末端时刻约束，对应于给定的t0时刻的初始状态

和tf时刻的末端状态

考虑到飞行中的能源消耗问题，希望在满足其他约束条件的情况下，使吊挂无人机在优化轨迹中的能源消耗较少.进一步考虑状态误差抑制需求，选取最优性能指标［13-15］为

其中Q'和R'分别是状态和对角加权系数矩阵.

2.2 路径约束

为了计算出安全有效且满足吊挂无人机自身物理限制的轨迹，需要对其模型施加适当的约束限制.主要考虑的约束如下：

（1）互补约束.上文中提到将系绳拉紧和松弛状态表达为一个互补约束，其含义为：当绳子绷紧时l0-ρ=0，且 λ≥0；绳子松弛时 l0-ρ≥0，且λ =0.该约束对应式（2）-式（4）.

（2）推力约束.考虑到吊挂无人机的硬件限制，各个电机驱动螺旋桨产生的推力Fi应满足

式中Fmin和Fmax分别表示推力下、上限.

（3）角度约束.考虑到吊挂无人机在控制飞行时的安全问题，给四旋翼无人机的姿态角和系绳的摆动角施加如下约束

（4）避障约束.为了使吊挂无人机能安全地实现障碍物绕飞.可以考虑使用吊挂无人机系统包络圆来有效简化系统的外形描述，如图2所示.考虑挂载荷摆动导致的避撞包络圆切换问题：a）当载荷位置超出四旋翼无人机本体包络圆范围时，取系统整体包络圆半径为r1；b）当载荷运动没有超出四旋翼无人机本体包络圆范围时，取包络圆半径为r2.

图2 吊挂无人机系统包络圆最大半径Fig.2 Maximum radius of enveloping circle of hanging uav system

根据吊挂无人机的系统模型可得

其中，rp为四旋翼无人机螺旋桨半径，L为吊挂无人机系统中四旋翼无人机的机臂长度.数值计算过程中发现，若直接根据包络圆切换条件和式（14）计算整体包络圆直径，数值计算耗时很长.为此，基于R函数，对包络圆直径做近似计算，以提高计算效率.R函数是指仅由实变量的符号决定自身符号的实值函数［9］，例如可以用来求最小值或最大值：

其中，在碰撞检测中，将吊挂无人机系统抽象成半径为rmax的包络圆OQ.考虑对中心在坐标原点o的长为a，宽为b且顺时针旋转s度的矩形障碍物进行避障.同时，由于无人机定位精度差异，可以增加安全距离r'，使系统安全地避开障碍物.

为移除圆形障碍物假设，处理更一般性的障碍物 .采 用 GJK（Gilbert-Johnson-Keerthi）和 EPA（Expanding Polytope Algorithm）算法，进行吊挂无人机包络圆与障碍区域的碰撞检测和距离计算［10］.由于GJK算法只适用于检测两凸体间的距离，以及是否发生碰撞，无法解决优化迭代过程中两凸体相互重叠和穿刺的情况.为此，引入EPA算法对两凸体碰撞后相互重叠部分的穿透深度和穿透向量进行计算，从而保证定义的名义距离能够在实数域内连续，以保证优化算法迭代顺利进行.

2.3 LGR伪谱算法

针对上述优化问题的求解，目前应用较广泛的是LGR伪谱算法，其优点在于可以处理任务约束和非线性因素，并对给定性能指标进行优化.针对于吊挂无人机机动最优控制模型，使用全局多项式对状态变量、控制变量和乘子变量进行插值近似，在一系列LGR配置点施加动力学方程以及其他路径约束，从而将最优控制问题转化为非线性规划问题.由于LGR配置点位于半开区间τ∈[-1，1)中，首先需要对变换时间τ和连续飞行时间t∈[t0，tf]进行仿射变换

在转换后的区间τ∈[-1，1)内分布着N个LGR配置点(τ1，τ2，L，τN)，其中

并且引入未配置的点τN+1=1来对应机动飞行的终止时间tf.值得注意的是，为了提高数值求解效率和精度，实际计算中常通过hp自适应算法等策略对所使用的配置数目和分布进行自适应调整［12］.使用Lagrange内插多项式对状态向量、控制向量和乘子变量进行插值近似

式中Xi，Ui和Λi分别为状态向量和控制向量在配置点处的离散近似值.对式（23）中的第一个式子进行求导运算，可得τk处状态变量导数近似为：

式中撇号表示对变换时间τ的导数.由此可将式（8）中的状态方程离散为

式（8）中的端点和路径约束经过伪谱法离散得到

此外可采用Gauss积分公式对式（11）对应的目标函数进行离散近似.基于上述方法，将吊挂无人机轨迹优化问题转化成了标准的非线性规划（Nonlinear Programming，简称NLP）问题［12］.

3 数值仿真结果

本节通过数值仿真算例对前述轨迹优化方法进行考核验证.表1给出了仿真采用的吊挂无人机系统物理参数，表2为吊挂无人机的初始状态和路径规划的参数，表3为施加在路径上的路径约束参数.设定飞行时间tf=4s，吊挂无人机系统由pw，0飞行至pw，1，给定系数矩阵分别为

表1 吊挂无人机系统的物理参数Table1 Physical parameters of hanging drone system

表2 吊挂无人机初始状态和末端状态的位置和速度参数Table2 Position and speed parameters of the initial state and end state of the hanging drone

表3 路径约束参数Table3 Path constraint parameter

数值仿真中，采用通用伪谱最优控制软件GPOPS（GeneralPseudospectralOptimalControl Software）对最优问题进行离散，并选用hp自适应算法对配置点进行自动调整，以提高数值求解效率和精度.使用SNOPT求解器（Sparse Nonlinear Optimizer）中的SQP算法求解离散后的NLP问题［16］. 此 外 ，通 过 引 入 Flexible-Collision-Library（FCL）碰撞检测库中的GJK-EPA模块，实现吊挂无人机包络圆和障碍物之间的距离检测及计算［17］.该仿真程序运行计算机的中央处理器为AMD R5 3550H，其主频为2.1GHz；内存容量为16G；仿真软件为Matlab R2019a.仿真算例的结果显示经过了hp自适应算法迭代，其总耗时约为262秒.

LGR伪谱法求得的最优轨迹优化结果如图3-图6所示.其中，图3为吊挂无人机避障飞行运动轨迹在x-y平面的投影.图4给出了吊挂无人机位移、姿态角和系绳摆动角随时间历程，图5为相应的速度响应曲线.从图3可以看出，吊挂无人机成功避开了矩形障碍物，未发生任何碰撞，并且在系统通过矩形障碍物第二个顶点时，吊挂无人机的包络圆发生了切换.从图4（a-c）可以看出，吊挂无人机的位移、姿态角和系绳摆动角最终都到达了期望位置，证明吊挂无人机系统成功完成了避障飞行.此外，图5（a）和（b）表明要求的速度和角速度的终端约束得到了满足.图6表明所给定的推力约束式12得到了满足.受限于计算规模和硬件性能，难以通过LGR伪谱法实时求解出最优轨迹.因此，拟在后续研究中以求得的最优轨迹为参考，通过轨迹跟踪方法实现实时控制和实验验证.

图3 吊挂无人机运动轨迹图Fig.3 Trajectory diagram of the UAV with slung load

图4 吊挂无人机位移（a）、姿态角（b）和系绳摆角（c）随时间历程Fig.4The displacement（a），attitude angle（b）and tethered pendulum angle（c）of the UAV with slung load over time

图5 吊挂无人机平移速度（a）与角速度（b）随时间历程Fig.5 The translation velocity（a）and angular velocity（b）of the UAV with slung load over time

图6 吊挂无人机控制输入力随时间历程Fig.6 The UAV with slung load controls the input force over time

4 总结

本文基于带有互补约束的三维吊挂无人机模型，研究了轨迹优化和控制问题.通过互补约束对拉紧-松弛系绳进行统一描述，建立了吊挂四旋翼无人机系统的整体动力学模型，为吊挂无人机轨迹优化提供了基础.采用R函数在轨迹优化过程中实时判断吊挂无人机系统最小包络圆半径，并使用GJK-EPA算法来检测包络圆与障碍物之间的名义距离，通过非线性最优控制方法实现吊挂无人机避障轨迹优化.利用LGR伪谱法，将开环最优控制问题离散为非线性规划问题，通过数值求解得到了吊挂无人机的最优运动轨迹.