基于螺旋样条的旋翼无人机区域轨迹规划

陈 杰 董 伟 盛鑫军 朱向阳

上海交通大学机械系统与振动国家重点实验室，上海，200240

0 引言

多旋翼无人机具有体积小、结构简单、能够垂直起降与悬停等特点，在民用航拍、工业巡检[1]、农业作业[2]、遥感测绘[3]、公共安防、军事侦察[4]等领域具有广阔的应用前景。将旋翼无人机应用于区域作业时，一种普遍的做法就是先提取作业区域的范围，规划无人机的飞行轨迹，之后，无人机按照规划的轨迹自动完成作业区域的巡检。相比于传统的人工检测和手动操控无人机完成作业任务，这种方式能极大提高效率。

无人机飞行轨迹规划的传统做法是，先在作业区域范围内生成一系列的航迹点，然后以Z字形的折线连接生成覆盖区域的轨迹[5-6]。这种方法简单高效，在植保喷洒农药作业、地理信息航拍等领域广泛应用。Z字形的轨迹规划方法生成的轨迹与无人机的动力学性能不相符，引起了飞行姿态的不平滑。CHEN等[6]对每段直线进行速度规划来保证无人机加减速过程的姿态平滑，该方法需要无人机在拐点处频繁进行加速与减速，作业效率低，且频繁的加速与减速增加了无人机的能耗，缩短了无人机的续航时间。加工制造领域中，刀具的轨迹规划及优化已有成熟的基础，文献[7-8]将螺旋加工轨迹的方法用于型腔铣，在保证刀具磨损度的前提下，提高了加工效率。这种加工轨迹的规划方式也同样适用于无人机区域作业的轨迹规划。

除了折线型的直线连接，另外一种生成轨迹的方法为样条拟合，通过控制样条拟合的阶数来保证曲线在连接点处连续的导数阶数，从而规划得到无人机姿态平滑变化的轨迹，这更符合旋翼无人机飞行特性。数控加工领域，样条拟合是一种较为常用的轨迹生成方法[9-10]，拟合的曲线通过约束速度、加速度，对加工过程的加减速进行规划，生成平滑的加工轨迹。在无人机的轨迹规划研究中，PHANG等[11]基于B样条拟合文字曲线，采用二次规划的方法迭代获得时间最优的轨迹，控制无人机完成给定文字的绘制。为了提高轨迹生成效率与实时性，董伟[12]基于非均匀有理样条，采用线性规划的方法来求解最优飞行时间问题。上述研究中，无人机飞行轨迹的拟合多基于样条曲线，生成的轨迹为单参数的方程，便于进一步的轨迹优化与插补。

本文提出一种用于旋翼无人机区域作业的螺旋轨迹生成方法，基于非均匀有理B样条(non-uniform rational B-spline，NURBS)曲线拟合螺线曲线，在曲线离散化后，采用时间最优的优化模型，将速度、加速度和跃度作为约束条件，求解最优飞行速度序列，并根据无人机的控制周期进行插补，基于轨迹追踪控制器实现螺旋轨迹的追踪控制。

1 区域螺旋轨迹的生成

为了使生成的轨迹能够覆盖整个作业区域，选择距离中心点最远的点，根据无人机作业覆盖区域得到螺旋线的圈数：

(1)

轮廓线向中心方向偏置σ(根据实际作业需求确定)，得到第一层曲线。以第一层曲线为基准，以每两层向内偏置2Rε，生成m层轮廓曲线，如图1b所示。轮廓集表示为

Q={Q1，Q2，…，Qm}

(2)

qi,j=q1,j+i(o-q1,j)/m

(3)

其中，Q为由所有轮廓组成的总轮廓点集；Qi为i层轮廓组成的点集，i=1，2，…，m；j为该层轮廓点数。

图1 螺旋曲线的控制点生成Fig.1 Generation of spiral control points

中心点和所有的轮廓点将m层轮廓曲线分成n份。对每两层轮廓曲线进行计算，得到曲线在折线处的控制点集

D0={di，j|di，j=qi，j+(j-1)(qi+1,j-qi，j)/n}

(4)

选定最外层的一个点为起点，按照顺时针或逆时针方向逐层向内将控制点集组成有序的点集序列：

D={dI|I=1，2，…，mn}

(5)

式中，dI为点I的坐标。

