基于动态运动基元的微小型四旋翼无人机路径规划①

陈鹏震 陈 洋

(武汉科技大学信息科学与工程学院 武汉 430081)



基于动态运动基元的微小型四旋翼无人机路径规划①

陈鹏震②陈 洋③

(武汉科技大学信息科学与工程学院 武汉 430081)

针对微小型四旋翼无人机的路径规划问题，引入运动学习框架，提出了一种基于动态运动基元的路径规划方法。该方法通过对给定运动样本的学习提取出运动基元，并将学习结果推广到新的飞行目标，从而泛化出相应的运动轨迹。有障碍物的情况下，在已有学习基础上通过设计耦合因子规划出避障路径，然后将规划的轨迹点集提供给微小型四旋翼无人机完成路径跟踪飞行任务。该路径规划方法的可行性通过微小型四旋翼无人机不同目标点的飞行任务仿真得到了验证。仿真实验还验证了该方法在三维空间进行有效避障的性能。

微小型四旋翼， 无人机(UAV)， 动态运动基元(DMPs)， 路径规划， 避障

0 引 言

近年来，微小型四旋翼无人机(unmanned aerial vehicle, UAV)以其简单的机械结构和独特的飞行方式引起了无人机研究领域的广泛关注[1]。四旋翼无人机是一种非共轴多旋翼式飞行器，它通过调节对称分布的四个旋翼的转速，实现对飞行姿态的控制。与常规直升机相比，其四个旋翼提供的升力比单旋翼更均匀，因而飞行更加平稳且机动性更强。微小型四旋翼无人机体积小、隐蔽性好、飞行平稳，特别适用于完成近地面环境的航拍、侦查、监视等任务，在民用和军事领域均具有广泛的应用[2]。

目前国内外很多研究机构和高校对于微小型四旋翼无人机的控制与导航进行了相关研究。在无人机的路径规划上，传统的方法有蚁群算法[3，4]、粒子群算法[5，6]、A*算法[7]等。文献[8]建立了室内微小型无人机路径规划模型，将三维规划空间离散为均匀的栅格模型，分别基于改进的蚁群算法和无记忆A*展开了室内微小型无人机全局路径规划的研究；文献[9]提出一种基于混合变异粒子群优化算法的三维路径规划方法。另外文献[10]提出一种基于视觉的三维导航技术，将漏斗通道理论应用于四旋翼无人机的路径跟踪控制。然而以上路径规划算法均在环境建模的基础上添加诸多的约束条件，通过各种算法搜索飞行路径。搜索到路径的优越性依赖于环境模型的精确程度及算法的实时性和有效性，这些搜索的路径方式限制了飞行运动形式，因而希望从一种运动学习的角度出发，通过学习已有的运动形式，不但能够规划出一条所求轨迹，并能够融入运动特性。于是本文提出了一种基于动态运动基元(dynamic movement primitives, DMPs)[11，12]的学习型算法，它从运动学习的角度将小型四旋翼无人机的一次具体飞行看作学习样本，提取其运动基元。对于新的飞行任务，能够利用学习获得的运动基元泛化出相应的飞行路径。在有障碍物的情况下，也能够通过动态运动基元规划出一条可行的避障路径。这样一种运动学习机制提高了微小型四旋翼无人机的自主导航能力。本研究在MATLAB仿真环境下分别设计了微小型四旋翼无人机姿态和位置的PID控制器，通过定点悬停和轨迹跟踪任务验证了本文提出的路径规划方法的可行性。

1 基于动态运动基元的轨迹产生方法

1.1 动态运动基元理论

动态运动基元可广泛应用于解决机器学习问题[13，14]，它能够产生离散的运动从而迅速适应动态变化的环境。动态运动基元理论的核心是将一系列带有吸引点的非线性微分方程来描述运动。对于单一的自由度位移y，引入带有恒定系数线性微分方程并称之为动力学系统，此系统作为对运动样本学习的基础[15]：

(1)

(2)

