时空约束条件下多旋翼机动轨迹优化方法

蔡志浩，左一鸣，王英勋

（北京航空航天大学 自动化科学与电气工程学院，北京100083）

飞行器的航迹规划指在给定的规划空间内，寻找飞行器从起始点到达目标点，且满足某些约束条件，并使某项性能指标达到最优的轨迹［1］。航迹规划可分为轨迹规划和路径规划两大类。轨迹规划是一种考虑了飞行器的动力学约束，从控制的角度进行飞行轨迹的优化，生成基于时间描述的空间曲线的规划方法［2-5］。

近年来，多旋翼飞行器在震后搜救、地质勘探、电力巡检等民生方面得到了越来越多的应用，这些任务场景往往有着环境复杂多变的特点，并且考虑到飞行器续航能力的限制，如何规划其机动飞行轨迹，满足多旋翼在续航时间、地形地势等时空上的约束条件有着重要意义。

文献［6-7］将控制器与规划相结合，生成一条由12维向量表示的轨迹，能够实现optitrack环境下的90°穿缝等激进飞行；Tordesillas等［8］针对未知环境，开发了一种快速规划算法，能在确保飞行安全性的同时提高飞行速度，飞行验证显示最大飞行速度能达到7.8m/s；曾佳和申功璋［9］采用了Dijkstar算法进行路径的实时规划，并基于飞机的运动学与动力学特性，采用圆弧拟合方法来生成平滑的轨迹，仿真显示，能够生成满足需求的平滑轨迹；陈杰等［10］针对螺旋样条区域，建立了以飞行时间为代价函数的规划问题，并采用二阶泰勒展开法生成轨迹控制序列，仿真显示，相比原先的折线式轨迹，新的平滑轨迹能大大缩减作业时间，提高效率；孟少华等［11］针对微小型直升机的低空避障问题，通过将飞行器的动力学限制和空间障碍转为不等式约束，建立起非线性最优控制问题，采用高斯伪谱法进行求解，仿真显示，该方法可快速且准确地实现避障。这些研究都针对设定环境的任务，从飞行器自身的机动性能等角度，对飞行轨迹作了优化，但对于穿缝、躲投掷物等激进场景，往往还需要飞机有优越的控制器响应性能，且上述研究并没有从任务环境本身出发来思考轨迹优化方法，存在一定的局限性。

本文设定一定时间内穿越室内斜缝的典型任务场景，从针对性分析环境的角度出发，将时空约束具体化，并结合生活经验和环境特点，设计了一种基于抛物原理和庞特里亚金极小值原理的多约束条件下轨迹优化方法。将穿缝过程分解为靠近和穿越2个阶段，在穿越段采用抛物思路优化多旋翼的穿缝轨迹，在靠近段采用庞特里亚金极小值原理设计轨迹实现飞行状态的转换。可视化仿真试验显示，多旋翼最大能穿越竖直方向63°或水平方向32°倾斜的斜缝。

1 四旋翼模型

为了能准确并直观地观察穿缝轨迹的优化效果，需建立可用于仿真试验的飞行平台，综合考虑任务环境和性能需求，本文选用如图1所示的大疆F330作为建模对象进行研究。

图1 大疆F330Fig.1 DJI F330

四旋翼的模型分为3个部分：动力系统模型、控制效率模型、刚体模型。动力系统模型由电机、电调、桨叶组成，由文献［12-13］可知输入的油门信号和输出电机转速之间成一次函数关系，且电机的响应可由一阶惯性环节表示，从而可建立动力系统的数学模型：

式中：ωk为螺旋桨实际转速；Tm为电机响应时间常数；CR为油门和转速线性关系比例系数；ω0为怠速转速；σ为油门。

控制效率模型根据输入的电机转速及桨叶的拉力系数等四旋翼相关性能参数计算其在三轴上受到的力和力矩，可建立数学模型如下：

式中：f为四旋翼总拉力大小；τx、τy、τz分别为螺旋桨升力作用在四旋翼三轴方向上的力矩大小；ct为桨叶拉力系数；cm为力矩系数；la为机臂长度；ω1～ω4分别为4个电机的角速度。

1）刚体模型包括运动学及动力学2部分，为了便于四旋翼的运动学及动力学建模，可作如下假设［13］：①四旋翼飞行器是刚体；②四旋翼整体完全对称；③质量和转动惯量不变；④四旋翼仅受重力和桨推力，仅受旋翼在机体产生的力矩和陀螺力矩。