定义参数δ来控制折线处生成曲线的曲率。在点集序列D的每两个相邻控制点之间生成控制点，如图1b所示，得到曲线的控制点集P={pi}，pi为控制点坐标。生成控制点方法如图2所示。

图2 控制点生成流程图Fig.2 Flow diagram of control points generation

本文采用NURBS曲线拟合无人机的飞行轨迹，得到轨迹相对于单个参数的函数，以便于进一步的轨迹优化。

一条k(k=0，1，2，…)阶的NURBS曲线定义如下[11]：

(6)

其中，wj为权因子；u为曲线参数，0≤u≤1；Nj，k(u)为NURBS曲线的基函数，通过递推公式获得，k=0时

(7)

k>0时

(8)

曲线的节点向量为

U=(u0，u1，…，un+k+1)

(9)

对于NURBS开曲线，两端节点的重复度取k+1，即u0=u1=…=uk=0，un+1=un+2=…=un+k+1=1。节点向量U根据哈特利-贾德方法[11]确定。

这里令wj=1，可以简化得到

(10)

为了得到无人机平滑飞行的轨迹，即保证无人机飞行过程中姿态的平滑变化，无人机姿态的变化率需要是可导的，其中，姿态对应于加速度，姿态的变化率对应于跃度，所以跃度是可导的，飞行曲线需4阶可导。本文采用4次NURBS曲线进行拟合。

根据得到的控制点集P、节点向量U，可获得拟合的螺旋曲线，图3所示为由4阶NURBS样条拟合控制点得到的螺旋样条曲线。

图3 4阶NURBS样条拟合的螺旋样条曲线Fig.3 Fitting of spiral curve by 4-order NURBS

对于一般具有较为复杂凸边界的区域，通过提取边界轮廓的点集，应用以上轨迹生成算法，均可生成覆盖整个区域的螺旋作业轨迹，图4所示为不规则区域的螺旋轨迹。

图4 不规则区域螺旋轨迹生成Fig.4 Trajectory generation on an irregular region

如图5所示，区域中心不在区域范围内的凹边界无法直接采用上述的算法生成轨迹。针对这种情况，将区域进行分割，得到2个凸边界组成的区域，分别对2个区域生成轨迹控制点，连接两个区域的控制点，然后进行样条拟合，就可得到连续的覆盖区域的双螺旋飞行轨迹。对于更加复杂的边界，同样可采用这种分割拟合的方法，生成多螺旋区域覆盖轨迹。

图5 特殊区域分割多螺旋轨迹生成Fig.5 Trajectory with multi-spiral curve for special region

2 轨迹规划

2.1 速度规划

得到生成的轨迹后需要进行速度规划。在无人机满足自身动力学性能的条件下以最短的时间完成作业任务，需要建立有速度约束、加速度约束及跃度约束，以最短的时间为目标的优化问题，完成轨迹的速度规划。

引用文献[10]的最优速度规划方法，将飞行时间最短问题等效为每点速度最大的优化问题，将曲线离散化为N个区间，可以得到如下最优化问题：

(11)

式中，v(uJ)为轨迹离散点uJ的速度；(·)μ表示向量在μ轴上的分量，μ∈{x,y,z}；vmax为最大飞行速度约束；aμmax为μ轴的最大飞行加速度约束；jμmax为μ轴的最大跃度约束；τ为轨迹追踪精度约束；ρJ为uJ处的曲率半径；εmax为最大弓高误差约束；T为插补周期。

令β=‖dC(u)/du‖，γ=v2(u)/β2，则加速度表示为

进而通过差分以离散形式在uJ得到加速度

对于A(u)的μ轴分量

即

aμ(uJ)=C″μ(uJ)γ(uJ)+C′μ(uJ)γ′(uJ)/2

跃度可表示为

式中，F′(uJ)、F″(uJ)、F‴(uJ)分别为函数F(uJ)对uJ的1阶导数、2阶导数、3阶导数。

以v2(uJ)为变量，将式(11)的优化问题转换为一个线性规划问题，确定轨迹的速度、加速度的边界约束，利用最优理论求解得到最优速度序列。图6所示为图3中的矩形区域螺旋轨迹在速度约束为3 m/s、加速度约束为5 m/s2、跃度约束为3 m/s3时的规划结果。轨迹的速度、加速度基本达到约束要求，且曲线曲率较大位置的速度较小，符合旋翼无人机的动力学特性。有跃度约束的情况下，无人机的加速度在约束范围内受到了一定的限制。

图6 螺旋轨迹速度规划结果Fig.6 Results of spiral trajectory planning