式中τ表示时间常数，αv和βv为正常数，g为吸引点，也为常数。若选取合适的值如βv=αv/4且τ>0，则系统临界阻尼，y能够避免周期性振动而最快地收敛于g。此类二阶线性系统描述了具有唯一吸引点(v,y)=(0,g)的一种特性。此系统中y收敛于g的过程可以看作是离散的点到点的运动过程。然而上述系统只能得到一种特定的临界阻尼运动，为获得更广泛的具有不同运动形式的动力学系统，在式(1)右边增加强制项f。本文将f设计为径向基核函数的线性加权和，从而得到更一般的点到点运动拟合形式：

(3)

ψi(s)=exp(-hi(s-ci)2)

(4)

(5)

(6)

(7)

式(6)和(7)能够近似拟合不同运动的形式。由式(3)和(5)可知，当s由1趋近于0时，非线性强制项f(s)随时间衰减，最终式(6)和(7)收敛于(v,y)=(0,g)。式(6)将一般的点到点运动描述为一种简易的二阶动态系统，对于非线性强制项f(s)随时间推移取值也随之变化，从而使系统表现出不同的运动形式。

1.2 基于动态运动基元的运动学习和泛化

(8)

由式(8)可得问题转化为函数逼近，即式(6)中如何找到合适的权重wi，使得f尽可能接近fdemo。为估计动态运动基元的参数wi，将式(3)写成线性方程的形式如下：

Tw=f≈fdemo

(9)

对应的T和w用下式表示：

(10)

(11)

在最小二乘意义上，通过线性方程式(9)可以计算出wi。得到的权重wi能够拟合出运动的位移、速度和加速度量离散序列，从而达到学习的过程。若改变运动的起点y0和终点g，也能够利用已经学习的结果规划出一条离散的运动轨迹点，从而达到泛化的过程，并且此运动轨迹具有原学习轨迹的运动特性。具体运动学习和泛化流程如图1所示。

图1 运动学习及泛化过程

1.3 推广到多自由度的运动学习及应用于路径规划

为了将多自由度运动学习应用于图1所示的系统，将多自由度的运动共享一个正则系统，对应每个自由度有其相应的动力学系统。正则系统是共享的，而每个自由度都有自己的非线性函数f和动力学系统。同一个正则系统保证了每个自由度运动的时间耦合性，即在时间上的同步和一致，而对应的动力学系统保证了每个自由度有其相应的运动特性，描述如图2所示。

图2 多自由度运动学习原理图(以三个自由度为例)

由图1和图2可知，基于对运动的学习可以规划出一条所需要的运动轨迹，以实现运动目标。鉴于上述的动态运动基元的运动学习策略特点，将此方法运用到微小型四旋翼无人机的路径规划上。首先将无人机的空间飞行分解为三个自由度，分别建立各自由度的非线性强制项f以及相应的动力学系统，如图2所示。先让系统学习一条所需的运动轨迹，这个运动轨迹可以是人为给定，也可以是通过遥控器操纵无人机运动后记录下来的运动轨迹。再通过图1所示的流程，学习和泛化出四旋翼无人机实际所需要的运动轨迹点，本研究对无人机的控制是基于位置和姿态的双闭环PID控制策略，所以四旋翼无人机在此控制策略和给定的路径轨迹点下能够完成目标飞行任务。

1.4 三维路径避障

(12)

(13)

(14)

2 四旋翼飞行器控制器设计

为建立微小型四旋翼无人机动力学模型，先定义两个坐标系：导航坐标系E(Xe, Ye, Ze)和机体坐标系B(Xb, Yb, Zb)，如图3所示。定义无人机的欧拉角Ω=[θ,γ,φ]T，其中θ为机体俯仰角，γ为机体的横滚角，φ为机体的偏航角。本文中假定机体的转动顺序为先绕Z轴转过的角度为航向角φ，再绕Y轴转过的角度为横滚角γ，最后绕X轴转过的角度为俯仰角θ。则在此顺序下的导航坐标系到机体坐标系的旋转矩阵R:E→B为

(15)

m为无人机的质量，以b和d分别表示无人机的升力系数和阻力系数，l表示电机转轴到机体中心的距离。Jx、Jy和Jz分别表示无人机分别绕机体坐标系下xyz轴的转动惯量。