在此基础上，可建立四旋翼的动力学方程，并由控制效率模型输入的三轴力和力矩求解出其线加速度和角加速度，再根据运动学方程作积分运算，求得其位置和姿态，实现对四旋翼各运动参数的求解，其刚体模型的数学表达式如下：

式中：r为四旋翼位置向量；v为四旋翼速度向量；ω为四旋翼角速度向量；e3=［0 0 1］为单位向量；τ为四旋翼所受三轴力矩；R为地面坐标系到体坐标系的变换矩阵；Ga为旋翼的陀螺力矩；J为机体转动惯量；g为重力加速度；m为四旋翼质量；Θ=［φ θ ψ］为欧拉角；W 为机体角速度到姿态角速度的转换矩阵；下标e代表物理量在地面坐标系下的表达，下标b代表物理量在体坐标系下的表达。

至此，得到了由式（1）～式（3）组成的四旋翼模型的数学表达，在MATLAB中按照上述方程搭建Simulink模型，其中刚体模型部分引用6DOF（Euler Angles）模块表示。

2 PID控制器

2.1 悬停控制器

考虑到悬停段飞行器在飞行过程中俯仰角和滚转角都较小，总拉力约等于四旋翼的重力，可对刚体模型作线性化处理：

式中：φ、θ、ψ分别为四旋翼的滚转角、俯仰角、偏航角；下标xe、ye、ze代表物理量地面坐标系表达下在三轴上的分量。

显然，外环的控制器可以解耦到水平和高度2个通道上。以水平通道为例，可建立位置环和速度环的PID控制，控制器的数学模型如下：

式中：KP、KI、KD分别为速度环的比例、积分、微分环节增益；e为四旋翼误差；下标dh代表物理量水平通道上的期望值，下标h代表物理量水平通道上的实际值，下标w代表控制器位置环的相关增益，下标s代表控制器速度环的相关增益，下标r代表位置环相关物理量，下标v代表速度环相关物理量。

根据式（5）即可得期望姿态角的表达式为

式中：Θdh=［φ θ］为姿态角在水平通道的分量；Aψ为由ψ表示的水平通道加速度与姿态角间的转换矩阵。

类似可得到高度通道上期望净推力的表达式，建立起完整的悬停控制器数学模型并在Simulink中搭建出来。

2.2 追踪控制器

追踪控制器要追踪一条在位置、速度、加速度上都是时变的轨迹，根据芭芭拉引理，可以设计如下所示的水平通道控制器方程：

这样通过加入前馈通道，可实现对轨迹的快速无差跟踪。

3 机动轨迹规划

本文将穿缝轨迹划为2个阶段：靠近段和穿越段。四旋翼从指定位置出发，按照靠近段规划的轨迹，以确定的速度和姿态到达抛出点；再按照抛物线轨迹，被从抛出点抛出，借助惯性穿过斜缝。下文分别对穿越段和靠近段进行具体的理论推导，以得到可行的数学解。

3.1 基于抛物原理的穿越轨迹规划

3.1.1 原 理

在穿越过程中，飞行器的首要目标是避免与斜缝发生相撞，将其可能性降到最小。为了简化分析难度，可将四旋翼按照其带桨状态下的尺寸简化为一个长方体，则易知为了最大程度避免碰撞，需使飞行器以同样的倾斜状态从斜缝的正中心穿越。设计如图2所示的平面Ⅱ，可知，平面Ⅱ与斜缝所在平面垂直且与斜缝两长边平行并过斜缝中心。则规划的穿越轨迹应是过斜缝中心且在平面Ⅱ上的曲线，四旋翼姿态始终保证机架平面与平面Ⅱ重合。

图2 斜缝示意图Fig.2 Schematic diagram of diagonal seam

考虑到斜缝倾斜的角度能达到30°甚至更多，使四旋翼保持长期稳定的大角度倾斜姿势平稳飞过斜缝的难度过大，本文学习文献［14］的方法选择在平面Ⅱ内设计一条抛物线轨迹，借助四旋翼自身的惯性实现穿越，以提高规划轨迹的鲁棒性。

在地面坐标系oexeyeze下，用一组单位向量｛e1，e2，e3｝建立坐标系C描述平面Ⅱ内的运动，为了将四旋翼的运动限制在平面Ⅱ内，需要补偿重力加速度g在平面Ⅱ外的分量，将g投影到e3中，则有重力加速度分量gT：

