Professional Documents
Culture Documents
1
2022 年 2 月 FLIGHT DYNAMICS Feb. 2022
基于改进人工势场法的无人直升机三维航迹规划
李廷珍, 招启军, 张夏阳, 杜思亮
( 南京航空航天大学 直升机旋翼动力学国家级重点实验室, 江苏 南京 210016)
摘 要:针对民用直升机的复杂贴地环境,开展了预设障碍物环境结合无人直升机飞行性能包线约束的航迹
规划研究。 首先,基于障碍物建模方法设计了航迹规划算法,解决了传统人工势场法较难适用于三维环境下
无人直升机航迹规划的诸多问题,提出了一种改进的人工势场算法并扩展至三维空间。 然后,针对三维环境
中的目标不可达问题,建立了一种基于相对距离判断的斥力势场函数方法;针对局部极小值问题,发展了一
种基于无人直升机飞行性能约束的航迹点回溯法以逃离出局部极小值区域的方法。 最后,通过仿真验证分
析了该算法的有效性。 仿真结果表明:改进的人工势场法能够有效克服传统人工势场法的不足,实现无人直
升机在飞行性能包线约束下的三维航迹规划。
关 键 词:无人直升机; 航迹规划; 人工势场法; 航迹点回溯
中图分类号:V279; V249
文献标识码:A 文章编号:1002-0853(2022)01-0069-07
的性能约束和飞行环境,建立并验证一种改进的
0 引言
无人直升机三维人工势场法,以用于无人直升机
无人直升机是无人机系列的一个重要分支, 的飞行航迹规划。
其悬停 / 垂直起降的功能,在复杂地形测绘、多障
碍运输以及军事侦察等领域内有着广泛的应用价
1 问题描述
值 [1] 。 无人直升机按照其尺寸大小分为:普通型 路径规划算法的主要环节包括环境建模和路
( ≥300 cm) 、小型( 100 ~ 300 cm) 、超小型 ( 15 ~ 径生成:环境建模指对障碍物的表示和自由空间
100 cm) 和微型( ≤15 cm) 。 本文以小型无人直 的表示;路径生成是根据所设计的算法搜索出一
升机为研究对象。 小型无人直升机的飞行特点和 条可行路径。 因此,无人直升机的三维在线航迹
体积优势,使其很适用于城市之间的低空飞行环 规划问题,需要满足最基本的两个条件:( 1) 能够
境。 低空障碍物是无人直升机飞行的重要威胁, 实时规划出一条从起点到终点的安全的飞行路
小型无人直升机需要及时躲避建筑物和未知障碍 径;(2) 符合其自身飞行性能约束。
物,因此必须提升动态环境中的避障能力。 1. 1 小型无人直升机环境建模
航迹规划是无人直升机自主飞行的重要组成 根据安装在小型无人直升机上的雷达或摄像
部分,其任务是在具有障碍物的飞行环境中,根据 头的反馈信息,可以发现障碍物,例如出现在空中
自身约束寻找出一条从起始点到目标点的无碰撞
的飞鸟、气球、其他小型无人直升机、建筑以及山
的安全路径。 目前智能算法主要包括 A ∗ 算法、
丘等,根据探测到的位置和大小信息,及时更新障
启发式搜索算法、专家系统及机器学习等;还有常
碍物信息以避开障碍物。 因此本文假设:
规算法,如 D ∗ 算法、快速扩展随机树法( RRT) 、
(1) 小型无人直升机的飞行区域中所有建筑
人工势场法等 [2] 。
物为标准圆柱体,且横截面半径和高度不同;
近年来,研究者们不断提出新的航迹规划算
(2) 小型无人直升机的飞行区域中所有树、
法,或 者 是 改 进 和 融 合 已 有 的 航 迹 规 划 算 法。
电线杆以及山( 如重庆城市区域) 等为标准圆锥
Cekmez 等 [3] 提出多菌群蚁群算法找到优化路径,
体,且横截面半径和高度不同;
但收敛时间过长。 贝前程等 [4] 通过在斥力函数
(3) 通过雷达测得的未知障碍物为球体,且
中引入到目标点的相对距离约束,使机器人在靠
位置和大小不同。
近目标点时斥力减小。 Arantes 等 [5] 利用启发式
小型无人直升机为直径为 2 m 的球体;圆柱
贪婪算法进行路径搜索,再利用遗传算法进行路
体高度为 10 ~ 100 m,半径为 10 ~ 50 m;球体半径
径优化。 Geva 等 [6] 通过在斥力函数中引入动态
为 1 ~ 10 m;圆锥体高度为 50 ~ 800 m,底面圆半径
斥力因子以解决目标不可达问题。 Sun 等 [7] 通过
为 10 ~ 50 m。
改变斥力函数克服目标不可达问题,并加入动态
1. 2 路径生成
步长以解决局部震荡问题。 李克玉等 [8] 结合模
无人直升机要自主从当前时刻点飞往下一时
糊算法计算出模糊输出力,打破局部最小值处的
二力平 衡 以 解 决 局 部 最 小 陷 阱 问 题。 郭 一 聪 刻点,必须接收两个基本 控 制 指 令: ( 1) 飞 行 方
等 [9] 通过设定启发式子目标点的方法解决局部 向;(2) 飞行速度。 无人直升机根据指定的飞行
最小陷阱问题。 Zhang 等 [10] 利用概率论建 立 了 方向和飞行速度到达下一时刻点,一系列时刻点
一个势场模型,并通过势场模型的解析式和概率 即构成一条路径。 从空间状态来说,飞行方向可
密度函数求出势场的表达式,以解决目标不可达 被视为人工势场法计算出的单位矢量合力,而飞
问题。 行速度可被视为两个时刻点的步长,则下一时刻
目前,小型无人直升机针对建筑物障碍的航 点的位置空间状态可以表示如下:
迹规划研究还较少,且大多仅在二维环境下进行 q i + 1 = af + q i (1)
了仿真实验。 现有的路径规划方法和算法中,传 式中:q i 为无人直升机第 i 个时刻的位置矢量;a
统人工势场法具有原理简单、计算时间短、满足实 为无人直升机两个时刻点位置坐标间的步长; f
时控制等优点,因此本文将深入结合无人直升机 为势场法计算出的单位矢量合力。
第1期 李廷珍,等. 基于改进人工势场法的无人直升机三维航迹规划 71
1. 3 性能约束 为根据不同障碍物优化无人直升机所受到的
综合考虑小型无人直升机的飞行性能,小型 斥力,发展了根据不同障碍物选取不同障碍点计
无人直升机航迹规划主要满足以下约束条件: 算无人直升机到障碍物的相对距离的方法。 不同
(1) 最大爬升角:无人直升机在垂直面内的 障碍点的选取是为了找出距离无人直升机相对最
最大角度限制; 近的点,从而可以更加精准地避障。
(2) 最大转向角约束:无人直升机需在最大 根据前文对建模环境的描述,对于所扫描并
转向角范围内转向; 补形后的球体障碍物,根据当前时刻无人直升机
(3) 最大航程约束:无人直升机携带燃油有 到球心的矢量距离,将球心坐标沿该矢量方向朝
限,所规划的航迹长度应小于等于最大飞行距离。 无人 直 升 机 移 动 2 m ( 直 升 机 的 机 动 作 业 域 大
小) ,其他尺寸不变,形成一个新的虚拟球体障碍
2 人工势场法 物,在该虚拟球体障碍物的表面找到一个无人直
人工 势 场 法 是 由 Khatib 于 1986 年 提 出 升机到障碍点直线距离最短的点。 这样,即使无
的 [11]
,其核心思想是通过引力牵引直升机前往目 人直升机在引力的牵引下距离障碍物太近时,也
标点,同时通过斥力阻止直升机碰撞障碍物,越靠 有一定的安全域可完成相应高难度作业。 同理,
近目标点其所受到引力越大,同时越靠近障碍物 圆锥体障碍物,当无人直升机的位置在圆锥体上
其所受到的斥力越大,即无人直升机在该算法计 方时,圆锥体顶峰点为障碍物点,当无人直升机的
算得到的合力的牵引下成功避障并到达目标点。 位置在圆锥体周围时,其下表面的圆心坐标同理
传统人工势场法在航迹规划的使用中,一直存在 变化形成虚拟圆锥体障碍物;圆柱体同样选择下
着两大问题:目标不可达和局部极小值 [12]
。 常用 表面圆心坐标同理变化,形成虚拟圆柱体障碍物。
的引力函数和斥力函数可以表示如下: 2. 1. 1 球体障碍物
F att = - ( U att ) = - k att d g d gv (2)
Δ
障碍物表面障碍点位置坐标的计算方法如图
F rep = - ( U rep ) 1 所示。 图中:r g 为所定义球体障碍物的半径;Q g
Δ
1 1 1 2
为球体的表面障碍点位置坐标;q 为无人直升机
■|
=■
|k rep ( do
-
R )( ) d
do ov ( d o ≤ R)
(3) 的位置坐标;q g 为目标点的位置坐标;q cg 为球体
■0 ( d o > R)
|
的球心位置坐标。
式中:F rep 和 F att 分别为无人直升机受到的斥力
和引力;U rep 和 U att 分别为无人直升机在当前时
刻受到的斥力势能和引力势能;d o 和 d g 分别为无
人直升机在当前时刻点到障碍物的距离和到目标
点的距离;d ov 和 d gv 分别为无人直升机在当前时
刻点,障碍物指向无人直升机的单位距离矢量和
无人直升机指向目标点的单位距离矢量;k rep 和
k att 分别为斥力势场和引力势场正比例增益因子;
R 为障碍物最大影响距离。
图1 球体障碍物的表面障碍点示意图
2. 1 改进的人工势场法
Fig. 1 Schematic diagram of surface obstacle point
改进的斥力函数可以表示如下 [8] : calculation for spherical obstacles
1 1 1 2 m
■|
|
k rep (
do
-
R )( )
do
d g d ov - 用公式可以表示为:
q - q cg
| Qg = rg + q cg (5)
F rep =■ m
( d1 - R1 ) d
2
m-1
d ov ( d o ≤ R) d g,g
| 2 k rep g
| o 式中:d g,g 为无人直升机到球体球心的距离。
|0 ( d o > R)
■ 2. 1. 2 圆锥体障碍物
(4) 障碍物表面障碍点位置坐标的计算方法如图
式中:m 为斥力势场因子。 2 所示。
72 飞 行 力 学 第 40 卷
用公式可以表示为:
q - q cyq ■
Q cy = r cy ( q z ≥ q z,cyu ) |
+ q cy,new
d cyq |
|
q - q cyq ■ (7)
Q cy = r cy + q cyq ( q z < q z,cyu ) |
d cyq |
q z,cyu = q z,cy + h cy |
■
式中:Q cy 为圆柱体的表面障碍点位置坐标;r cy 和
h cy 分别为圆柱体的底面圆半径和高度;q z,cyu 为圆
柱体上表面圆心位置的 z 坐标值;qcy = (qx,cy ,qy,cy ,
qz,cy )为圆柱体障碍物的下表面圆圆心坐标;qcy,new =
图2 圆锥体障碍物的表面障碍点示意图
Fig. 2 Schematic diagram of surface obstacle point
( q x,cy ,q y,cy ,q z,cyu ) 为圆柱体障碍物的上表面圆心
calculation for cone obstacles 坐标;q cyq = ( q x,cy ,q y,cy ,q z ) 为圆柱中心线上与无人
用公式可以表示为: 机等高的位置点的坐标;d cyq 为圆柱体与无人直
■|qcp (qz ≥ qz,cp ) 升机等高位置到无人机的距离。
|
Qc = ■ q - qcq (6) 目标不可达是指障碍物在目标点附近出现
||rc,new + qc,new (qz < qz,cp ) 时,存在斥力大于引力的情况,这时将导致无人直
■ dcq
其中: 升机无法到达目标点。 针对此问题,需要在斥力
r c,new 大于引力时修正斥力,使无人机能继续在引力的
■ ■
q c,new = || q x,c ,q y,c ,q z,cp - | 作用下到达目标点,同时又不能发生碰撞。 通过
■ tan ( r c h c ) |■
r c,new = [ d cq - ( q z,cp - q z ) tan ( r c h c ) ] × 距离和合力的判断,进入改进斥力公式的子程序,
有且仅当,d o -d g ≥0 以及 e = F rep - F att ≥0,斥
sin2 [ arctan ( rc hc ) ] + (qz,cp - qz )tan ( rc hc )
力可以表示为:
式中:Q c 为圆锥体的表面障碍点位置坐标;q c,new
1 1 1 2
为圆锥体中心线上与圆锥体的表面障碍点等高位
置点的 坐 标; r c,new 为 q c,new 点 到 Q c 点 的 标 量 距
F rep = k rep ( do
-
R )( )
do
- ne (8)
式中:n 为改进斥力函数因子。
离; q z 和 q z,cp 分别为无人机和圆锥体顶点位置的
因为引力从当前点指向目标点,斥力从障碍
z 坐标值;r c 和 h c 分别为圆锥体的底面圆半径和
物点指向当前点,所以当斥力大于引力时,合力反
高度;q c = ( q x,c ,q y,c ,q z,c ) 为圆锥体障碍物的底面
向使其远离目标点,此时根据上述公式可以合理
圆心位置坐标;q cp = ( q x,c ,q y,c ,q z,cp ) 为圆锥体顶点
减小斥力直至到达目标点且不碰撞障碍物。
位置坐标;q cq = (q x,c ,q y,c ,q z ) 为圆锥体中心线上与
2. 2 局部极小值问题
无人直升机等高的位置点的坐标;d cq 为圆锥体上
局部极小值问题是指无人直升机飞到某位置
与无人直升机等高的位置点到无人直升机的距离。
时,其所受到的引力和斥力大小相等,方向相反,
2. 1. 3 圆柱体障碍物
使其误以为已到达终点而停滞不前,即因为合力
障碍物表面障碍点的计算方法如图 3 所示。
为零而不知道下一时刻的飞行方向,出现无法到
达目标点的现象 [13] 。 当处于局部极小值区域时,
并不是停在一个位置,而是在一部分区域内徘徊。
有且仅当,d g > 0 以及 F rep - F att = 0,陷入局部
最小陷阱,无人直升机的位置被视为陷阱点。
建立一个产生下一时刻合力矢量的部分虚拟
球:以无人直升机的某一时刻为其球心,以该球心
图3 圆柱体障碍物的表面障碍点示意图
点到陷阱点的距离并乘以合力的缩小系数作为其
Fig. 3 Schematic diagram of surface obstacle point 半径;其在无人直升机所处水平面的扇面大小,由
calculation for cylindrical obstacles 无人直升机最大转向角 φ max 决定,即根据当前时
第1期 李廷珍,等. 基于改进人工势场法的无人直升机三维航迹规划 73
刻点本来的合力矢量在水平面的投影,绕球心逆
时针旋转角度 φ max 以及顺时针旋转角度 φ max ;其
在无人直升机所处垂直面的扇面大小,由无人直
升机最大爬升角 θ max 决定,即根据当前时刻点本来
的合力矢量在垂直面的投影,向上旋转角度 θ max 。
采用航迹点回溯法确定部分虚拟球的位置,
确定该部分虚拟球安全的准则是:在球面离散出
的 N 个点到障碍物的相对距离大于等于所设定
的防碰撞安全距离;否则以此时刻的上一时刻点 图5 单个圆柱体障碍物飞行航迹
开始,倒序选择存储好的无人直升机的飞行路径 Fig. 5 Single cylinder obstacle flight path
时刻点,直至产生安全部分虚拟球。 离散安全部
分虚拟球表面的 N 个点则存在 N - 1 个新的合力
矢量( 排除掉该时刻点本来的合力矢量) ,根据最
大航程约束决策出相对最优的合力矢量。
3 仿真验证与分析
本文在 python 仿真环境中进行仿真,首先利
用改进人工势场法对单个障碍物模型、多个障碍
物模型、随机障碍物模型进行仿真,验证不同障碍 图6 单个圆锥体障碍物飞行航迹
物采用不同相对距离的计算方法规划路径;然后 Fig. 6 Single cone obstacle flight path
对单个障碍物模型进行仿真,验证含相对距离判
断的改进斥力函数的计算方法规划路径;最后验
证启发式合力逃离出局部极小值区域的有效性并
与传统人工势场算法进行比较。
3. 1 不同障碍物的仿真验证
根据上文假设,可知障碍物模型主要有圆锥
体( 树、山等) 、球体( 飞鸟、气球等) 及圆柱体( 所
有建筑) 。 为了保证传统人工势场算法能够获得
最优性能,郭一聪等[9] 对传统算法采用最佳参数 图7 不同障碍物飞行航迹
进行仿真,给出了仿真设定参数为:k att = 8,k rep = 20, Fig. 7 Different obstacles flight path
R = 6 m。 本文采用其所设定的势场参数并设置: 由图 4 ~ 图 7 可以看出,本文所采用的不同相
m = 0. 7,n = 1. 1 进行仿真实验,图 4 ~ 图 7 分别为无 对距离计算方法规划的飞行路径,可以使无人直
人直升机遇到不同障碍物的飞行航迹仿真结果。 升机有效避障且达到目标点,对于圆锥和圆柱障
碍物均可以从顶部或侧面规避,规划路径平滑,适
合无人直升机在城市的飞行环境中飞行。
3. 2 传统及改进人工势场算法仿真验证
设定仿真参数:k att = 8,k rep = 20,R = 6 m,m =
0. 7,n = 1. 1。 传统人工势场算法和改进算法仿真
结果如图 8 和图 9 所示。
由图 8 和图 9 可看出,改进的人工势场算法能
到达目标点位置坐标,而传统人工势场算法在目标
图4 单个球体障碍物飞行航迹 点附近停滞,不能到达目标点位置坐标,这表明本
Fig. 4 Single sphere obstacle flight path 文所提出的改进算法可以解决目标不可达问题。
74 飞 行 力 学 第 40 卷
图 10 陷入局部极小值区域
图8 目标不可达示意图
Fig. 10 Trapped in the local minimum region
Fig. 8 Schematic diagram of unreachable target
图9 目标可达示意图 图 11 逃离出局部极小值区域
Fig. 9 Schematic diagram of reachable target Fig. 11 Escape from the local minimum region
3. 3 逃离局部极小值区域仿真验证 由图 10 和图 11 可看出,改进的人工势场法
针对传统人工势场法容易陷入局部最小陷阱 可以使无人直升机跳出局部最小值区域,而传统
的问题,将传统算法与改进算法进行了对比。 设 人工势场算法会在局部最小区域来回震荡,无法
定仿真参数:k att = 8,k rep = 20,R = 6 m,n = 0. 7。 传 逃脱,这表明本文所提出的基于无人直升机飞行
统算法和改进算法的仿真对比结果如图 10 和图 性能约束的航迹点回溯法的改进算法可以拖拽无
11 所示。 人直升机逃离出局部最小陷阱。
第1期 李廷珍,等. 基于改进人工势场法的无人直升机三维航迹规划 75