图3 四旋翼无人机动力学示意图

微小型四旋翼无人机在无风及慢速飞行的情况下，所受到的空气阻力可以忽略不计，经过一系列的推导和简化可得到无人机在导航系下如下式所示的动力学模型[18，19]：

(16)

其中无人机四个独立控制通道的控制输入量为

(17)

3.实现提高政府形象和就业机会等非经济性收益。采用PPP模式进行城镇综合开发，在经济效益方面是显而易见的，但除此之外，还有不少非经济效益。比如可以有效提高政府形象。城市的发展是体现政府形象最直接的手段，政府要有效地发挥手中有限资本的杠杆作用，撬动更大规模的社会资本，加快城市发展，提高政府形象。再如，可以提供更多的就业机会。高就业率是政府追求的目标之一，城镇综合开发项目多、规模大，有利于提供更多的就业机会。

根据以上微小型四旋翼无人机动力学模型可以看出无人机的运动可以为质心的移动和机体的转动，质心的移动依赖于机体的侧倾提供的侧向加速度，所以控制无人机的关键在于姿态控制。采用如图4的控制结构[19]，将姿态控制作为内环，位置控制为外环，姿态环嵌套在位置环中。图4中Pr、φr、Ωr分别为位置、偏航角和欧拉角的参考值。由此建立了微小型四旋翼无人机的动力学模型并设计了双闭环PID控制器，在此基础上进行路径规划仿真实验。

图4 控制结构示意图

3 实验与分析

采用Matlab的Simulink模块搭建上述基于动态基元的路径发生器和四旋翼动力学模型及其控制器，通过仿真结果验证此方法的可行性。本次实验所用到的微小型四旋翼无人机如图5所示，机身最大长度(翼尖到翼尖)106cm，旋翼直径30cm；飞行控制系统为APM飞控板，内置陀螺仪、加速度计和气压计等传感器。通过参数辨识可以得到飞行器的参数如表1所列，将表1中参数带入四旋翼动力学模型进行后续的仿真实验。

图5 四旋翼无人机实物图

变量数值单位b2.2893×10-5N·s2d1.1897×10-6N·ms2m2.467kgl0.3875mJx0.05887kg·m2Jy0.05887kg·m2Jz0.13151kg·m2

3.1 定点悬停

图6 四旋翼无人机飞行过程位置曲线

图7 四旋翼无人机飞行过程姿态曲线

依图6和图7可以得出四旋翼无人机在4s左右到达设定目标点并能够稳定悬停，因此设计出的双闭环PID控制器控制无人机稳定飞行是可行的。

图8 四旋翼无人机路径规划仿真界面

设样本路径为从(0, 0, 0)到目标点(10, 10, 10)，如图9和图10所示，由此次运动样本得到三个轴自由度的运动序列。

图9 运动样本三维曲线

图10 三自由度分解运动曲线

基于上述图10得到的已知运动序列进行学习，通过式(9)计算回归出权重wi。由上述运动模板回归出三个分自由度的权值序列如表2所列。本次仿真设计基核函数个数N=10。

表2 回归出权重值

图11 四旋翼无人机不同目标点飞行三维路径

选取其中目标点为(8, 8, 12)的飞行轨迹进行分析，得到无人机在xyz轴的三个自由度的运动曲线及姿态变化曲线，如图12和图13。由图12可知，动态运动基元算法计算出一条轨迹点后，四旋翼无人机能够按照规划的轨迹点稳定的飞向目标点，由图13看出12s左右无人机到达目标点并维持悬停状态。图14为规划路径与无人机实际飞行路径之间的跟踪误差曲线，由此曲线得出跟踪误差小于0.25m。

图12 四旋翼无人机飞行过程位置曲线

图13 四旋翼无人机飞行过程姿态曲线

图14 四旋翼飞行轨迹与规划路径跟踪误差曲线

通过比较图11和图9可得，DMP算法能够对现有的运动进行学习，在学习基础上能够重复以往的运动并能够在运动目标点变化的情况下泛化出一条可行路径。

