无人机快速三维航迹规划算法_尹高扬

摘
提出了基于快速扩展随机树的三维航迹规划方法。 要： 针对无人机三维航迹规划的实时性问题， 该算法能够根据当前环境快速有效搜索规划空间， 通过随机采样点将搜索导向空白区域， 使三维航迹 规划能够用于实时航迹规划。通过引入航迹距离约束， 搜索树将沿着路径距离最短的近似最优航迹 的方向进行扩展， 克服了基本快速扩展随机树方法随机性强， 只能快速获得可行航迹， 无法获得较优
2016 年 8 月 第 34 卷第 4 期
西北工业大学学报 Journal of Northwestern Polytechnical University
Aug． 2016 Vol． 34 No． 4
无人机快速三维航迹规划算法
尹高扬，周绍磊，吴青坡
（ 海军航空工程学院 控制科学与工程系，山东 烟台 264001 ）
令 q new （ x near ， y near ， z near ） ， q near 的 根 节 点 为 q near － root （ x near － root ， z near － root ） 。 q new 必须满足避障和无人机自身性 y near － root ， 能约束的条件下才能加入到扩展树 T 中。
始化搜索树 T。 2 ） 以概率 p 选择目标点作为采样点； 以概率 1 － p 在整个规划窗口内随机选择采样点 q rand 。 3 ） 通过随机采样点 q rand ， 找出已存在扩展树 T 的树节点 q 中离随机采样点距离最近的一个树节点 q near 。 在 q near 和 q rand 的连线上， 计算从 q near 以最小航 迹段长度 L 到达的新点 q new 。 4 ） 判断 q new 是否满足避障和文中所述无人机 自身性能约束， 若满足， 则将 q new 加入到扩展树 T 中。 否则转到步骤 2 ） 。 5 ） 判断 | q new － q goal | ≤ L， 若满足， 转到步骤 6） ， 否则转到步骤 2 ） 。
0212 收稿日期： 2016-
作者简介： 尹高扬（ 1987 —） ， 制导与控制的研究。 海军航空工程学院博士研究生， 主要从事导航、
第4 期
尹高扬， 等：无人机快速三维航迹规划算法
· 565·
约束。无人机在惯性坐标系下的三自由度质点运动 方程为：
{
x = vcosψcosγ y = vsinψcosγ z = vsinγ
航迹的缺点。在搜索过程中无人机的航迹约束条件和地形信息得到了充分利用， 使算法生成的航迹 ， 。 能够自动回避地形和威胁 同时满足无人机的动力学约束 通过生成的虚拟数字地图对算法进行了 仿真验证， 仿真结果表明该方法能够快速有效地规划出满意的无人机三维航迹。 词： 无人机; 快速扩展随机树; 实时性; 地形回避; 三维航迹 2758 （ 2016 ） 04056407 中图分类号： V249． 1 文献标志码： A 文章编号： 1000关 键 在现代战争中， 无人机所处的战场环境瞬息万 变， 同时由于其所执行任务的不确定性 ， 无人机要能 够根据任务过程中的实时环境信息进行在线的自主 航迹规划， 这对规划算法的实时性提出了很高要 求 。传统的航迹规划方法是基于预先确定的代 价函数生成一条具有最小代价的路径 。由于无人机
航迹规划的规划区域广阔， 虽然近年来研究人员从 规划环境建模和求解算法两方面均提出了许多改进 策略， 但由于规划算法强调航迹的最优性 ， 加上要考 虑无人机的性能约束等要求， 传统的规划方法如遗 ［34 ］ ［56 ］ * ［78 ］ 、 、 、 传算法 蚁群算法 稀疏 A 算法 粒子群 算法 等要获得一条最优路径需要很长的收敛时 间和极大的内存需求， 失去了在线航迹规划的现实 可行性。 快速扩展随机树 （ RRT ） 方法是一种基于采样 的单查询随机搜索算法， 由 S． M． LaValle 在 1998 年 首次提出 。RRT 的节点扩展不需要在规划前执 行预处理， 能够根据当前的环境信息快速有效地搜 索规划空间， 在可行路径的搜索概率意义上是完全 的。RRT 方法已被成功应用于许多非完整系统的 ［12 ］ 规划问题中， 包括地面移动机器人运动规划 和无 人机航迹规划
［12 ］
很多良好特性， 但是其随机性太强， 只能够保证高效 快速地获取无人机的可行轨迹， 无法获得较优轨迹。 针对基本 RRT 方法的不足， 研究者们从 RRT 方法 的节点采样方式、 节点扩展方式和节点选择方式 3 个方面对其进行了改进， 取得了一定的成果
［1419 ］
。
基本 RRT 及其改进方法虽然能够满足无人机 在线自主航迹规划的应用要求， 但是它们都是在二 维平面上进行航迹搜索， 因而未能利用地形的高度 信息， 不能有效地利用地形进行地形规避和威胁规 避。本文在基本 RRT 方法的基础上， 提出了一种三 维快速航迹规划方法。并在生成的虚拟数字地形图 上进行了仿真验证， 实验结果表明， 该方法能够充分 利用地形的高程信息， 有效地进行地形回避和威胁 回避， 是一种快速有效的三维航迹规划算法 。
［20 ］
最近的一个树节点， 然后在 q rand 和 q near 的连线上以 如果在 扩展步长 step 为单位截取一个新节点 q new ， 向新节点 q new 行进的过程中没有碰到障碍物， 则将 应 q new 加入到扩展树中， 否则需要重新选择为 q rand 。 继续迭代计算直到 q new 到达目标区域则算法结束， 此时可以在扩展树 T 中找到一条从起点 q start 到目标 点 q goal 的路径。 RRT 算法主要包括 3 个部分： 采样点选择； 搜 索扩展树上距采样点最近的节点； 扩展节点。 下面 将从这 3 个部分， 结合无人机的性能约束和地形高 程数据， 将 RRT 方法引入到无人机三维航迹规划。 2． 1 采样点选择 采样点 q rand 的选择不仅影响航迹质量， 而且影
响规划速度。 三维航迹规划中采样点的随机选取原 则如下： 以概率 p 选择目标点作为采样点； 以概率 1 － p 在整个规划窗口内随机选择采样点 。 对于随机 采样点， 根据采样点的在水平面的位置信息可以查 询到采样点的高程信息。 令随机采样点在水平面的 y rand ） ， 投影坐标为 （ x rand ， 则随机采样点的高程信息 y rand ） 。 为 z（ x rand ， 令无人机的最低飞行高度约束为 H min ， y rand ， 则随机采样点的三维坐标为 q rand = （ x rand ， z（ x rand ， y rand ） + H min ） 。 2． 2 节点扩展 可以找出已存在扩展树 T 通过随机采样点 q rand ， 的树节点 q 中离随机采样点距离最近的一个树节点 q near ， 使得 q rand ） ≤ Dis（ q， q rand ） Dis（ q near ， （2） 计算从 q near 以最小航 在 q near 和 q rand 的连线上， 迹段长度 L 到达的新点 q new q new = q near + L· （ q rand － q near ） （3） ‖q rand － q near ‖ q near 的坐标为 y new ， z new ） ， 的坐标为 （ x new ，
2
基于 RRT 方法的三维航迹规划
ຫໍສະໝຸດ Baidu
基于 RRT 方法的航迹规划以状态空间中的规 划起始点为根节点， 通过随机采样逐渐增加叶节点 的方式生成随机扩展树。当随机树的叶节点中包含 了目标点或者目标区域的点时， 随机树的扩展停止， 便可在随机树中找到一条以根节点组成的从起始点 到目标点的路径。RRT 的扩展方式如图 1 所示。 q rand 表示状 图 1 中 T 表示当前存在的扩展树， q near 表示离随机采样点 q rand 态空间中的随机采样点，
［13 ］ ［11 ］ ［910 ］
1
问题提出
无人机三维在线航迹规划要求无人机根据预先
装载的任务区的三维地形数据， 规划出一条从规划 起始点到目标点的三维航迹， 该航迹避开了所有三 维障碍物和威胁， 并且满足无人机自身的飞行性能
基金项目： 航空科学基金（ 20135184007 ） 资助
等问题。 虽然基本 RRT 方法具有
图1 RRT 算法的扩展过程
（1）
（ x， y， z） 表示无人机在惯性坐标系中的位置 ； 式中， v 表示无人机的速度；ψ 为无人机的航向角；γ 为无 人机的航迹倾角。 具体来说， 三维航迹规划生成的目标航迹要求 满足以下几个约束条件 ： 1 ） 最小航迹段长度：无人机在开始改变飞行姿 态前必须保持直飞的最短距离。这一约束取决于无 人机的机动能力和导航要求。 2 ） 最大拐弯角：无人机只能在小于或等于由自 身水平平面机动性能预先确定的最大拐弯角范围内 转弯。 3 ） 最大爬升、 俯冲角： 无人机只能在小于或等 于由自身垂直平面机动性预先确定的上升和下滑的 最大角度内爬升或俯冲。 4 ） 最大航迹长度：无人机的规划航迹的长度必 须小于或等于一个预先设置的最大距离 ， 它由无人 机所携带的燃料以及指定任务中到达目标的允许飞 行时间决定。 5 ） 最低飞行高度：无人机利用地形进行地形回 避和威胁回避时， 需要尽可能在离地面低的高度上 飞行， 但是飞得过低往往会增加与地面的相撞概率 。 一般在保持离地高度不小于某一指定高度的前提 下， 使飞行高度尽量降低。 只有满足了以上基本约束条件的三维航迹才是 无人机的三维可行航迹。本文将这些约束条件结合 到基本 RRT 算法中， 从而得到一种快速有效的无人 机三维航迹规划方法。
骤如下： 1 ） 以无人机当前位置作为规划起始点 q start ， 初
图3 爬升角约束示意图
3 ） 最大航迹长度 最大航迹长度是无人机的航迹距离约束， 它取 决于无人机的燃料载荷和任务执行时间的限制 ， 记 节点扩展过程中长度大于这个距离的航迹 为 d max 。 认为是无效航迹， 新节点为无效点。 该约束在二维空 通过 间的投影表示如图 4 所示。 新的扩展节点 q new ， 追溯 根 节 点 得 到 从 起 始 点 到 q new 的 航 迹 距 离 为 D（ q new ） ， SL（ q new ） 是从扩展 图中黑色粗实线航迹。 节点 q new 到 目 标 点 q goal 的 欧 氏 距 离。 只有当满足
(
)
（5）
冗余节点剪裁后的节点序列为 φ， φ 初始为空。 令j = n， 1， …j － 首先将目标点 wp j 添加到 φ 中。 对于 i ∈ ［ wp i ） 之间的连线是否存在障碍 1］ ， 循环检查 （ wp j ， 或者威胁， 是否存在不满足无人机自身性能约束的 情况。 如果存在， 则令 i = i + 1 ；否则， 只要检测出第 令 j = i， 并 一个不违背条件的节点 wp i 就停止循环， 重复上述循环， 直到 j = 1 时 将 wp i 添加到 φ 中。 结束。 2． 4 算法描述 基于 RRT 方法的三维航迹规划算法的具体步
D（ q new ） + SL（ q new ） ≤ d max 时， 才能把 q new 加入到已 有的扩展树 T 中。
图4
最大航迹长度约束
2． 3
图2 拐弯角约束示意图
冗余扩展点剪裁 由于 RRT 方法是一种随机方法。 在 RRT 方法 产生的可行航迹中可能会包含冗余的节点 。去除冗 余的航迹节点， 可以缩短无人机的航迹长度， 减少无 人机的机动次数。 设经过 RRT 方法求得的可行路径的节点序列 …， wp n ｝ ， 为 ｛ wp1 ， 记经过 其中 wp n 为目标点位置。
2 ） 最大爬升 / 俯冲角约束 如图 3 所示， 航迹段 q near q new 的坐标为 q near q new = （ x new － x near ， y new － y near ， z new － z near ） 设最大爬升、 俯冲角为 φ， 则 q new 必须满足 | z new － z near | Δγ = asin ≤φ ‖q near q new ‖
· 566·
西
北
工
业
大
学
学
报
第 34 卷
1 ） 最大拐弯角约束 令航迹段 q near － root q near 和 q near q new 的 如图 2 所示， 水平投影分别为 hor1 = （ x near － x near － root ， y near － y near － root ） hor2 = （ x new － x near ， y new － y near ） 设最大允许拐弯角为 θ， 则 q new 必须满足 cosΔψ = hor1 （ hor2 ） T ≥ cosθ ‖hor1 ‖·‖hor2 ‖ （4）