2.2 曲线插值

若需无人机完成生成轨迹飞行的任务，则需要将轨迹按控制周期向时域内映射，即需要对轨迹按照飞行器控制周期进行插值。综合考虑精度与计算量，本文采用二阶泰勒展开法[12]进行插补参数计算。

曲线参数的时间函数记为u(t)，ti为第i个控制周期的时刻，ti+1为第i+1个控制周期的时刻，记u(ti)=ui，u(ti+1)=ui+1，对ui+1进行二阶泰勒展开可得

由于初始点的速度为0，无法计算得到下一个插值点，因此需要给初始点一个小的速度。计算得到新的插值点ui+1后，基于每两个离散点之间速度的变化是线性的假设，离线点之间的新插值点的速度由相邻两个离散点已知的速度，通过线性拟合得到，从而通过插值得到间隔为控制周期的位置序列与速度序列。图7a所示为曲线插值的结果，由图7b可以看到，在速度较小的位置，插值的轨迹点更加密集。

图7 螺旋曲线插值Fig.7 Curvilinear interpolation of spiral trajectory

3 轨迹追踪控制

多旋翼无人飞行器的控制系统一般由4个级联的PID控制器组成，包括位置环、速度环、姿态环和角速度环，如图8所示，通过设定目标位置实现位置闭环。

图8 飞行控制器的级联控制框图Fig.8 Cascade control block diagram of flight controller

由前一节的插值得到对应控制周期的位置序列，将这些位置作为每个控制周期的目标位置，在每个控制周期向无人机发送位置设定指令，实现轨迹的追踪，但采用这种追踪方式时，无人机的轨迹追踪性能不佳，故基于轨迹的速度序列，加入速度前馈，从而获得较好的轨迹追踪性能，如图9所示。速度控制量可表示为

v=kpep+vff

式中，ep为位置误差；kp为位置追踪器比例系数；vff为速度前馈。

图9 轨迹追踪控制框图Fig.9 Control block diagram of trajectory tracking

本文基于开源飞行控制器Pixhawk搭建了飞行平台 ，以实现基本飞行姿态的控制，将Raspberry Pi 2作为上层控制器，以实现轨迹追踪控制。

4 实验与结果

针对一块特定的10 m×10 m的矩形区域，在相同的动力学约束以及作业覆盖要求的条件下，分别采用本文的螺旋轨迹生成算法、折线型轨迹规划算法[6]生成并规划轨迹。实际作业过程中，会根据无人机的动力、作业需求等实际情况，限制无人机飞行的最大速度、最大的飞行姿态倾角、最大的姿态角速度，对应于无人机的速度、加速度以及跃度，本文设定无人机的速度约束为3 m/s，加速度约束为5 m/s2，跃度约束为3 m/s3，无人机作业覆盖直径为2 m。两个算法生成的覆盖轨迹如图10所示。

图10 10 m×10 m的正方形区域轨迹规划Fig.10 Trajectory planning of a square with 10 m×10 m

Pixhawk平台能实时记录飞行的位置、速度、姿态等无人机的状态信息并将其保存到SD卡。测试完成后，读取记录的数据，并画出无人机的位置、速度、姿态信息。图11、图12所示为两种区域轨迹规划的实验结果。

在相同的动力学约束条件下，两次实验过程中，无人机的速度、姿态均在给定的约束范围内，本文方法的作业时间29.3 s比折线型区域轨迹规划方法的作业时间34.7 s缩短了15.5%，有效地提高了区域飞行的效率。

5 结论

针对旋翼无人飞行器在区域作业中的应用需求，提出了一种基于非均匀有理B样条拟合螺旋曲线的轨迹生成方法，并针对特殊区域提出先分割区域，再拟合的多螺旋轨迹生成方法。根据旋翼无人机的动力学特性，基于速度、加速度、跃度约束条件进行轨迹规划，获得了无人机在最短时间完成作业轨迹的位置速度序列，采用二阶泰勒展开法进行曲线的插值，得到用于控制的轨迹序列，设计了轨迹追踪控制器，基于开源飞行平台完相比较于传统折线区域轨迹规划方法，螺旋成了飞行对比实验。轨迹规划方法能够在保证无人机飞行姿态平滑且完成相同作业任务的前提下，提高无人机区域作业的效率，缩短作业时间。

图12 折线轨迹规划实验结果Fig.12 Flight test result of zigzag trajectory