3.3 避障实验

设障碍物是中心(5, 5, 5)半径为1.5的球体，四旋翼无人机运动的起点为坐标(0, 0, 0)，终点坐标(10, 10, 10)。结合3.2节运动学习后泛化的结果及1.4节的三维避障理论，根据式(13)计算三个自由度的耦合因子，然后代入式(12)得到新的避障轨迹序列。本实验中式(13)比例常数设定为k=10000，β=20/π。四旋翼无人机避障路径如图15，将四旋翼无人机避障飞行曲线正投影到XOZ平面得到如图16所示的避让路线，三自由度耦合因子实时计算结果如图17所示，位置曲线及姿态曲线如图18和图19。分析图17在5s前由于障碍物的阻挡，耦合因子起作用改变飞行轨迹，在5s左右由于无人机基本避过障碍物耦合因子子迅速衰减到0。由图15～图19可知，无人机能够由规划出的轨迹完成避障并在12s左右到达目标点并稳定悬停。

图15 四旋翼无人机避障飞行曲线

图16 四旋翼无人机避障飞行正投影到XOZ平面曲线

图17 三自由度耦合因子变化曲线

图18 四旋翼无人机避障飞行位置曲线

图19 四旋翼无人机避障飞行姿态曲线

4 结 论

本文提出一种基于动态运动基元的微小型四旋翼无人机路径规划方法，摆脱传统的三维路径规划对环境建模与搜索算法性能的依赖。通过对一次运动样本的学习后对新的运动任务进行泛化产生所需要的运动轨迹，在已建立的微小型四旋翼无人机模型及双闭环PID控制器的基础上跟踪路径轨迹点完成飞行任务。仿真结果表明通过对运动学习从而泛化出新的运动，同时能够根据障碍物位置规划出一条可行性避障路线。对比传统的无人机路径规划方法如蚁群算法、粒子群算法、A*算法等，本文方法特有之处在于引入了运动学习机制，基于这样一种机制，微小型四旋翼无人机不仅能够复制其学习到的运动，而且能够泛化出满足不同目标点的路径完成飞行任务，同时能够进行避障。传统的无人机路径规划方法由于未引入学习机制，从而面对相同或相似的环境原先的规划结果并没有对现有的规划有任何帮助。例如本文实验中在原有的环境中可行路径上出现了障碍物即球体，传统方法需要全局搜索，而本文方法只需在原有规划结果上添加耦合因子调整原有规划路径即可。因此该方法提高了微小型四旋翼无人机的智能性。虽然此方法对于运动学习后进行泛化结果表现良好，但是对于避障时参数的选择会影响避障轨迹，所以这是进一步需要研究的问题。本文的方法为小型无人机路径规划、避障、导航等相关领域提供了参考。

[1] 岳基隆, 张庆杰, 朱华勇. 微小型四旋翼无人机研究进展及关键技术浅析. 电光与控制, 2010, 10: 46-52

[2] 聂博文, 马宏绪, 王剑等. 微小型四旋翼飞行器的研究现状与关键技术. 电光与控制, 2007, 06: 113-117

[3] Cui C, Wang N, Chen J. Improved ant colony optimization algorithm for UAV path planning. In: Proceedings of the IEEE 5th International Conference on Software Engineering and Service Science, Beijing, China, 2014. 291-295

[4] He Y F, Zeng Q H, Liu J Y, et al. Path planning for indoor UAV based on ant colony optimization.In: Proceedings of the 25th Chinese Control and Decision Conference (CCDC), Guiyang, China, 2013. 2919-2923

[5] 傅阳光. 粒子群优化算法的改进及其在航迹规划中的应用研究: [博士学位论文]. 武汉: 华中科技大学自动化学院, 2011

[6] Roberge V, Tarbouchi M, Labonte G. Comparison of parallel genetic algorithm and particle swarm optimization for real-time UAV path planning.IEEETransactionsonIndustrialInformatics, 2013, 9(1): 132-141

[7] Wang Z, Liu L, Long T, et al. Enhanced sparse A* search for UAV path planning using dubins path estimation. In: Proceedings of the Control Conference (CCC), Nanjing, China 2014. 738-742

