第13卷第4期 智能系统学报 Vol.13 No.4 2018年8月 CAAI Transactions on Intelligent Systems Aug.2018 D0:10.11992/tis.201708031 网络出版地址：http:/kns.cnki.net/kcms/detail/23.1538.TP.20180402.1559.008.html 基于滚动时域的无人机动态航迹规划 王文彬2，秦小林2，张力戈2，张国华 (1.中国科学院成都计算机应用研究所，四川成都610041；2.中国科学院大学计算机与控制学院，北京 100080:3.广州大学智能软件研究院.广东广州510006) 摘要：针对带有动力学约束的多旋翼无人机航迹规划问题，提出了一种基于滚动时域控制和快速粒子群优 化(RHC-FPSO)方法。该方法引入了基于VORONOI图的代价图方法说明从航迹端点到达目标点的距离估 计。根据滚动时域和人工势场法的思想，将路径规划问题转化为优化问题，以最小距离和其他性能指标为代价 函数。设计评价函数准则，按照评价准则使用变权重粒子群优化算法求解。针对无人机靠近危险区飞行的问 题，将斥力场引入到代价函数中，提升其安全性。仿真实验结果显示，使用文中方法可以有效地在满足约束条 件下穿过障碍物区域，以及在复杂环境下可以动态计算。 关键词：航迹规划：滚动时域控制；VORONOI图；变权重：粒子群优化；人工势场 中图分类号：TP18:V279文献标志码：A文章编号：1673-4785(2018)04-0524-10 中文引用格式：王文彬，秦小林，张力戈，等.基于滚动时域的无人机动态航迹规划J智能系统学报，2018,13(4)：524-533. 英文引用格式：WANG Wenbin,QIN Xiaolin,ZHANG Lige,etal.Dynamic UAV trajectory planning based on receding horizon[Jl.. CAAI transactions on intelligent systems,2018,13(4):524-533. Dynamic UAV trajectory planning based on receding horizon WANG Wenbin'2,QIN Xiaolin"2,ZHANG Lige2,ZHANG Guohua (1.Chengdu Institute of Computer Applications,Chinese Academy of Sciences,Chengdu 610041,China;2.School of Computer and Control Engineering,University of Chinese Academy of Sciences,Beijing 100080,China;3.Academy of Intelligent Software, Guangzhou University,Guangzhou 510006,China) Abstract:Using receding horizon control and fast particle swarm optimization(RHC-FPSO),in this paper,we propose an algorithm for unmanned aerial vehicle(UAV)trajectory planning with dynamic constraints.We introduce the cost map method based on the VORONOI graph to estimate the distance from the end point of the trajectory to the target point.Using the concept of receding horizon control and the artificial potential field method,the path planning problem is transformed into an optimization problem,with the minimum distance and other performance indicators as cost func- tions.We design the evaluation function criteria based on the evaluation criteria and obtain the solution using a particle swarm optimization algorithm with variable weight.To address the problem in which a UAV approaches a danger zone, we introduce a repulsion field into the cost function to ensure safety.The simulation results show that the proposed method can effectively avoid obstacles within the constraint conditions and perform dynamic calculations in a complic- ated environment. Keywords:trajectory planning;receding horizon control;VORONOI graph;variable weight;particle swarm optimiza- tion;artificial potential field 无人机(UAV)航迹规划(trajectory planning) 环境信息的情况下获得性能最优的规划问题，是 是指在初始状态、任务目标、威胁区和一些已知 任务规划(mission planning)系统的关键技术之 一。其在任务规划系统中起着非常重要的作用， 收稿日期：2017-08-31.网络出版日期：2018-04-02 也是实现无人机智能导航并且完成任务的技术保 基金项目：国家自然科学基金项目(61402537). 通信作者：秦小林.E-mail:qinl@casit..ac.cn 障。基于飞行安全的需要，综合考虑障碍物、无
DOI: 10.11992/tis.201708031 网络出版地址: http://kns.cnki.net/kcms/detail/23.1538.TP.20180402.1559.008.html 基于滚动时域的无人机动态航迹规划 王文彬1,2，秦小林1,2,3，张力戈1,2，张国华1,2 （1. 中国科学院 成都计算机应用研究所，四川 成都 610041; 2. 中国科学院大学 计算机与控制学院，北京 100080; 3. 广州大学 智能软件研究院，广东 广州 510006） 摘 要：针对带有动力学约束的多旋翼无人机航迹规划问题，提出了一种基于滚动时域控制和快速粒子群优 化 (RHC-FPSO) 方法。该方法引入了基于 VORONOI 图的代价图方法说明从航迹端点到达目标点的距离估 计。根据滚动时域和人工势场法的思想，将路径规划问题转化为优化问题，以最小距离和其他性能指标为代价 函数。设计评价函数准则，按照评价准则使用变权重粒子群优化算法求解。针对无人机靠近危险区飞行的问 题，将斥力场引入到代价函数中，提升其安全性。仿真实验结果显示，使用文中方法可以有效地在满足约束条 件下穿过障碍物区域，以及在复杂环境下可以动态计算。 关键词：航迹规划；滚动时域控制；VORONOI 图；变权重；粒子群优化；人工势场 中图分类号：TP18；V279 文献标志码：A 文章编号：1673−4785(2018)04−0524−10 中文引用格式：王文彬, 秦小林, 张力戈, 等. 基于滚动时域的无人机动态航迹规划[J]. 智能系统学报, 2018, 13(4): 524–533. 英文引用格式：WANG Wenbin, QIN Xiaolin, ZHANG Lige, et al. Dynamic UAV trajectory planning based on receding horizon[J]. CAAI transactions on intelligent systems, 2018, 13(4): 524–533. Dynamic UAV trajectory planning based on receding horizon WANG Wenbin1,2 ，QIN Xiaolin1,2,3 ，ZHANG Lige1,2 ，ZHANG Guohua1,2 (1. Chengdu Institute of Computer Applications, Chinese Academy of Sciences, Chengdu 610041, China; 2. School of Computer and Control Engineering, University of Chinese Academy of Sciences, Beijing 100080, China; 3. Academy of Intelligent Software, Guangzhou University, Guangzhou 510006, China) Abstract: Using receding horizon control and fast particle swarm optimization (RHC-FPSO), in this paper, we propose an algorithm for unmanned aerial vehicle (UAV) trajectory planning with dynamic constraints. We introduce the cost map method based on the VORONOI graph to estimate the distance from the end point of the trajectory to the target point. Using the concept of receding horizon control and the artificial potential field method, the path planning problem is transformed into an optimization problem, with the minimum distance and other performance indicators as cost functions. We design the evaluation function criteria based on the evaluation criteria and obtain the solution using a particle swarm optimization algorithm with variable weight. To address the problem in which a UAV approaches a danger zone, we introduce a repulsion field into the cost function to ensure safety. The simulation results show that the proposed method can effectively avoid obstacles within the constraint conditions and perform dynamic calculations in a complicated environment. Keywords: trajectory planning; receding horizon control; VORONOI graph; variable weight; particle swarm optimization; artificial potential field 无人机 (UAV) 航迹规划 (trajectory planning) 是指在初始状态、任务目标、威胁区和一些已知 环境信息的情况下获得性能最优的规划问题，是 任务规划 (mission planning) 系统的关键技术之 一。其在任务规划系统中起着非常重要的作用， 也是实现无人机智能导航并且完成任务的技术保 障。基于飞行安全的需要，综合考虑障碍物、无 收稿日期：2017−08−31. 网络出版日期：2018−04−02. 基金项目：国家自然科学基金项目 (61402537). 通信作者：秦小林. E-mail：qinxl@casit.ac.cn. 第 13 卷第 4 期 智 能 系 统 学 报 Vol.13 No.4 2018 年 8 月 CAAI Transactions on Intelligent Systems Aug. 2018
第4期 王文彬，等：基于滚动时域的无人机动态航迹规划 ·525· 人机自身性能和飞行时间等约束条件，所规划出 粒子的速度，改进后的算法相比文献[18]在计算 的航迹既要尽可能减少无人机在飞行中坠毁的概 时间方面有很大提高，单次规划的平均时间减少 率，又要使得综合性能指标最小，并根据实际需 42.43%,并且在求解结果的稳定性方面具有一致 要进行飞行中的局部调整。传统的航迹规划方法 性。仿真结果也表明了RHC-FPSO算法的有效性。 是基于预先给定的地图生成一条具有最小代价的 航迹，包括A*算法山、人工势场法21、蚁群算 1相关工作 法)、概率地图方法(PRM0和快速扩展随机树 1.1滚动时域控制原理 (RRT)等。这些传统的航迹规划方法一般都用于 滚动时域控制(RHC)是一种基于模型的反 离线规划，用于在线规划时，需要很长的时间或 馈控制策略。在每一采样时刻，采集系统当前状 者极大的内存才能规划出一条最优或次优路径。 态作为初始状态，根据系统状态空间模型和约 特别地，无法对环境的变化做出快速的反应，并 束，在线求解一个有限时域的最优控制问题，将 且在规划的过程中很少涉及无人机自身的动力学 求解最优控制的第1个控制信号实际作用到系统 约束，比如速度、最大加速度的限制等。因此，很 中，重复以上过程。对于含状态约束和输入约束 少考虑无人机的安全性和航迹的可行性。 等限制的系统，滚动时域控制是一种有效的控制 近十年来，滚动时域控制的思想也被用于解 方法。随着时间的推移，当无人机执行任务时不 决航迹规划问题6)，采用滚动时域优化策略可以 断地靠近目标，在此过程中，每一次的输人都是 对带有输入和状态约束的线性系统进行最优控 通过求解一个有限时域内带约束的优化问题。因 制，易于处理约束以及多变量的优化问题。滚动 此，相比固定时域能够极大地减少计算时间，满 时域控制用于航迹规划，不要求一次规划到达 足在线航迹规划。 目标，将规划分成多个阶段，成功地绕开障碍物， RHC的基本思想：假设当前时间点为k,在有 从而极大地减少了规划的时间，使得该方法可用 限时域[k,k+H],通过使某一性能指标最优化来 于在线航迹规划。分布式滚动时域控制方法被提 确定其未来的控制作用，选择第一个最优控制输 出4，进一步减少了多无人机航迹规划的规划 入作为当前的控制输入，在下一时刻k+1,计算 时间。然而，上述工作主要集中于采用混合整数 k+1,k+H。+1的控制输入，重复以上过程，直到 线性规划(mixed integer linear programming, 任务完成。具体步骤如下： MLP)求解航迹规划最优化问题，方法面临以下 1)根据当前状态x(),考虑当前约束和未来约 两个问题：处理带复杂约束问题可扩展性不强和 束，计算未来时域Hp内的输入[u()(k+1) 求解时间随着问题规模指数增加。基于以上工作 u(k+Hp】。 的不足，本文提出了基于RHC-FPSO算法以解决 2)选择控制时域H,即[u(k)u(k+1)…(k+H)] 带有动力学约束的多旋翼无人机航迹规划问题。 作为当前的输入。 从整个航迹规划看，单次规划属于局部优化，因 3)当到达k+H。+1时刻时，测量当前状态 此需要一个全局的代价图（终端罚函数）来表示 xk+H+I)。 航迹端点到目标点的代价估计。区别于文献[8]， 4)当k+H+1时刻，在有限时域[u(k+H+1), 本文提出的基于VORONOI图的代价图可以使得 (k+H。+H。+1)内重复步骤1)~3)直到达到目标。 规划出的航迹尽可能地远离障碍物，提高了无人 滚动时域优化的基本思想是：首先，根据对应 机的生存机率。 的目标函数和约束条件，RHC利用多旋翼无人机 文献[17]通过空战人工势场确定其威力，将 的状态空间模型，预测未来规划时域内的控制输 合威力引入PSO算法。通过人工势场启发粒子 入，将其作用于系统中；然后，测量下一时刻的状 群算法在当前飞行方向的可机动范围内进行寻 态，根据当前的状态进行下一步优化，随着时间 优，重点加强对合威力方向的寻优。区别于文献[17， 的推进反复滚动执行。具体过程如图1所示。 本文将人工势场法加入到目标函数使无人机远离 从以上对模型预测控制的介绍和分析，可以 障碍物，增加了无人机的安全性。此外，结合两 看出：RHC是根据当前的状态和目标函数进行不 种方法的优势，减少了计算量且在环境发生改变 断地迭代，求解该时刻的有限时段的最优输入 时，只需更新势场。RHC-APF是一种非常优秀的 采用的是局部优化，不是一个不变的全局最优目 航迹规划算法，易于扩展。在优化过程中，计算 标。因此，需要代价图使得规划时跳出局部最优 每个粒子的约束违背量，根据约束违背量来更新 值，完成全局优化
人机自身性能和飞行时间等约束条件，所规划出 的航迹既要尽可能减少无人机在飞行中坠毁的概 率，又要使得综合性能指标最小，并根据实际需 要进行飞行中的局部调整。传统的航迹规划方法 是基于预先给定的地图生成一条具有最小代价的 航迹，包括 A*算法[ 1 ] 、人工势场法[ 2 ] 、蚁群算 法 [3] 、概率地图方法[4] (PRM) 和快速扩展随机树[5] (RRT) 等。这些传统的航迹规划方法一般都用于 离线规划，用于在线规划时，需要很长的时间或 者极大的内存才能规划出一条最优或次优路径。 特别地，无法对环境的变化做出快速的反应，并 且在规划的过程中很少涉及无人机自身的动力学 约束，比如速度、最大加速度的限制等。因此，很 少考虑无人机的安全性和航迹的可行性。 近十年来，滚动时域控制的思想也被用于解 决航迹规划问题[6-9] ，采用滚动时域优化策略可以 对带有输入和状态约束的线性系统进行最优控 制，易于处理约束以及多变量的优化问题。滚动 时域控制[8-13]用于航迹规划，不要求一次规划到达 目标，将规划分成多个阶段，成功地绕开障碍物， 从而极大地减少了规划的时间，使得该方法可用 于在线航迹规划。分布式滚动时域控制方法被提 出 [14-16] ，进一步减少了多无人机航迹规划的规划 时间。然而，上述工作主要集中于采用混合整数 线性规划 (mixed integer linear programming， MILP) 求解航迹规划最优化问题，方法面临以下 两个问题：处理带复杂约束问题可扩展性不强和 求解时间随着问题规模指数增加。基于以上工作 的不足，本文提出了基于 RHC-FPSO 算法以解决 带有动力学约束的多旋翼无人机航迹规划问题。 从整个航迹规划看，单次规划属于局部优化，因 此需要一个全局的代价图 (终端罚函数) 来表示 航迹端点到目标点的代价估计。区别于文献[8]， 本文提出的基于 VORONOI 图的代价图可以使得 规划出的航迹尽可能地远离障碍物，提高了无人 机的生存机率。 文献[17]通过空战人工势场确定其威力，将 合威力引入 PSO 算法。通过人工势场启发粒子 群算法在当前飞行方向的可机动范围内进行寻 优，重点加强对合威力方向的寻优。区别于文献[17]， 本文将人工势场法加入到目标函数使无人机远离 障碍物，增加了无人机的安全性。此外，结合两 种方法的优势，减少了计算量且在环境发生改变 时，只需更新势场。RHC-APF 是一种非常优秀的 航迹规划算法，易于扩展。在优化过程中，计算 每个粒子的约束违背量，根据约束违背量来更新 粒子的速度，改进后的算法相比文献[18]在计算 时间方面有很大提高，单次规划的平均时间减少 42.43%，并且在求解结果的稳定性方面具有一致 性。仿真结果也表明了 RHC-FPSO 算法的有效性。 1 相关工作 1.1 滚动时域控制原理 滚动时域控制[19] (RHC) 是一种基于模型的反 馈控制策略。在每一采样时刻，采集系统当前状 态作为初始状态，根据系统状态空间模型和约 束，在线求解一个有限时域的最优控制问题，将 求解最优控制的第 1 个控制信号实际作用到系统 中，重复以上过程。对于含状态约束和输入约束 等限制的系统，滚动时域控制是一种有效的控制 方法。随着时间的推移，当无人机执行任务时不 断地靠近目标，在此过程中，每一次的输入都是 通过求解一个有限时域内带约束的优化问题。因 此，相比固定时域能够极大地减少计算时间，满 足在线航迹规划。 k [k, k+ Hp] k+1 [k+1, k+ Hp +1] RHC 的基本思想：假设当前时间点为 ，在有 限时域 ，通过使某一性能指标最优化来 确定其未来的控制作用，选择第一个最优控制输 入作为当前的控制输入，在下一时刻 ，计算 的控制输入，重复以上过程，直到 任务完成。具体步骤如下： x(k) Hp [u(k) u(k+1) ··· u(k+ Hp)] 1) 根据当前状态 ，考虑当前约束和未来约 束，计算未来时域 内的输入 。 2) 选择控制时域 He，即 [u(k) u(k+1) ··· u(k+ He)] 作为当前的输入。 k+ He +1 x(k+ He +1) 3) 当到达 时刻时，测量当前状态 。 k+ He +1 [u(k+ He +1), u(k+ He + Hp +1)] 4) 当 时刻，在有限时域 内重复步骤 1)～3) 直到达到目标。 滚动时域优化的基本思想是：首先，根据对应 的目标函数和约束条件，RHC 利用多旋翼无人机 的状态空间模型，预测未来规划时域内的控制输 入，将其作用于系统中；然后，测量下一时刻的状 态，根据当前的状态进行下一步优化，随着时间 的推进反复滚动执行。具体过程如图 1 所示。 从以上对模型预测控制的介绍和分析，可以 看出：RHC 是根据当前的状态和目标函数进行不 断地迭代，求解该时刻的有限时段的最优输入， 采用的是局部优化，不是一个不变的全局最优目 标。因此，需要代价图使得规划时跳出局部最优 值，完成全局优化。 第 4 期 王文彬，等：基于滚动时域的无人机动态航迹规划 ·525·
·526· 智能系统学报 第13卷 当前 发，在规避障碍物和满足相应的约束条件下，以 过去+ 一一未来 Wk+k) 最小代价到达目标区域。其线性时不变系统动力 kj) 学描述为 预测时域认+) x(k+1)=Ax(k)+Bu(k) (4) yk)=Cx(k)+Du(k)∈yk) (5) kj) 控制时域 式中：x(∈RW,是状态向量，（阳∈R是输入向量， k+m k+H。 等式(4)为无人机的状态空间模型，等式（⑤）为状 态和输入需要满足的约束条件，其中y为k时刻 图1滚动时域控制示意图 Fig.1 The diagram of receding horizon control 需要满足的约束条件，输出向量y()∈R心。 般的目标函数，采用如下形式： 1.2人工势场法 人工势场法2o是由Khatib在1986提出的一 J=2u.xW.xe） (6) 种虚拟力场法。其方法是将研究对象所处的环境 式中：I()是代价函数，F是目标集。在RHC框架 用势场来定义，通过位置信息来控制对象的避障 中，优化是在一个有限域内执行的，第1个控制输 运动。将研究对象在实际环境中的运动转换为在 入被执行，形成新的状态，基于当前的状态进行 虚拟势场的运动，在该势场中目标点对研究对象 下一步优化，直到到达目标集。 产生引力，环境中的障碍物产生斥力。引力和斥 因此基于RHC框架的优化完整的描述为 力的合力决定了研究对象的运动方向和运动速 度。人工势场相比其他的算法具有计算量小、描 mind I(u(k+jk).x(k+k).x)+ 述简单、实时性高、应用广泛。 j=0 (7) f(x(++1).X) 根据人工势场的理论，对于静态或者动态的 x(k+j+1k)=Ax(k+jk)+Bu(k+jik) 环境，都可以计算相应的人工势能图。对于任意 j=0,1,…,Hp (8) 状态，研究对象的位姿用x表示，则势场可以用 y(k+ilk)=Cx(k+jk)+Du(k+ik)ey(k) U()表示，目标转态位姿用x,表示，因此相应的吸 j=0,1,…,H2 (9) 引势为Um(x),障碍物的位姿用x表示，排斥势 式中：x(k+派)表示在时刻k,(k+)时刻的预测值， Up(x,因此空间中的任意位姿的势能场都可以表 y表示输出约束，H,为滚动时域，f()表示终端惩 示为 罚项。 U(x)=U.(x)+Urep(x) (1) 文献[10]使用MLP优化目标函数(7)，如果 目标点对研究对象产生的势能大小与两者的 存在非线性约束条件或者目标函数，通过将其转 距离相关，距离大、势能大，反之亦然。当二者之 化为多约束条件进行线性化，将会导致约束条件 间的距离为零时，表示到达目标点，因此引力势 呈指数增长，会损失一部分求解精度，可扩展性 能与距离成正比关系，可表示为 不强。并且对于复杂的环境和目标函数，求解速 Um因）=ikmd(x,） (2) 度进一步降低。PSO可以快速求解一个可行解， 因此，使用PSO结合RHC可以平衡求解时间和 与引力势场类似，障碍物对研究对象产生斥 力场。斥力场势能大小与障碍物和研究对象的距 求解精度。实验结果表明，基于改进的PSO方法 (FPSO)能够有效地提高计算效率。 离有关，距离越近，斥力势能越大，反之越小。因 此可以表示为 3RHC-FPSO航迹规划 1 Urep(x)= kmd.而 dx,xo)≤dn (3) 本文将整个航迹规划分成两个阶段，第1阶 0,d(x,x)>d 段为代价评估阶段，根据当前环境生成代价图， 对于势场Ux)的定义方式可以有很多种，可 当环境发生改变时，重新计算代价图，使用基于 以按照实际情况具体定义。势场法在数学的描述 VORONOI图的代价图来表示航迹端点到目标点 上简单，在路径规划中广泛应用。 的代价估计；第2阶段为在线航迹规划阶段，在问 2问题描述 题描述中，已经将航迹规划表示成一个优化问 题，因此本阶段主要是基于RHC-FPSO在线航迹 以多旋翼无人机为例，讨论其由初始状态出 规划的一个优化过程
k− j k k+m 过去 当前 未来 控制时域 预测时域 k+Hp y(k−j) u(k−j) y(k+j|k) u(k+j|k) 图 1 滚动时域控制示意图 Fig. 1 The diagram of receding horizon control 1.2 人工势场法 人工势场法[20]是由 Khatib 在 1986 提出的一 种虚拟力场法。其方法是将研究对象所处的环境 用势场来定义，通过位置信息来控制对象的避障 运动。将研究对象在实际环境中的运动转换为在 虚拟势场的运动，在该势场中目标点对研究对象 产生引力，环境中的障碍物产生斥力。引力和斥 力的合力决定了研究对象的运动方向和运动速 度。人工势场相比其他的算法具有计算量小、描 述简单、实时性高、应用广泛。 x U(x) xg Uatt(x) xo Urep(x) 根据人工势场的理论，对于静态或者动态的 环境，都可以计算相应的人工势能图。对于任意 状态，研究对象的位姿用 表示，则势场可以用 表示，目标转态位姿用 表示，因此相应的吸 引势为 ，障碍物的位姿用 表示，排斥势 ，因此空间中的任意位姿的势能场都可以表 示为 U(x) = Uatt(x)+Urep(x) (1) 目标点对研究对象产生的势能大小与两者的 距离相关，距离大、势能大，反之亦然。当二者之 间的距离为零时，表示到达目标点，因此引力势 能与距离成正比关系，可表示为 Uatt(x) = 1 2 kattd 2 (x, xg) (2) 与引力势场类似，障碍物对研究对象产生斥 力场。斥力场势能大小与障碍物和研究对象的距 离有关，距离越近，斥力势能越大，反之越小。因 此可以表示为 Urep(x) = 1 2 krep( 1 d(x, x0) − 1 dn ), d(x, xo) ⩽ dn 0, d(x, xo) > dn (3) 对于势场 U(x) 的定义方式可以有很多种，可 以按照实际情况具体定义。势场法在数学的描述 上简单，在路径规划中广泛应用。 2 问题描述 以多旋翼无人机为例，讨论其由初始状态出 发，在规避障碍物和满足相应的约束条件下，以 最小代价到达目标区域。其线性时不变系统动力 学描述为[14] x(k+1) = Ax(k)+ Bu(k) (4) y(k) = Cx(k)+ Du(k) ∈ γ(k) (5) x(k) ∈ R Nx u(k) ∈ R Nu γ(k) k y(k) ∈ R Ny 式中： 是状态向量， 是输入向量， 等式 (4) 为无人机的状态空间模型，等式 (5) 为状 态和输入需要满足的约束条件，其中 为 时刻 需要满足的约束条件，输出向量 。 一般的目标函数，采用如下形式： J = ∑∞ k=0 l(u(k), x(k), χF) (6) 式中： l(∗) 是代价函数，χF 是目标集。在 RHC 框架 中，优化是在一个有限域内执行的，第 1 个控制输 入被执行，形成新的状态，基于当前的状态进行 下一步优化，直到到达目标集。 因此基于 RHC 框架的优化完整的描述为 J ∗ = min u(.) { ∑Hp j=0 l(u(k+ j|k), x(k+ j|k), χF)+ f(x(k+ Hp +1), χF)} (7) x(k+ j+1|k) = Ax(k+ j|k)+ Bu(k+ j|k) j = 0,1,··· ,Hp (8) y(k+ j|k) = Cx(k+ j|k)+ Du(k+ j|k) ∈ γ(k) j = 0,1,··· ,Hp (9) x(k+ j|k) k (k+ j) γ Hp f(∗) 式中： 表示在时刻 ， 时刻的预测值， 表示输出约束， 为滚动时域， 表示终端惩 罚项。 文献[10]使用 MILP 优化目标函数 (7)，如果 存在非线性约束条件或者目标函数，通过将其转 化为多约束条件进行线性化，将会导致约束条件 呈指数增长，会损失一部分求解精度，可扩展性 不强。并且对于复杂的环境和目标函数，求解速 度进一步降低。PSO 可以快速求解一个可行解， 因此，使用 PSO 结合 RHC 可以平衡求解时间和 求解精度。实验结果表明，基于改进的 PSO 方法 (FPSO) 能够有效地提高计算效率。 3 RHC-FPSO 航迹规划 本文将整个航迹规划分成两个阶段，第 1 阶 段为代价评估阶段，根据当前环境生成代价图， 当环境发生改变时，重新计算代价图，使用基于 VORONOI 图的代价图来表示航迹端点到目标点 的代价估计；第 2 阶段为在线航迹规划阶段，在问 题描述中，已经将航迹规划表示成一个优化问 题，因此本阶段主要是基于 RHC-FPSO 在线航迹 规划的一个优化过程。 ·526· 智 能 系 统 学 报 第 13 卷
第4期 王文彬，等：基于滚动时域的无人机动态航迹规划 ·527· 3.1代价图 因此整个平面被划分成n个单元，具有以下性质： 根据第1节分析可知，从航迹规划全局看，滚 任意一点g位于点P所对应的单元中，当且仅当对 动时域优化是局部优化，因此容易陷入局部最 于任意的P,j≠都有dist(q,P)<dist(q,P)o 优。如图2所示，这种情况下无人机陷入了局部 根据定义1可知，VORONOI多边形的每条边 最优，目标不可达，因此需要估计预测航迹端点 上的点到相对应的两个点的距离相等，即VORO 到目标点的代价。代价估计阶段根据给出的障碍 NOI多边形上的点是到障碍物或威胁点的最远 物和目标预先计算，当探测到环境发生改变将会 点。因此可以得出，飞行器以VORONOI多边形 重新计算。航迹规划阶段使用预先计算的代价图 的顶点作为可视点，可以提高飞行器的安全系数。 来估计航迹端点到目标点的代价。 定理1 VORONOI图定理22：若n>3,则在 与平面上任意n个基点相对应的VORONOI图中， 顶点的数目不会超过2n-5,而且边的数目不会超 00 过3n-6。 0 由定理1可知，采用基于VORONOI图的可 视点的方法，相对于文献[8]，可视点呈线性增长， 并且小于2n。 3.2基于RHC-FPSO在线航迹规划 航迹规划是复杂约束条件下的最优化问题， -1 采用粒子群等智能优化算法进行航迹规划时，往 10-8-6-4-2024 往算法迭代初期，控制输入和状态可能存在违反 图2不含代价图的航迹规划示意图 约束的情况，算法需要迭代若干次才能产生满足 Fig.2 The diagram of trajectory planning without cost 要求的输入，并在此基础上进一步寻优。粒子群 map 算法中种群最优个体影响着粒子搜索方向，如何 文献[8]提出的代价图表示航迹端点到目标点 缩短搜索到可行输入的时间对提高算法的效率至 的时间估计值。基本思想为：在不考虑无人机自 关重要。因此，我们根据粒子违反约束条件的程 身的动力学约束条件下，对于多边形的障碍物， 度来更新粒子速度。 连接开始点、障碍物顶点和目标点，如果这些点 评价函数设计：评价函数用于给粒子群中每 的连线没有穿过障碍物，则添加一条边，这样形 个个体计算适应度。所有约束条件都满足的输入 成的图称为可视图。对于可视图使用Dijkstra2山 粒子称为可行个体，违反任何一项约束条件的个 单源最短路径算法，搜索从目标点到各个点的最 体称为不可行个体。在进行个体评价时遵循以下 短路径。由第3节问题描述可知，航迹端点为 准则： x(k+H。+1),因此基于代价图的终端惩罚项f(*)可 1)对于可行输入来说，根据式(3)，代价小的 以表示如下： 输入优于代价大的输人； f(x(++1).x))=d(x(k++1).xvis)+Cvis (10) 式中：x为基于代价图的可视点，dx(k+Hp+1),x) 2)对于不可行航迹来说，约束违背小的输入 为航迹端点到可视点的代价，C为可视点到目标 优于约束违背大的输入； 3)对于可行输人与不可行输人，可行输入总 点的代价。 是优于不可行输入： 然而，基于文献[8]提出的代价图，选择的可 视点为障碍物的顶点，这会导致规划出的航迹会 不可行输入意味着在实际中输入是不可行 沿着障碍物边缘，降低无人机安全性。因此，我 的，因此没有必要计算它的代价，可以减少计算 们引入基于VORONOI图的可视图，VORONOI图 量。基于以上分析，对任意输入，采用下面的评 的优点是使得可视点离障碍物的距离尽可能远： 价函数。 VORONOI图使得可视点的选择不再是障碍物的 C(u),可行解 F(u)= BigM+C,不可行解 (11) 顶点，而是障碍物之间的顶点连线的中点。 定义1 VORONOI图22。任意两点p和q之 式中：Cw为式(T)计算的航迹代价，BigM为一个 间的欧式距离，记作dist(p,q),假设平面P= 比较大的数。C为约束违背量，采用式(12)计算方式： {p1,P2,…,P为平面上的任意n个互异的点，P所对 C (wif()+w28(v:)+w3h(a)) (12) 应的VORONOI图是平面的一个子区域的划分
3.1 代价图 根据第 1 节分析可知，从航迹规划全局看，滚 动时域优化是局部优化，因此容易陷入局部最 优。如图 2 所示，这种情况下无人机陷入了局部 最优，目标不可达，因此需要估计预测航迹端点 到目标点的代价。代价估计阶段根据给出的障碍 物和目标预先计算，当探测到环境发生改变将会 重新计算。航迹规划阶段使用预先计算的代价图 来估计航迹端点到目标点的代价。 −10 −8 −6 −4 −2 0 2 4 −10 −5 0 5 x y 图 2 不含代价图的航迹规划示意图 Fig. 2 The diagram of trajectory planning without cost map x(k+ Hp +1) f(∗) 文献[8]提出的代价图表示航迹端点到目标点 的时间估计值。基本思想为：在不考虑无人机自 身的动力学约束条件下，对于多边形的障碍物， 连接开始点、障碍物顶点和目标点，如果这些点 的连线没有穿过障碍物，则添加一条边，这样形 成的图称为可视图。对于可视图使用 Dijkstra[21] 单源最短路径算法，搜索从目标点到各个点的最 短路径。由第 3 节问题描述可知，航迹端点为 ，因此基于代价图的终端惩罚项 可 以表示如下： f(x(k+ Hp +1), χF)) = d(x(k+ Hp +1), xvis)+Cvis (10) xvis d(x(k+ Hp +1), xvis) Cvis 式中： 为基于代价图的可视点， 为航迹端点到可视点的代价， 为可视点到目标 点的代价。 然而，基于文献[8]提出的代价图，选择的可 视点为障碍物的顶点，这会导致规划出的航迹会 沿着障碍物边缘，降低无人机安全性。因此，我 们引入基于 VORONOI 图的可视图，VORONOI 图 的优点是使得可视点离障碍物的距离尽可能远， VORONOI 图使得可视点的选择不再是障碍物的 顶点，而是障碍物之间的顶点连线的中点。 p q dist(p,q) {p1, p2,··· , pn} n P 定义 1 VORONOI 图 [22]。任意两点 和 之 间的欧式距离，记作 ，假设平 面 P = 为平面上的任意 个互异的点， 所对 应的 VORONOI 图是平面的一个子区域的划分， n q Pi Pj j , i dist(q,Pi) < dist(q,Pj) 因此整个平面被划分成 个单元，具有以下性质： 任意一点 位于点 所对应的单元中，当且仅当对 于任意的 ， 都有 。 根据定义 1 可知，VORONOI 多边形的每条边 上的点到相对应的两个点的距离相等，即 VORONOI 多边形上的点是到障碍物或威胁点的最远 点。因此可以得出，飞行器以 VORONOI 多边形 的顶点作为可视点，可以提高飞行器的安全系数。 n > 3 n 2n−5 3n−6 定理 1 VORONOI 图定理[22] ：若 ，则在 与平面上任意 个基点相对应的 VORONOI 图中， 顶点的数目不会超过 ，而且边的数目不会超 过 。 2n 由定理 1 可知，采用基于 VORONOI 图的可 视点的方法，相对于文献[8]，可视点呈线性增长， 并且小于 。 3.2 基于 RHC-FPSO 在线航迹规划 航迹规划是复杂约束条件下的最优化问题， 采用粒子群等智能优化算法进行航迹规划时，往 往算法迭代初期，控制输入和状态可能存在违反 约束的情况，算法需要迭代若干次才能产生满足 要求的输入，并在此基础上进一步寻优。粒子群 算法中种群最优个体影响着粒子搜索方向，如何 缩短搜索到可行输入的时间对提高算法的效率至 关重要。因此，我们根据粒子违反约束条件的程 度来更新粒子速度。 评价函数设计：评价函数用于给粒子群中每 个个体计算适应度。所有约束条件都满足的输入 粒子称为可行个体，违反任何一项约束条件的个 体称为不可行个体。在进行个体评价时遵循以下 准则： 1) 对于可行输入来说，根据式 (3)，代价小的 输入优于代价大的输入； 2) 对于不可行航迹来说，约束违背小的输入 优于约束违背大的输入； 3) 对于可行输入与不可行输入，可行输入总 是优于不可行输入； 不可行输入意味着在实际中输入是不可行 的，因此没有必要计算它的代价，可以减少计算 量。基于以上分析，对任意输入，采用下面的评 价函数。 F(u) = { C(u), 可行解 BigM+C, 不可行解 (11) C(u) C 式中： 为式 (7) 计算的航迹代价，BigM 为一个 比较大的数。 为约束违背量，采用式 (12) 计算方式： C = ∑Hp i=1 (w1 f(li)+w2g(vi)+w3h(ai)) (12) 第 4 期 王文彬，等：基于滚动时域的无人机动态航迹规划 ·527·
·528· 智能系统学报 第13卷 式(12)用于计算不可行输入，f)表示航迹段 人机一个斥力场，因此此时的目标函数可以表示 i穿越禁飞区的约束违背量，g(y)和h(a)表示速度 为式(16)的形式： 和加速度的约束违背量，w、w、w为权系数。 J=J+Urep (16) 图3显示了函数F()的图像，在可行解区域 2)代价图的计算会增加计算负担，而且不利 F(w)和C(w)相等，在不可行区域，离区域边界越 于在线规划，因为当环境发生改变时，需要重新 远，F(W值越大。 计算代价图，特别突发事件，会降低实时性。人 不可行区域 可行区域 工势场法只需增加障碍物的一个势场，当目标发 代价， 生改变时，也只需改变目标函数的势场，并且势 场的计算非常简单。将RHC和APF结合后能够 F(u),C(u) 克服AP℉的不足，同时增加了实时性。此时的目 约束违背量 标函数为 J=Uam+Urep (17) 3.4算法实现流程 约束违背量 1)首先初始化无人机参数，设置最大速度、 最大加速等限制，设置滚动时域，以及无人机采 图3评价函数F(W)图像 Fig.3 The image of evaluation function F(u) 样间隔。 假设粒子群种群数为N,每个粒子的空间维数 2)初始化粒子群P,包括粒子的位置x= 为D,第个粒子的输入向量和速度为=(1,2，…， (1,x2,…,xH,)和速度：=(y,V2,…,,),按照表1 山Dy,=(wy2,…,vD。第个粒子最优解为P=(, 设置相关参数。 P2,…,PD),粒子群最优解为G=(g1,82,…,8n)。因 表1仿真参数和值 Table 1 此，根据粒子群优化策略，粒子更新公式为 Simulation parameters and values ,k+1)=w(k)P,()+c1n1(P:(k)-x(k)+ 参数 值 描述 c2r2(G(x(k (13) G 100 最大迭代次数 x(k+1)=x,(k)+ww,(K) 心 20 粒子数量 式中：c和c2为学习因子，1和2为区间[0,1]的随机 数，式(13)的第1部分为粒子的全局搜索能力， △t/s 2.6 时间 第2部分为粒子的记忆能力，第3部分为粒子之 Hp 6 规划时域 间信息共享。w()为粒子的迭代权重 Vmax/(m's) 0.5 最大速度 w因=wa一”--k (14) itermax amax/(m's) 0.17 最大加速度 式中：k为当前迭代次数，iterm为总的迭代次数， 1.9 加速因子 wm和wx分别为w()的最小和最大值，在这里 C2 2.1 加速因子 wx不再是一成不变的值，而是根据粒子的适应 值来更新，如果粒子是可行解，则按照w(k)更新， 3)按照式(11)和式(12)分别计算每个粒子的 适应度和约束违背量。 如果粒子是不可行解，则按照式(15)更新： a(k,i)=1-、Fu(0 4)更新粒子群最优解和每个粒子的最优解。 (15) max(F(u)) 5)如果粒子满足约束条件，按照式(13)和式 (14)更新粒子的位置和速度。否则，按照式(13) 式中：F(W)为第k次迭代第i个粒子的适应度。实 验结果表明，相比于文献[15]，这种更新方式在计 和式(15)更新粒子的位置和速度。 算时间效率上更快。 6)如果粒子群不满足终止条件，转3)。 3.3基于RHC-APF在线航迹规划 T)取最优结果xe=(e,be2,…,eH,)的第 3.1节介绍的方法中，引入的VORONOI图， 一个控制输入xe!输入模型(4)中。计算无人机 构建比较复杂，而且当环境改变的时候，需要重 下一时刻的状态。 新计算，不利于实时航迹规划。本节结合APF实 8)如果到达目标点，完成航迹规划。否则转2)。 时计算和计算简单的特点，提出RHC-APF算法。 4仿真结果与分析 1)基于4.1提出的代价图，会导致无人机靠 近障碍物飞行。按照APF的思想，让障碍物给无 本文的基本算法已在MATLAB2016中进行
f(li) i g(vi) h(ai) w1 w2 w3 式 (12) 用于计算不可行输入， 表示航迹段 穿越禁飞区的约束违背量， 和 表示速度 和加速度的约束违背量， 、 、 为权系数。 F(u) F(u) C(u) F(u) 图 3 显示了函数 的图像，在可行解区域 和 相等，在不可行区域，离区域边界越 远， 值越大。 C(u) F(u), C(u) BigM 不可行区域 可行区域 约束违背量 代价 约束违背量 图 3 评价函数 F(u) 图像 Fig. 3 The image of evaluation function F(u) N D i ui =(ui,1,ui,2,··· , ui,D) vi =(vi,1, vi,2,··· , vi,D) i Pi =(pi,1, pi,2,··· , pi,D) G = (g1,g2,··· ,gD) 假设粒子群种群数为 ，每个粒子的空间维数 为 ，第 个粒子的输入向量和速度为 ， 。第 个粒子最优解为 ，粒子群最优解为 。因 此，根据粒子群优化策略，粒子更新公式为 vi(k+1) = w(k)vi(k)+c1r1(Pi(k)− xi(k))+ c2r2(G(k)− xi(k)) xi(k+1) = xi(k)+wvi(k) (13) c1 c2 r1 r2 w(k) 式中： 和 为学习因子， 和 为区间[0, 1]的随机 数，式 (13) 的第 1 部分为粒子的全局搜索能力， 第 2 部分为粒子的记忆能力，第 3 部分为粒子之 间信息共享。 为粒子的迭代权重: w(k) = wmax − wmax −wmin itermax k (14) k itermax wmin wmax w(k) wmax w(k) 式中： 为当前迭代次数， 为总的迭代次数， 和 分别为 的最小和最大值，在这里 不再是一成不变的值，而是根据粒子的适应 值来更新，如果粒子是可行解，则按照 更新， 如果粒子是不可行解，则按照式 (15) 更新： wmax(k,i) = 1− Fk,i(u) max i (Fk,i(u)) (15) 式中： Fk,i(u) 为第 k 次迭代第 i 个粒子的适应度。实 验结果表明，相比于文献[15]，这种更新方式在计 算时间效率上更快。 3.3 基于 RHC-APF 在线航迹规划 3.1 节介绍的方法中，引入的 VORONOI 图， 构建比较复杂，而且当环境改变的时候，需要重 新计算，不利于实时航迹规划。本节结合 APF 实 时计算和计算简单的特点，提出 RHC-APF 算法。 1) 基于 4.1 提出的代价图，会导致无人机靠 近障碍物飞行。按照 APF 的思想，让障碍物给无 人机一个斥力场，因此此时的目标函数可以表示 为式 (16) 的形式： J = J ∗ +Urep (16) 2) 代价图的计算会增加计算负担，而且不利 于在线规划，因为当环境发生改变时，需要重新 计算代价图，特别突发事件，会降低实时性。人 工势场法只需增加障碍物的一个势场，当目标发 生改变时，也只需改变目标函数的势场，并且势 场的计算非常简单。将 RHC 和 APF 结合后能够 克服 APF 的不足，同时增加了实时性。此时的目 标函数为 J = Uatt +Urep (17) 3.4 算法实现流程 1) 首先初始化无人机参数，设置最大速度、 最大加速等限制，设置滚动时域，以及无人机采 样间隔。 xi = (xi,1, xi,2,··· , xi,Hp ) vi = (vi,1, vi,2,··· , vi,Hp ) 2) 初始化粒子 群 P，包括粒子的位置 和速度 ，按照表 1 设置相关参数。 表 1 仿真参数和值 Table 1 Simulation parameters and values 参数 值 描述 G 100 最大迭代次数 P 20 粒子数量 ∆t /s 2.6 时间 Hp 6 规划时域 vmax/(m·s–1) 0.5 最大速度 amax/(m·s–2) 0.17 最大加速度 c1 1.9 加速因子 c2 2.1 加速因子 3) 按照式 (11) 和式 (12) 分别计算每个粒子的 适应度和约束违背量。 4) 更新粒子群最优解和每个粒子的最优解。 5) 如果粒子满足约束条件，按照式 (13) 和式 (14) 更新粒子的位置和速度。否则，按照式 (13) 和式 (15) 更新粒子的位置和速度。 6) 如果粒子群不满足终止条件，转 3)。 xbest = (xbest,1, xbest,2,··· , xbest,Hp ) xbest,1 7) 取最优结果 的第 一个控制输入 输入模型 (4) 中。计算无人机 下一时刻的状态。 8) 如果到达目标点，完成航迹规划。否则转 2)。 4 仿真结果与分析 本文的基本算法已在 MATLAB 2016 中进行 ·528· 智 能 系 统 学 报 第 13 卷