由四旋翼的质量为m，则需大小为m gT推力来抵消分量影响。由此，将三维的轨迹规划问题转为平面Ⅱ上的二维轨迹规划。此时，重力加速度g只剩下平面Ⅱ内的分量gⅡ，其值如下：

显然，物体在平面Ⅱ内做匀加速度运动，根据牛顿第二定律，有

式中：i取值区间为｛1，2｝，表示e1、e2两个方向上的运动学公式；t0c和t分别为起始时间和当前时间；ri（t）为时间t时四旋翼在坐标轴ei上的位置；vi（t）为时间t时四旋翼在坐标轴ei方向上的速度；gⅡ，i为gⅡ在坐标轴ei方向上的分量。考虑到斜缝不仅有竖直方向的倾斜，还可能存在水平方向的倾斜，因此，物体运动需在2个方向上进行描述。例如，当斜缝只有竖直方向的倾斜时，此时gⅡ，2=0，则四旋翼只有e1方向上的匀加速度运动，e2方向上为匀速运动。

如图3所示，可将三维的轨迹求解问题转为对二维抛物线轨迹的求解，在加速度gⅡ及斜缝中心已知的情况下，给定抛出点p0的位置，可求得穿越段轨迹的表达及相关的状态量。

图3 平面Ⅱ内飞行轨迹Fig.3 Flying trajectory in Plane Ⅱ

3.1.2 数学推导

由上可知，在斜缝倾斜程度已知的条件下，穿越段轨迹唯一地由抛出位置确定，可以建立一个最优化的问题，选出最优的抛出点p0以确保飞行的安全。

考虑式（10）中的r（t0c）和v（t0c），由于在平面Ⅱ内，斜缝对四旋翼的约束仅有2条短边，从安全性的角度出发，需要设计一条抛物轨迹，确保当四旋翼到达中心pg时，其在e1方向上没有速度分量，从而降低四旋翼与斜缝短边相撞的几率。假设到达pg的时间为tg，p0与pg在e1方向上的距离为l，在e2方向上的距离为d，则由v1（tg）=0，有

显然为了安全，tg应当越小越好，即l越小越好，此时考虑四旋翼在动力系统和e2方向上安全距离的约束，可建立最优化问题的数学模型如下：

用l和d来描述2个约束条件：

由于采用的是F330机型，且桨叶直径为203mm，e2方向上抛出点位置至少要距离斜缝265mm，考虑到可能存在的飞行器跟踪误差及现实中对斜缝位置的测算误差，设定e2方向上最小安全距离为350mm，给定不同的最大速度限制，则以l为横轴，d为纵轴，可得到由如下速度约束曲线族及位置约束曲线所框定的可行域范围，如图4所示。

图4 可行域范围Fig.4 Feasible range

根据计算及观察，当取d的最小值时，可取得最小即最优的l，满足最优化条件，从而可通过坐标系C下的三轴单位向量｛e1，e2，e3｝表示出抛出点p0的位置、速度及加速度。本文中设定斜缝的姿态角已知，则可建立坐标系C与地面坐标系的转换矩阵，得到抛出点相关信息在地面坐标系下的表达。

在此基础上，还要考虑穿越轨迹结束的位置，四旋翼到达此点后即进入悬停工作状态。由于一般情况下e1、e2两个方向上都有加速度，四旋翼穿越后并不会回到p0关于斜缝的对称点上，因此，考虑到此时d的取值为最优解，且穿越后易与斜缝在e2方向上发生碰撞，以d为衡量指标，保证穿越起始点、结束点与斜缝在e2轴上的距离相等均为d，利用牛顿第二定律求得轨迹的终止点作为四旋翼穿越后的悬停恢复点ph。

3.2 基于庞特里亚金极小值原理的靠近轨迹规划

靠近段的目标是规划一条状态转换轨迹，确保四旋翼从初始状态出发，追踪此轨迹，最终能实现期望的抛出状态。据此建立最优控制问题，设四旋翼的加加速度j为控制量，取s=［r v a］，以四旋翼的位置r、速度v、加速度a为状态量，建立靠近段任务的问题模型：

