Optimal Trajectory Planning of a Quadrotor toward Free Flying Target Catching
-
摘要:
自由飞行目标物捕获作为动态任务，在其被执行的过程中，四旋翼不仅要规划出一条时间最优的追踪轨迹，而且还要根据目标物的位置反馈信息实时对轨迹进行重新规划，以实现在最短的时间内追上目标物.针对这一问题，提出了诱导时间最优MPC（model predictive control）算法用于四旋翼的轨迹规划.该算法通过宽松约束条件下时间最优轨迹的引导，利用MPC的滚动优化策略，可以在每个控制周期内用反馈信息实时求解时间最优的追踪轨迹.为了躲避追踪路径中的障碍物，本文还提出了一种用动态线性约束表示障碍物的方法，以提高障碍物约束下轨迹求解的效率.结合诱导时间最优MPC的算法，可以在线实时地求解出具有障碍物避碰能力的时间最优轨迹.仿真结果表明了本文提出算法的有效性，其高效的计算效率也能满足实际系统对算法实时性的要求.
-
关键词:
- 四旋翼 /
- 轨迹规划 /
- 时间最优控制 /
- 模型预测控制(MPC) /
- 障碍物避碰
Abstract:Free flying target catching is a dynamic task. When this task is executed, in order to catch up with the free flying target in the minumun time, the quadrotor not only need to plan a time optimal trajectory to pursue the target, but also need to replan the trajectory based on target's postion feedback in real time. Toward free flying target catching, We propose a guidance time optimal MPC (model predictive control)algorithm which can be used in trajectory planning of the quadrotor. Based on the receding-horizon optimization strategy, this algorithm can generate time optimal trajectory in every control cycle under the guidance of the time optimal trajectory in the relaxing constrains. In order to avoid obstacles in the pursuing path, a dynamic linear constrain of the obstacle is also presented, which can improve the computationally efficient. Combining with the dynamic linear constrain of the obstacle, the proposed algorithm can generated trajectory that can avoid obstacle reactively while catching up with the target in the minimum time. Simulation results show the validity of the proposed algorithm, and the higher computationally efficiency make it possible to apply it in the real system.
-
0. 引言
近年来随着导航和控制技术的发展，四旋翼(多旋翼)已经成功地应用于农业[1]、灾难救援[2]和地型测绘[3]等领域.对于简单的任务，如获取某个地点的图像信息等，四旋翼可通过其底层的控制器跟踪提前规划好的某些路径点完成.这些简单任务的特点是只需要四旋翼无人机到达几何空间中的某些位置点，对其到达该点的时间没有限制.但是，对于高难度的任务，如四旋翼飞行穿过狭小的缝隙[4]等，这些任务往往需要考虑运动学和动力学约束并在执行过程中追求某些最优指标，如时间最短或耗能最小等.对于此类任务，四旋翼需要按照一条可执行轨迹飞行，使其在任何时间上都能到达一个指定位置点，即需要一系列带有时间标签的位置点，以满足系统约束和性能指标的要求.因此，对于高难度和高标准的任务，四旋翼的轨迹规划是其能够完成任务的关键.
轨迹规划问题可以描述为，在满足系统动力学约束、输入约束和状态约束的情况下，找到一条可执行并且能够完成任务的轨迹.按照轨迹的表示形式，四旋翼的轨迹规划方法的研究大致可以分为两类.
其中，在第1类研究中，四旋翼的轨迹用时间的多项式来表示.用于表示轨迹的多项式可以为高阶多项式[5]、伯恩斯坦基多项式(Bernstein Basis Polynomials)[6]或贝塞尔曲线[7]等.此类研究往往与路径规划相结合使用，首先，路径规划作为上层的规划找到一系列满足空间几何约束条件的路径点，如障碍物避碰的路径点.之后轨迹规划用来解决从初始路径点到下一个路径点的时间平滑问题，找到一条可执行的轨迹，连接两个路径点.此类研究以时间多项式作为轨迹的模型，利用四旋翼系统动力学的微分平滑特性(flatness)，把多项式轨迹关于时间的各阶导数与系统的输入和状态约束建立对应关系[5, 8]，在轨迹的多项式的各阶导数满足约束的情况下，该轨迹就能被四旋翼执行.在多项式各个参数求解的过程中，可以引入加速度平方和最小或者耗能最小等二次型指标把它转化为二次型优化问题[9-10].由于上层的路径规划算法多为基于采用或基于搜索的算法[11]，如A*[12]和RRT*[13]等，计算量大，因此，该类方法适用于静态任务，在需要障碍物避碰的动态任务中，难以实现实时的反馈闭环规划，不能满足动态任务的要求.
第2类研究为通过最优控制方法对轨迹进行求解.众所周知，最优控制理论可以求解一个动力系统的最优控制律以及该最优控制律对应的最优轨迹.第2类研究的特点是在轨迹求解的过程中直接考虑四旋翼的动力学约束，把轨迹求解描述成一个最优控制问题[14]，在用最优控制的方法进行求解，如庞特里亚金极大值原理(Pontryagin′s maximum)[15-16]、MPC[17]等.此类轨迹规划的算法具有闭环的结构，在执行动态任务是有比较好的效果[18].该类方法由于直接考虑系统动力学约束，在状态约束比较复杂的情况下，如考虑几何空间障碍物，难以通过最优控制的方法进行求解.
四旋翼和简单的手爪或机械臂相结合后，具备简单的抓取作业能力，可以完成简单的抓取任务[19]，也使四旋翼捕获自由飞行目标物，如微型无人机，成为了可能.在执行自由飞行目标物的捕获任务时，四旋翼需要在最短的时间内追上目标物并实施抓捕.与之相关的研究有四旋翼接住抛出去的小球[20-22]，跟踪动态目标物[23-24]等.由于抛出去小球在空中运动时受到的外力主要是重力，因此，可以用抛物线的模型对小球的轨迹进行预测.结合小球的预测轨迹，文[20]用LBMPC(linearing based MPC)的算法控制四旋翼快速飞向小球所要经过的点，并在该点快速镇定，以接住飞过来的小球.与之类似，文[21]采用滑模控制的方法完成接球任务.文[22]用多项式作为四旋翼飞行轨迹的模型，并求解出一条与小球的预测轨迹相交的多项式轨迹，以保证四旋翼与小球相遇以完成接球任务.可以看出由于小球轨迹的可预测性，接球任务可以看作静态任务，可用开环规划飞行轨迹的方法完成.由于动态目标物的运动是不可预测的，动态目标物跟踪时只能利用目标物当前的信息.为了实现对目标物的跟随飞行，可把更新的目标物的位置作为四旋翼的期望位置，由四旋翼的底层控制器对其进行跟踪，如，PID[23]和几何控制算法(geometric control)[24]等.与简单的动态目标物跟踪不同，自由飞行目标物捕获过程中，目标可能会采取逃逸策略，如加速绕障碍物飞行等.因此，为了防止目标物逃逸，四旋翼需要规划出一条时间最优轨迹，以实现在最短的时间内完成捕获任务.
自由飞行目标物捕获是动态任务，目标飞行所经过的环境也是难以预测的.这就要求四旋翼要根据目标物和环境中障碍物的状态反馈在线实时的规划出可执行的轨迹.该轨迹除了能够快速地追踪上目标物外，同时还要具备障碍物避碰的功能.因此，为了更好地完成自由飞行目标物的捕获任务，在追逐目标物的期间四旋翼的轨迹应该具备以下特征：
1) 闭环特性，即为了完成动态任务，在每个控制周期根据当前自身、目标物和环境反馈的信息在线实时生成可执行的轨迹.
2) 时间最优特性，即能保证四旋翼在最短的时间内追上自由飞行目标物.
3) 障碍物避碰特性，即能保证四旋翼在复杂环境中追逐追踪目标物时不与障碍物发生碰撞.
面向飞行目标物捕获任务，本文针对四旋翼追逐自由飞行目标过程的轨迹规划展开研究，根据其所需要的以上3种轨迹特性提出了一种新的轨迹规划方法.主要贡献为：
1) 提出了诱导时间最优MPC算法，即GTO-MPC(guidance time optimal model predictive control).该算法规划出的轨迹具有闭环特性和时间最优特性，能够保证四旋翼快速地追踪上自由飞行的目标物.
2) 提出了障碍的动态线性物约束表示方法.用动态线性约束来表示障碍物的非线性约束，并与GTO-MPC相结合，提高了障碍物约束下轨迹求解效率，使其能够在每个控制周期内求解出一条能够自主避碰的轨迹.
1. 问题描述
针对四旋翼位置轨迹规划的问题，本节首先利用动力学解耦的方法简化四旋翼的平动动力学约束.在此基础上把四旋翼追逐自由飞行目标物过程的位置轨迹规划描述为一个时间最优控制问题.
1.1 四旋翼动力学约束
四旋翼的运动为一个6自由度刚体运动，可通过牛顿—欧拉方程对其进行动力学建模.首先定义运动坐标系，如图 1所示，惯性坐标系表示为ΣI.四旋翼机体坐标系表示为ΣB.点O为坐标系ΣB的原点并与四旋翼的质心重合.四旋翼在惯性坐标系ΣI中的位置表示为p.从坐标系ΣB到坐标系ΣI中的旋转为R.四旋翼的动力学方程为[25]
(1) 
(2) 其中，v为四旋翼的速度；m为四旋翼的质量；g为重力加速度；e1、e2和e3为3维单位向量，即Ι3×3=[e1 e2 e3]为三维单位矩阵；ω为ΣB坐标系中四旋翼的旋转角速度；skew(·)为向量的反对称矩阵映射函数；I为四旋翼的转动惯量；Ft为四个螺旋桨产生的总升力；τ为四个螺旋桨在机体坐标轴xB、yB和zB三个方向产生的力矩向量；Ft和τ与四个螺旋桨的转速有以下关系：
(3) 其中，ωi(i=1，2，3，4)为各个螺旋桨的转速，cT和cτ分别为单个螺旋桨的升力系数和力矩系数，d为螺旋桨中心到机体坐标原点O的距离.
方程(1)和方程(2)分别为四旋翼的平动动力学和旋转动力学，可以看出二者通过旋转矩阵耦合在一起，动力学呈现出非线性的特性.在求解轨迹过程中如果直接把动力学约束直接考虑进去计算量大，效率低.四旋翼的特殊结构使其可通过姿态角的调整获取相应的惯性坐标系下的线加速度.利用这一特性，文[15]提出了一种动力学解耦的方法，把四旋翼的平动动力学表示为一个加速度和加加速度受约束下的积分系统，以提高四旋翼无人机位置轨迹求解的效率.动力学解耦之后的平动动力学方程为
(4) 其中，a为四旋翼的线加速度，线加速度可由姿态环的控制器进行跟踪[25].由于四旋翼驱动能力有限，解耦之后还需要对其加速度和加加速度进行限幅，即：
(5) 
(6) 其中，amax和jmax分别为四旋翼的最大加速度和最大加加速度的约束，二者受限于四旋翼的最大升力[15].
1.2 自由飞行目标物追踪最优轨迹问题构造
在自由飞行目标物捕获任务中，假设目标物的飞行运动是不可预测的，但是其位置和速度为可测量的，并分别表示为ptarg和vtarg.如图 2所示，为了让四旋翼尽快的追上目标物，需要一条可执行的轨迹p(t)，该轨迹能够使四旋翼在t0时刻从四旋翼的当前位置p0开始飞行，在tf到达目标物的位置ptarg，并能保证飞行时间最短，同时在此过程中避免与障碍物发生碰撞.在下一个控制周期测量更新后在重新规划.障碍物用半径为robs的球体表示，障碍物的位置为球体的中心位置，表示为pobs.为了保证四旋翼的安全，在整个追逐过程中四旋翼无人机与障碍物上的任意一点的距离要大于安全距离ds.因此，为了得到具备引言所提到的3个特性的追逐轨迹，可构建时间最优控制问题：
(7) 其中，||·||表示向量的欧氏范数.如果时间最优控制问题(7)能够在每个控制周期内实时的在线求解，那么就可以得到一条可执行的轨迹，使得四旋翼在最短的时间内能安全地追上目标物，完成捕获任务.
2. 问题求解
在上一节中，构造了时间最优控制问题(7)，以得到四旋翼追逐自由飞行目标物的轨迹.由于时间最优问题把时间作为优化指标，为非凸优化问题，往往难以求解.再加上障碍物约束为非线性约束，使得在每个控制周期内在线实时求解最优控制问题(7)更加困难.为了在线实时求解最优控制问题(7)，本节将用诱导时间最优轨迹指标函数和障碍的动态线性物约束把问题(7)转化为线性二次型规划(LQP)问题.并结合MPC的滚动时域的构架，在每个控制周期内对其进行优化求解.
2.1 诱导时间最优轨迹指标函数
首先放宽约束条件，只考虑最大加速度约束，最优控制问题(7)将会转化二重积分系统在输入约束下的时间最优控制问题.即：
(8) 对于最优控制问题(8)，可通过庞特里亚金极大值原理求解出最优控制律和时间最优轨迹[27].最优控制律为bang-bang控制律，即：
(9) 其中，sgn(·)为符号函数. h为切换函数，与四旋翼无人机和目标的位置和速度相关，其具体的形式为
(10) 对于最优控制问题(8)在每一个控制周期内可得到一条时间最优轨迹p*(t)，即：
(11) 由式(9)~式(11)可以看出，每一个控制周期内时间最优轨迹p*(t)为当前时刻四旋翼和目标物状态的函数，即为闭环的轨迹.按时间最优轨迹p*(t)四旋翼追上目标物所用的时间最小，为
(12) p*(t)是最优控制问题(8)的时间最优轨迹，为最优控制问题(7)在放宽约束条件下的解.所以，对于四旋翼来说轨迹p*(t)存在不能被完全执行的可能. p*(t)可以作为最优控制问题(7)的极限最优解，即最优控制问题(7)在考虑更多约束的情况下，求得的轨迹在时间性能上都不如p*(t).
对于轨迹性能，如果两个轨迹在形状的越近似，则其在性能上也越为相似.如图 3所示，轨迹traj 1在形状上与极限最优轨迹(limit optimal traj)较traj 2更为近似，其在性能上较traj 2更优.因此，在最优控制问题(7)的约束条件下，如果一条轨迹在形状上与p*(t)最为近似，则其在性能上将优于其他的轨迹，可把其作为最优控制问题(7)时间最最优轨迹的解.
为了求解在形状上与p*(t)最为近似的轨迹，可定义二次型指标函数J，用来评价任意轨迹p(t)与p*(t)的相似度，指标函数J定义为
(13) 二次型指标函数J称之为诱导时间最优轨迹指标函数，即通过极限时间最优轨迹p*(t)的引导来求解时间最优轨迹.
对于最优控制问题(7)如果用二次型指标函数J代替时间最优指标函数，问题(7)将由从时间最小优化问题转化为二次型优化问题，可以通过凸优化的方法进行求解.这种通过指标函数J求解得到轨迹，本文称之为诱导时间最优轨迹，即，该轨迹不是理论上的最优轨迹，而是在二次型指标函数J的作用下求解出的一条与宽松约束条件下的理论最优轨迹最为近似的一条轨迹.
2.2 诱导时间最优MPC
把指标函数J应用于问题(7)，可把其转化为有限时域内的二次型优化控制问题，因此可以用MPC的滚动时域的方法进行求解.在不考虑障碍物约束的情况下，对其进行离散化处理，采用时间为Δt，可得：
(14) 最大速度约束：-vmax≤v[k]≤vmax，对于任意k≥0
最大加速度约束：-amax≤a[k]≤amax，对于任意k≥0
最大加加速度约束：-jmaxΔt≤Δa[k]≤jmaxΔt，对于任意k≥0
初始状态约束：p[0]=p(t0)=p0，v[0]=v(t0)=v0其中，
可以看出，把时间最优化问题转化为变预测步长的线性MPC问题，即问题(14)，其预测时间步长由四旋翼和目标当前的位置和速度决定.线性MPC实际上为线性二次型规划(LQP)问题，求解计算量小，可以在每个控制周期内对其在线实时求解.对于MPC问题，当预测步长Np过小时会导致算法不收敛或求解过程不可执行(Infeasible).因此，对Np的取值范围进行限制，即：
(15) 其中，Nmin和Nmax分别为Np限制的最小值和最大值.
像问题(14)这种利用诱导时间最优轨迹指标函数，以达到时间最优控制效果的MPC算法，称之为诱导时间最优MPC，即，GTO-MPC(guidance time optimal model predictive control).在点到点(point-to-point)的控制任务中，MPC往往把目标点作为参考值，以阶跃响应的过程到达目标点，与之相比GTO-MPC到达目标点过程在时间上的性能更好.
2.3 障碍物的动态线性约束表示
问题(7)中的障碍物的约束为非线性约束，为了提高求解效率，可用动态线性约束替代原来的非线性约束.如图 4所示，首先定义障碍物的危险区域，该区域由半径为robs+ds且与障碍物同心的球状区域表示.可以看出，如果四旋翼在危险区域外面飞行，则问题(7)中的障碍物的避碰约束必会满足.危险区域球面的切平面α可以把空间分为两个部分，一部分包含危险区域，另一部分不包含危险区域.如果以切平面α作为障碍物的边界，把安全飞行区域限制在不包含危险区域一侧，这样就能用线性约束实现障碍物的避碰.为了最大化线性约束所表示的安全飞行区域，在每个控制周期都根据四旋翼和障碍物的位置更新切平面α，使得其始终与四旋翼和障碍物的连接线垂直.这样用一个动态的线性约束来代替障碍物的非线性约束可以大大提高轨迹的求解的效率，以实现在每个控制周期内在线实时求解.
首先，根据四旋翼的当前位置p0和障碍物的位置pobs可得一个由障碍物指向四旋翼的单位方向向量n，即：
(16) 由方向向量n和危险区域的半径可得所需切平面α与危险区域边缘的切点on，即：
(17) 设ptang为切平面上α的任意一点，由于切平面α方向向量n垂直，则ptang满足：
(18) 切平面α可以把空间分为两个部分，一侧包含危险区域，另一侧不包含危险区域.障碍物的位置在包含危险物的一侧，由式(16)可得障碍物的位置pobs满足：
(19) 根据平面方程的性质，四旋翼如果要在平面另一侧空间飞行，则其经过位置点需要满足：
(20) 式(20)为障碍物的线性约束条件，在每个控制周期内可以由四旋翼和障碍物的位置反馈信息根据式(12)~式(20)进行更新，以动态线性约束表示障碍物的约束.将其离散化后加入GTO-MPC问题(14)，可在每个控制周期内求解出不与障碍物碰撞的轨迹.动态线性约束是球形危险区域的一系列的切平面，因此，障碍物的动态线性约束代替其非线性约束的条件是存在一个球形区域能够包络已感知的障碍物，这样就能定义球形危险区域以计算相应的线性约束.
3. 仿真
为了验证本所提出轨迹规划算法的时间最优性能和障碍物避碰性能，本文在Matlab环境下进行了仿真验证.
仿真中四旋翼的动力学模型为式(1)~式(3)，用于求解动力学微分方程的算法为定步长ode 45.四旋翼的物理参数如表 1所示.
表 1 仿真实验中四旋翼的物理参数Tab. 1. The parameters of quadrotor in the simulations序号 物理量 参数值 1 质量m 0.03 kg 2 xB方向上转动惯量Ixx 1.43×10-5 kg·m2 3 yB方向上转动惯量Iyy 1.43×10-5 kg·m2 4 zB方向上转动惯量Izz 2.89×10-5 kg·m2 5 螺旋桨升力系数cT 6.11×10-8 6 螺旋桨力矩系数cτ 1.5×10-9 7 螺旋桨中心到质心的距离d 0.046 m 3.1 时间最优性能验证
对于一个位置和速度可测量的动态目标物，可以把目标物的位置作为期望值，采用位置和速度反馈控制的方法进行跟踪.但是这种方法不能保证跟踪过程的时间最优特性，也不能实现物的避碰.为了验证本文所提出GTO-MPC算法所规划的轨迹在自由飞行目标物追踪过程中的时间最优特性，在仿真中对比了GTO-MPC算法和位置和速度反馈控制的方法在没有障碍物的情况下，分别追踪直线飞行运动和曲线飞行运动的目标物追踪的效果.用于对比的算法为几何控制算法(geometric control)[26]和MPC.由于四旋翼的螺旋桨只能产生单一方向的升力，其方向始终与机体坐标系Z轴方向相反，位置控制时，需调整其姿态角使得升力在某个方向产生分力，以实现该方向的位置控制.因此，位置控制时需计算出期望的姿态，再由姿态环跟踪期望的姿态，以实现位置控制.四旋翼的几何控制具有内外环的结构，其中内环为姿态环，外环为位置环.
1) 位置环
位置环中四旋翼期望加速度的反馈控制律为
(21) 其中，ad为期望加速度，ep和ev分别为位置和速度误差，kp和kv分别为位置和速度反馈增益矩阵，pd为位置期望.
2) 期望姿态解算
根据期望的加速度可求解出所需的升力和期望姿态角所确定的旋转矩阵，具体为
(22) 
(23) 
(24) 其中，bx、by和bz为互正交的3个单位向量；ψd为四旋翼航向期望值.
2) 姿态环
姿态跟踪的反馈的控制律为
(25) 其中，eR和eω分别为姿态误差和姿态角速度误差；eR定义为
在追踪直线飞行的目标物时，目标物的起始位置为[0 0 0]T，四旋翼的起始位置为[0.3 -0.4 0.1]T.在追踪曲线飞行的目标物时，目标物的起始位置为[5 0 0]T，四旋翼的起始位置为[6 -0.2 0.2]T.直线飞行目标物追踪对比仿真结果如图 5、图 6所示，可以看出采用几何控制算法(Geometric control)和MPC也能追上目标物，但是所用时间较长，大约分别在t=1.8 s和t=1.1 s时可以使四旋翼与目标物的相对距离收敛到零.而本文提出的GTO-MPC算法在同样的条件下，可在大约t=0.9 s时使四旋翼与目标物的相对距离收敛到零.曲线飞行目标物追踪对比仿真结果如图 7和图 8所示，与直线飞行目标物追踪结果类似.二者都能追上目标物，GTO-MPC算法能在更短的时间内追上目标物，大约t=1.3 s，相同条件下的几何控制算法(geometric control)和MPC大约分别在t=1.8 s和t=1.5 s才能跟踪上目标物.
![]()
图 5 直线飞行的目标物追踪时四旋翼与目标物之间的相对距离Fig. 5. Relative distance between the quadrotor and the target when it is pursuing the target flying in the straight line![]()
图 6 直线飞行的目标物追踪时四旋翼3轴位置轨迹Fig. 6. Position trajectory in the three axis of the quadrotor in pursuing the target flying in the straight line![]()
图 7 曲线飞行的目标物追踪时四旋翼与目标物之间的相对距离Fig. 7. Relative distance between the quadrotor and the target when it is pursuing the target flying in the circle line![]()
图 8 曲线飞行的目标物追踪时四旋翼三轴位置轨迹Fig. 8. Position trajectory in the three axis of the quadrotor in pursuing the target flying in the circle line对比结果可以看出本文提出GTO-MPC在自由飞行目标物追逐过程中，用时最短，表现出时间最优的特性.
3.2 障碍物避碰特性验证
为了验证本文所提出的障碍动态物线性约束表示方法能有效地替代障碍的非线性约束，以及与GTO-MPC算法结合能实现障碍物避碰.仿真中在追踪直线飞行目标物和曲线飞行目标物的过程中引入障碍物.在追踪直线飞行的目标物过程中，碍物的半径为0.1 m(危险区域半径)，位置为[0.2 -0.2 0.05]T.在追踪曲线飞行的目标物过程中，碍物的半径为0.3 m(危险区域半径)，位置为[5.45 0.45 0]T.仿真结果如图 9、图 10所示，可以看出在不加障碍物约束的情况下，GTO-MPC算法原本规划的路径与障碍物发生碰撞.加上动态线性约束后，GTO-MPC算法规划的路径能有效地躲避障碍物，使四旋翼能安全地追踪上直线飞行或曲线飞行的目标.仿真还表明，该算法的每次求解能够在20 ms内完成，即可以达到50 Hz，满足实际系统对算法计算效率的需求.
![]()
图 9 四旋翼追踪直线飞行目标物并躲避障碍物的路径在x-o-y平面内的投影Fig. 9. The path in the x-o-y plan of the quadrotor that catch the target moving in the straight linear while avoding the obstacle![]()
图 10 四旋翼追踪曲线飞行目标物并躲避障碍物的路径在x-o-y平面内的投影Fig. 10. The path in the x-o-y plan of the quadrotor that catch the target moving in the circle linear while avoding the obstacle4. 结论
针对自由飞行目标物捕获任务，本文提出了GTO-MPC算法，用于规划四旋翼追踪自由飞行目标物的轨迹. GTO-MPC利用诱导时间最优轨迹指标函数，把时间最优化问题转化为线性二次型规划问题，提高了计算效率.利用MPC的滚动优化策略可以实现闭环的轨迹规划，使其能够适用于动态任务.结合本文提出的动态线性障碍物约束还能实现目标物追逐过程中的障碍物避碰.仿真结果表明，GTO-MPC可以保证四旋翼安全快速地追踪上自由飞行的目标物，其高效的计算效率也能保证该算法在实际任务中的应用.
-
![]()
图 3 不同形状轨迹性能对比图
Figure 3. Performance comparison of trajectories with different shapes
![]()
图 5 直线飞行的目标物追踪时四旋翼与目标物之间的相对距离
Figure 5. Relative distance between the quadrotor and the target when it is pursuing the target flying in the straight line
![]()
图 6 直线飞行的目标物追踪时四旋翼3轴位置轨迹
Figure 6. Position trajectory in the three axis of the quadrotor in pursuing the target flying in the straight line
![]()
图 7 曲线飞行的目标物追踪时四旋翼与目标物之间的相对距离
Figure 7. Relative distance between the quadrotor and the target when it is pursuing the target flying in the circle line
![]()
图 8 曲线飞行的目标物追踪时四旋翼三轴位置轨迹
Figure 8. Position trajectory in the three axis of the quadrotor in pursuing the target flying in the circle line
![]()
图 9 四旋翼追踪直线飞行目标物并躲避障碍物的路径在x-o-y平面内的投影
Figure 9. The path in the x-o-y plan of the quadrotor that catch the target moving in the straight linear while avoding the obstacle
![]()
图 10 四旋翼追踪曲线飞行目标物并躲避障碍物的路径在x-o-y平面内的投影
Figure 10. The path in the x-o-y plan of the quadrotor that catch the target moving in the circle linear while avoding the obstacle
表 1 仿真实验中四旋翼的物理参数
Table 1 The parameters of quadrotor in the simulations
序号 物理量 参数值 1 质量m 0.03 kg 2 xB方向上转动惯量Ixx 1.43×10-5 kg·m2 3 yB方向上转动惯量Iyy 1.43×10-5 kg·m2 4 zB方向上转动惯量Izz 2.89×10-5 kg·m2 5 螺旋桨升力系数cT 6.11×10-8 6 螺旋桨力矩系数cτ 1.5×10-9 7 螺旋桨中心到质心的距离d 0.046 m 
下载: 导出CSV
-
[1] Zhang C, Kovacs J M. The application of small unmanned aerial systems for precision agriculture:A review[J]. Precision Agriculture, 2012, 13(6):693-712. doi: 10.1007/s11119-012-9274-5
[2] Tomic T, Schmid K, Lutz P, et al. Toward a fully autonomous UAV:Research platform for indoor and outdoor urban search and rescue[J]. IEEE Robotics&Automation Magazine, 2012, 19(3):46-56. https://www.researchgate.net/publication/234091563_Toward_a_Fully_Autonomous_UAV_Research_Platform_for_Indoor_and_Outdoor_Urban_Search_and_Rescue
[3] Niethammer U, James M R, Rothmund S et al. UAV-based remote sensing of the Super-Sauze landslide:Evaluation and results[J]. Engineering Geology, 2012, 128:2-11. doi: 10.1016/j.enggeo.2011.03.012
[4] Mellinger D, Michael N, Kumar V. Trajectory generation and control for precise aggressive maneuvers with quadrotors[J]. The International Journal of Robotics Research, 2012, 31(5):664-674. doi: 10.1177/0278364911434236
[5] Mellinger D, Kumar V. Minimum snap trajectory generation and control for quadrotors[C]//2011 IEEE International Conference on Robotics and Automation. Piscataway, NJ, USA: IEEE, 2011: 2520-2525. https://www.researchgate.net/publication/224252786_Minimum_snap_trajectory_generation_and_control_for_quadrotors
[6] Richter C, Bry A, Roy N. Polynomial trajectory planning for aggressive quadrotor flight in dense indoor environments[G]//Inaba M, Corke P. Robotics Research Cham: Springer International Publishing, 2016, 114: 649-666.
[7] Sahingoz O K. Generation of Bezier curve-based flyable trajectories for multi-UAV systems with parallel genetic algorithm[J]. Journal of Intelligent&Robotic Systems, 2014, 74(1):499-511. http://www.wanfangdata.com.cn/details/detail.do?_type=perio&id=5e916dcf0bc97367801b7f8b891b0928
[8] Chamseddine A, Li T, Zhang Y, et al. Flatness-based trajectory planning for a quadrotor unmanned aerial vehicle test-bed considering actuator and system constraints[C]//2012 American Control Conference. Piscataway, NJ, USA: IEEE, 2012: 920-925. https://www.researchgate.net/publication/261192972_Flatness-based_trajectory_planning_for_a_quadrotor_Unmanned_Aerial_Vehicle_test-bed_considering_actuator_and_system_constraints
[9] Ghommam J, Saad M. Autonomous landing of a quadrotor on a moving platform[J]. IEEE Transactions on Aerospace&Electronics Systems, 2017, 53(3):1504-1519. https://www.researchgate.net/publication/314098220_Autonomous_Landing_of_a_Quadrotor_on_a_Moving_Platform
[10] Verscheure D, Demeulenaere B, Swevers J, et al.. Time-optimal path tracking for robots:A convex optimization approach[J]. IEEE Transactions on Automatic Control, 2009, 54(10):2318-2327. doi: 10.1109/TAC.2009.2028959
[11] 韩忠华, 冯兴浩, 吕哲, 等.一种改进的无人机路径规划环境建模方法[J].信息与控制, 2018, 47(3):117-124. http://ic.sia.cn/CN/abstract/abstract12653.shtml Han Z H, Feng X H, Lv Z, et al. An improved UAV path planning environment modeling method[J]. Information and Control, 2018, 47(3):117-124. http://ic.sia.cn/CN/abstract/abstract12653.shtml
[12] Karaman S, Frazzoli E. Sampling-based algorithms for optimal motion planning[J]. The International Journal of Robotics Research, 2011, 30(7):846-894. doi: 10.1177/0278364911406761
[13] Dolgov D, Thrun S, Montemerlo M, et al. Path planning for autonomous vehicles in unknown semi-structured environments[J]. The International Journal of Robotics Research, 2010, 29(5):485-501. doi: 10.1177/0278364909359210
[14] Loock W V, Pipeleers G, Swevers J. Time-optimal quadrotor flight[C]//2013 European Control Conference (ECC). Piscataway, NJ, USA: IEEE, 2013: 1788-1792.
[15] Hehn M, Dandrea R. Real-time trajectory generation for quadrocopters[J]. IEEE Transactions on Robotics, 2015, 31(4):877-892. doi: 10.1109/TRO.2015.2432611
[16] Hehn M, Ritz R, D'Andrea R. Performance benchmarking of quadrotor systems using time-optimal control[J]. Autonomous Robots, 2012, 33(1/2):69-88. doi: 10.1007/s10514-012-9282-3
[17] Mueller M W, D'Andrea R. A model predictive controller for quadrocopter state interception[C]//2013 European Control Conference (ECC). Piscataway, NJ, USA: IEEE, 2013: 1383-1389.
[18] Hehn M, D'Andrea R. Quadrocopter trajectory generation and control[J]. IFAC Proceedings Volumes, 2011, 44(1):1485-1491. doi: 10.3182/20110828-6-IT-1002.03178
[19] 张广玉, 何玉庆, 代波, 等.面向抓取作业的飞行机械臂系统及其控制[J].机器人, 2019, 41(1):19-29. http://www.wanfangdata.com.cn/details/detail.do?_type=perio&id=jqr201901003 Zhang G Y, He Y Q, Dai B, et al. Towards grasping task:System and control of an aerial manipulator[J]. Robot, 2019, 41(1):19-29. http://www.wanfangdata.com.cn/details/detail.do?_type=perio&id=jqr201901003
[20] Bouffard P, Aswani A, Tomlin C. Learning-based model predictive control on a quadrotor: Onboard implementation and experimental results[C]//2012 IEEE International Conference on Robotics and Automation. Piscataway, NJ, USA: IEEE, 2012: 279-284.
[21] Ibarra-Jimenez E, Castillo P. Aerial autonomous catching ball using a nested second order sliding mode control[J]. IFAC-PapersOnLine, 2017, 50(1):11415-11420. doi: 10.1016/j.ifacol.2017.08.1805
[22] Mueller M W, Hehn M, Dandrea R. A computationally efficient motion primitive for quadrocopter trajectory generation[J]. IEEE Transactions on Robotics, 2017, 31(6):1294-1310. https://www.researchgate.net/publication/283710144_A_Computationally_Efficient_Motion_Primitive_for_Quadrocopter_Trajectory_Generation
[23] Yibei L I, Yao Y, Fenghua H E, et al. Autonomous control and target tracking algorithm design for a quadrotor[C]//2017 36th Chinese Control Conference. Piscataway, NJ, USA: IEEE, 2017: 6749-6754.
[24] Thomas J, Welde J, Loianno G, et al. Autonomous flight for detection, localization, and tracking of moving targets with a small quadrotor[J]. IEEE Robotics and Automation Letters, 2017, 2(3):1762-1769. doi: 10.1109/LRA.2017.2702198
[25] Mahony R, Kumar V, Corke P. Multirotor aerial vehicles:Modeling, estimation, and control of quadrotor[J]. IEEE Robotics & Automation Magazine, 2012, 19(3):20-32. https://www.researchgate.net/publication/255786198_Multirotor_Aerial_Vehicles_Modeling_Estimation_and_Control_of_Quadrotor
[26] Lee T Y, Leoky M, McClamroch N H. Geometric tracking control of a quadrotor UAV on SE(3)[C]//49th IEEE Conference on Decision and Control(CDC). Piscataway, NJ, USA: IEEE, 2010: 5420-5425.
[27] Naidu D S. Optimal Control Systems[M]. Boca Raton, FL, USA:CRC Press, 2002.
-
期刊类型引用(3)
1. 赵创新，遆好建，李照宏，冯山，杨述星，唐鹏. 多无人机协同编队控制及试飞验证. 航空学报. 2024(17): 326-337 . 
百度学术
2. 常振强，金龙，李帅，张继良. 微型扑翼飞行器的自适应位置跟踪控制器. 信息与控制. 2021(01): 88-94 . 本站查看
3. 张雪涛，方勇纯，张雪波，蒋静琦，华和安. 基于误差状态卡尔曼滤波估计的旋翼无人机输入饱和控制. 机器人. 2020(04): 394-405 . 百度学术
其他类型引用(4)
计量
- 文章访问数: 257
- HTML全文浏览量: 14
- PDF下载量: 132
- 被引次数: 7