[8] 何雨枫. 室内微小型无人机路径规划算法研究: [硕士学位论文]. 南京: 南京航空航天大学航天学院, 2014

[9] 陈世明, 谢竟, 陈文栋等. 基于HPSO算法的三维空间路径规划. 华中科技大学学报(自然科学版), 2013, 41(2): 109-113, 119

[10] Nguyen T, Mann G K I , Gosine R G. Vision-based qualitative path-following control of quadrotor aerial vehicle. In: Proceedings of the International Conference on Unmanned Aircraft Systems (ICUAS), Orlando, USA, 2014. 412-417

[11] Ijspeert A, Nakanishi J, Pastor P, et al. Dynamical Movement Primitives: Learning attractor models for motor behaviors.NeuralComputation, 2013, 25(2): 328-373

[12] Ijspeert A, Nakanishi J, Schaal S. Movement imitation with nonlinear dynamical systems in humanoid robots.In: Proceedings of the IEEE International Conference on Robotics and Automation, Washington, USA, 2002. 1642-1648

[13] Tadej P, Andrej G, Leon Z, et al. Online learning of task-specific dynamics for periodic tasks. In: Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems, Chicago, USA, 2014. 1790-1795

[14] Ghalamzan E A M, Paxton C, Hager G D, et al. An incremental approach to learning generalizable robot tasks from human demonstration.In: Proceedings of the IEEE International Conference on Robotics and Automation (ICRA), Seattle, USA, 2015. 5616-5621

[15] Ude A, Gans A, Asfour T, et al. Task-specific generalization of discrete and periodic dynamic movement primitives.IEEETransactionsonRobotics, 2010, 26(5): 800-815

[16] Park D H, Hoffmann H, Pastor P, et al. Movement reproduction and obstacle avoidance with dynamic movement primitives and potential fields. In: Proceedings of the IEEE-RAS International Conference on Humanoid Robots, Daejeon, Korea, 2008. 91-98

[17] Fajen B R, Warren W H. Behavioral dynamics of steering, obstacle avoidance, and route selection.JournalofExperimentalPsycholologyHumanPerceptionPerformance, 2003, 29(2): 343-362

[18] 聂博文. 微小型四旋翼无人直升机建模及控制方法研究: [硕士学位论文]. 长沙: 国防科学技术大学机电工程与自动化学院, 2006

[19] 白永强, 刘昊, 石宗英等. 四旋翼无人直升机鲁棒飞行控制. 机器人, 2012, 34(5): 519-524

Path planning for micro quadrotor UAVs based on dynamic movement primitives

Chen Pengzhen， Wu Huaiyu， Chen Yang

(School of Information Science and Engineering, Wuhan University of Science and Technology, Wuhan 430081)

Aiming at the path planning problem of micro quadrotor unmanned air vehicles (UAVs), a path planning method based on dynamic movement primitives is proposed under the movement learning framework. The method extracts movement primitives by learning the given movement samples, and generalizes the learning results to new flying targets to generate corresponding trajectories. Under obstacles, it produces the path of obstacle avoidance through designing the coupling factor on the basis of existing study, and then provides the planned trail dot sets for micro quadrotor UAVs for them to accomplish the path-following flight mission. The feasibility of the proposed path planning method was verified through different target mission simulations for micro quadrotor UAVs, and the simulations also showed this method’s performance of obstacle avoidance in three dimensional space.

micro quadrotor, unmanned aerial vehicle (UAV), dynamic movement primitives (DMPs), path planning, obstacle avoidance

10.3772/j.issn.1002-0470.2016.02.010

①国家自然科学基金(61203331， 61573263)，湖北省科技支撑(2015BAA018)，武汉科技大学科技创新基金(14ZRC147)和武汉科技大学研究生创新创业基金(JCY2015002)资助项目。

2015-09-16)

②男，1992年生，硕士生；研究方向：机器人路径规划，自主学习；E-mail: chenpzd@foxmail.com 吴怀宇

③通讯作者，E-mail: wuhy@wust.edu.cn