参考试验场地的尺寸限制，本文设定靠近段的初态st0即穿缝起始点为直面斜缝4m远与斜缝中心等高的位置，靠近段的末态stf由穿越段抛出点定义。为了保证飞行轨迹的平滑性，此处采用三轴线加加速度的积分和作为代价函数来建立问题。由于此处控制量j存在物理意义上的约束，需采用庞特里亚金极小值原理求解。

3.2.1 原 理

考虑到实际场景中，穿缝时间并不任意，飞机需综合考虑危险性和效率尽可能快得穿越斜缝，在调研国外学者对此问题的研究后，本文设定末态时间tf的取值范围为0～3 s，则此问题是一个积分型指标、末态时间tf约束、末端固定的最优控制问题。根据极小值定理，存在向量λ（t）和非零向量γ，使得最优解x*满足如下的必要条件［15］，其 中，哈 密 顿 函 数 为：H =L（x，u，t）+λT（t）f（x，u，t）。

正则方程：

边界条件与横截条件：

极小值条件：

H变化律：

3.2.2 数学推导

针对本文问题模型，将问题解耦到三轴通道上，为便于描述，下文所有向量在某方向上的分量均不加下标表示，则对任一方向可建立如下的哈密顿函数：

则由正则方程有

由此可解得

根据极小值条件，将λ（t）和H（s，j，λ）的具体表达式代入：

对其关于j求导，有

由此即可求最优控制律和最优状态即最优轨迹如下：

此处假设末态时间tf为一定值，则在上述由参向量α、β、γ表示的最优状态量基础上，令t=tf，可反解出参数值，计算α、β、γ公式如下：

式中：

在此基础上，编写程序循环比较每个末态时间tf对应的代价函数，以找到最优的状态量数学表达，完成靠近段轨迹的求解。

4 仿真试验

4.1 PID控制器仿真

4.1.1 悬停控制器仿真

悬停控制主要应用在起飞、回稳、降落3个阶段，起飞和降落阶段要求飞行器能够平稳、准确地到达指定位置，回稳阶段要求能快速恢复稳定，因此两者在设计上有一定的差异，回稳控制器引入飞行器实时位置作为输入，并适当调整PID控制器参数以使其尽快悬停。

根据试验环境，对起飞、降落阶段的悬停控制器，给定［1 0.8 -1.5］m作为期望位置，图5仿真表明，三轴仅x轴有2%的超调，且稳态误差均为0。

对于回稳控制器，给定4m/s的初速度和40°滚转的初始姿态，图6仿真表明，飞行器在1 s时即趋于稳定，速度响应在1 s时已降至0.1 m/s内，最终滚转和俯仰角均为0，平稳悬停。

4.1.2 追踪控制器仿真

给定螺旋线轨迹让飞行器跟踪，三轴上的位置响应误差最终能控制在4mm内，且x、y轴上的相角差约在0.7°左右，具体观察加速度和速度的跟踪误差，图7仿真表明最终能控制在厘米级。

图5 起飞悬停控制器位置响应Fig.5 Location response of takeoff hovering controller

图6 回稳控制器响应Fig.6 Response of stabilization controller

图7 追踪控制器速度、加速度响应Fig.7 Track controller speed and acceleration response

4.2 任务规划仿真

在前文模型、控制器、轨迹规划完成的基础上，设定如下任务：空间中存在一堵仅留有一倾斜斜缝的墙面，四旋翼从墙前内任意一点出发，追踪确定的轨迹穿过斜缝后，在墙后快速悬停，并平稳降落在指定的位置。

针对此问题，按如图8所示的架构编写状态机程序来检测四旋翼当前的状态，并据此打开所需的外环控制器。四旋翼会经历起飞、靠近、穿越、回稳、降落5个阶段，分别需接入悬停、追踪、油门、回稳、悬停控制器实现控制。任务开始时，系统预先观察斜缝，在得到斜缝中心位置和姿态角后线下完成轨迹规划的工作，并将计算得到的靠近段轨迹数学表达、穿越段所需净推力等信息发送给状态机，状态机据此建立起切换机制，每当四旋翼到达某关键位置时状态机即检测其是否满足设定的容许误差，并决定是否打开新的控制器通道，控制四旋翼下一步的飞行动作。状态机的容许误差由外环控制器性能及仿真穿越效果确定，其指标如表1所示。

本文选择给定斜缝中心位置为［40 -2］m，考虑设定起始位置、斜缝朝向、斜缝竖直方向及水平方向倾斜角、最大飞行速度限制4类变量来设计任务想定，并搭建可视环境，且在斜缝四边上加入碰撞检测传感器，以其输出作为判定指标，验证四旋翼的穿缝效果。

图8 任务规划模型Fig.8 Mission planning model

表1 状态机参数Table 1 State machine parameters

4.2.1 任务流程演示

建立如图9所示由飞机、斜缝、降落平台组成的仿真环境，并在MATLAB中使用3D工具箱搭建。以穿越竖直方向45°斜缝为例，观察整个穿越任务执行的流程，图9展示了飞行器8个阶段下的飞行状态。

具体观察状态的切换情况，如图10所示，状态机共进行了4次切换，经历了5个状态，与预期相符合。

观察此时传感器检测情况，如图11所示，发现4条边上均输出无效信号，说明四旋翼未与斜缝发生碰撞，成功穿越。

图9 飞行任务流程Fig.9 Flight mission process

图10 状态机工作流程Fig.10 State machine workflow

图11 传感器检测情况Fig.11 Sensor detection

4.2.2 仿真试验1：不同斜缝倾角及速度上限

斜缝倾斜角及最大飞行速度是限制穿缝成功的主要因素，本文综合竖直方向倾斜、水平方向倾斜及最大飞行速度设计了多组想定并进行仿真验证，总结四旋翼穿缝性能如表2所示。

由于整个穿缝任务的核心在靠近段的姿态转换，以竖直方向45°倾斜，水平方向倾斜0°斜缝，最大速度限制4m/s为例分析靠近段外环追踪控制器效果。

表2 四旋翼穿缝性能Table 2 Sewing ability of quad rotor

图12 靠近段轨迹加速度响应Fig.12 Acceleration response of tracking of approach trajectory

图12给出了飞行器三轴在加速度上的响应情况。仿真显示，三轴上位置和速度响应最终能较好地跟上期望的轨迹，但加速度在追踪后期的误差明显增大。具体测算靠近段到穿越段的切换点处响应情况，位置误差为0.03m，速度误差为0.05m/s，加速度误差为1.26m/s2，显然位置和速度跟踪效果较好，但加速度的跟踪性能明显下降。分析其原因是：由于在靠近段的末期姿态角较大，则先前基于运动学及动力学方程线性化设计的外环控制器性能会明显下降，为了改善这一情况，后续需针对外环控制器的非线性设计继续研究。

4.2.3 仿真试验2：不同起始位置及斜缝朝向

考虑到室内封闭环境的任务特点，四旋翼仅能通过相机来对斜缝进行观察，飞行过程中需要机头始终朝向斜缝方向，因此，斜缝的朝向会对任务的执行产生一定的影响。此外，由于飞行器可能在斜缝墙前任意位置出发，不同出发位置对穿缝效果也存在一定可能的影响。针对这两者提出的解决办法是：在开始穿缝前引导其飞向一个直面斜缝且距离为4m的等高点，并调整机头方向使正对斜缝。

给定［2 -2 0］m和［-2 2 0］m两个不同的起始位置观察穿越效果，如图13（a）所示，四旋翼主要在由起飞点到达起始点的轨迹上存在差异，从穿缝起始点出发的穿缝轨迹类似，最终都能成功降落在平台上。

再给定斜缝偏航发生0°、60°、90°偏转，仿真表明都能实现成功穿越，具体观察偏航角的偏转，图13（b）仿真显示靠近段和穿越段过程中飞机机头始终指向斜缝，最大仅发生8°左右的偏转，满足相机观测的需求。

图13 位置与朝向条件变化下穿越结果Fig.13 Crossing results at different locations and directions

5 结 论

针对时空约束条件下多旋翼机动轨迹优化的问题，设定了穿越室内斜缝的任务场景，从具体分析任务对象特点的角度出发，运用抛物原理和庞特里亚金极小值原理，对四旋翼的穿缝轨迹进行了优化。

1）进行了四旋翼建模、不同功能外环PID控制器设计、可视仿真环境搭建，构成用于进行规划效果验证的仿真飞行平台。

2）基于抛物原理的轨迹规划方法最大能实现竖直倾斜63°或水平倾斜32°的斜缝穿越。

3）本文方法减轻了轨迹规划的算力负担，降低了对内外环控制器的性能需求，在工程中具有较好的可行性。