考虑安全飞行通道约束的无人机飞行轨迹多目标优化策略

doi:10.16183/j.cnki.jsjtu.2021.154

考虑安全飞行通道约束的无人机飞行轨迹多目标优化策略

黄宇昊¹, 韩超¹, 赵明辉², 杜乾坤¹, 王石刚^,¹

1.上海交通大学机械与动力工程学院,上海 200240

2.中煤科工集团上海有限公司,上海 201100

Multi-Objective Optimization Strategy of Trajectory Planning for Unmanned Aerial Vehicles Considering Constraints of Safe Flight Corridors

HUANG Yuhao¹, HAN Chao¹, ZHAO Minghui², DU Qiankun¹, WANG Shigang^,¹

1. School of Mechanical Engineering, Shanghai Jiao Tong University, Shanghai 200240, China

2. Shanghai Co., Ltd., China Coal Technology and Engineering Group, Shanghai 201100, China

通讯作者: 王石刚,男,教授,博士生导师,电话(Tel.):13341691161;E-mail:wangshigang@sjtu.edu.cn.

责任编辑: 石易文

收稿日期: 2021-05-6

Received: 2021-05-6

作者简介 About authors

黄宇昊(1998-),男,江西省抚州市人,硕士生,主要从事无人机自主导航技术研究.

摘要

针对无人机在复杂环境下难以规划出兼顾平滑性和安全性等指标的时域连续轨迹问题,基于安全飞行通道提出了一种多目标轨迹规划算法.在基于快速拓展随机树(RRT)改进的RRT^*算法生成的初始离散路径点基础上,建立以凸多面体集合表示的安全飞行通道;根据轨迹在安全飞行通道内部的约束建立安全项目标函数,结合飞行平滑性、动力学特性、飞行时间等性能指标,建立加权多目标优化函数;采用基于梯度下降的凸优化算法,对离散路径点的位置、速度、加速度及轨迹的时间分配进行优化,生成分段多项式表示的时域连续轨迹.基于煤矿井下等复杂环境对算法的有效性及相关性能进行试验及对比验证,结果表明:相比现有算法,本文算法在综合性能上有一定的提升.

关键词： 无人机; 轨迹规划; 安全飞行通道; 凸优化

Abstract

Aimed at the problem of generating a smooth, safe, and dynamically feasible continuous-time trajectory for unmanned aerial vehicles (UAV) in complex environments, a trajectory planning algorithm is proposed to minimize a multi-objective function based on safe flight corridors. The safe flight corridor represented by a collection of convex polyhedra is built based on the initial discrete waypoints generated by the improved rapidly-exploring random tree(RRT), namely the RRT^* algorithm. The safety objective function is established according to the constraints of limiting the trajectory inside safe flight corridors. In combination with the flight smoothness, dynamic characteristics, and time performance, a multi-objective function is built. The gradient-based convex optimization algorithm is used to derive the continuous-time trajectory expressed as a piece-wise polynomial by optimizing the position, velocity, acceleration of waypoints, and time allocation. The effectiveness and performance of the proposed algorithm is tested and compared under complex environments such as the coal mine. The test results demonstrate that the proposed algorithm has a better comprehensive performance in comparison with existing algorithms.

Keywords： unmanned aerial vehicle (UAV); trajectory planning; safe flight corridor; convex optimization

PDF (5684KB) 元数据多维度评价相关文章导出 EndNote| Ris| Bibtex 收藏本文

本文引用格式

黄宇昊, 韩超, 赵明辉, 杜乾坤, 王石刚. 考虑安全飞行通道约束的无人机飞行轨迹多目标优化策略[J]. 上海交通大学学报, 2022, 56(8): 1024-1033 doi:10.16183/j.cnki.jsjtu.2021.154

HUANG Yuhao, HAN Chao, ZHAO Minghui, DU Qiankun, WANG Shigang. Multi-Objective Optimization Strategy of Trajectory Planning for Unmanned Aerial Vehicles Considering Constraints of Safe Flight Corridors[J]. Journal of Shanghai Jiaotong University, 2022, 56(8): 1024-1033 doi:10.16183/j.cnki.jsjtu.2021.154

无人机具备灵活多变、飞行速度快及检测范围广等特点,正被大规模运用于军事和民用领域.因此,无人机在复杂环境的轨迹规划问题受到国内外学者的广泛关注^[1⇓-3].尤其在如煤矿井下等复杂环境中,如何对安全飞行通道以及飞行轨迹进行优化,确保飞行安全性,更具有广泛的研究价值.

无人机巡检轨迹规划一般可分为初始路径规划和轨迹优化两部分.初始路径规划是指针对飞行环境,找到一条从初始位置到目标位置的无碰撞离散路径,常用的方法有随机树搜索^[4]、A^*算法^[5]、强化学习^[6]等.由离散路径点生成时域连续轨迹的过程存在碰撞风险,一般在初始路径基础上建立安全飞行通道,通过约束生成轨迹在通道内部以保障安全性.文献[7]使用立方体实现安全飞行通道的快速构建,但通道的体积受限,可用于轨迹规划的空闲区域较小.文献[8]采用凸多面体表示安全飞行通道,在最大化通道体积和计算效率之间实现了较好的平衡,但得到的通道在路径端点处间隙较小,复杂环境下性能较差.轨迹优化是指在初始路径规划得到的离散路径点基础上,生成一条时域连续的飞行轨迹.常采用的时域连续轨迹表示方式包括B样条^[9]、Bezier曲线^[10]、多项式曲线^[11]等.为使生成的巡检轨迹具备更好的综合性能,一般还需结合无人机的动力学特性对轨迹进行优化.文献[12]采用Bezier曲线拟合离散路径点,并对轨迹长度和能量消耗等目标函数进行优化.文献[13]在满足动力学可行性和安全性约束的条件下,对最小化轨迹耗时进行了研究.常用于轨迹优化的性能指标还包括轨迹的高阶导数项^[14]、安全性^[15]、动力学可行性^[16]等.

对于复杂环境下的无人机轨迹规划,需要考量轨迹平滑性、安全性、动力学特性等多种因素,上述文献大多对少数性能指标进行优化,难以获得综合性能优异的轨迹.例如,对平滑性进行优化会使轨迹的总耗时增长,对长度进行优化使得轨迹中的尖锐拐点增多、动力学性能变差.且现有安全飞行通道生成算法在路径中各处障碍物间隙相差较大,不利于保障轨迹在狭窄环境下的安全性.针对该类情况,本文基于安全飞行通道提出了一种多目标轨迹规划算法,对安全飞行通道生成过程中半平面法向量的调整优化了算法在复杂环境下的工作效果,并采用多目标函数对无人机轨迹的常用性能指标进行建模,通过对加权目标函数进行优化使轨迹具备更好的综合性能.首先,采用基于快速拓展随机树(RRT)改进的RRT^*算法生成初始离散路径点,通过在最近障碍物处多次迭代生成半平面,得到凸多面体形式的安全飞行通道.之后建立包含时间项、安全项、平滑项、动力学项的加权目标函数,采用基于梯度下降的凸优化算法求解轨迹的时间分配和路径端点处的位置、速度、加速度,生成分段多项式轨迹.最后,通过多种场景下的仿真实验及与现有算法的对比分析,验证了所提算法的有效性.

1 初始路径规划

环境的地图信息一般可通过激光雷达、深度相机等传感器以三维点云格式获取,通过设置一定的空间分辨率可将点云地图转化为以八叉树数据结构存储的占据栅格地图Octomap^[17],每个树节点均表示一个空间立方体,可实现对点云地图的压缩,本文采用该类地图进行路径规划.首先,在栅格地图基础上,使用RRT^*算法生成初始离散路径点p₀, p₁, …, p_n.每两个相邻路径点依次连接,可得到初始路径P={p₀→p₁→…→p_n},其中p_i→p_i₊₁表示起点为p_i,终点为p_i₊₁的线段L_i,在L_i基础上计算局部安全飞行通道C_i,在通道内部可规划得到分段多项式轨迹Φ_i.无人机轨迹规划得到的分段多项式轨迹如图1所示.

图1

新窗口打开| 下载原图ZIP| 生成PPT

图1 分段多项式轨迹

Fig.1 Piece-wise polynomial trajectories

1.1 离散路径点生成

RRT^*算法在基于节点采样的RRT算法基础上添加了随机几何图及剪枝优化步骤,使得生成树上的节点能够收敛到渐进最优值.传统RRT^*算法在剪枝优化时采用的代价函数为路径长度,可使生成路径最短.在如煤矿井下等复杂环境中,路径最短优化指标可能会使初始路径靠近障碍物,增大无人机飞行时的碰撞风险.针对该问题,本文采用综合路径长度和障碍物间隙的加权代价函数进行剪枝优化,以保障飞行过程无人机的安全性.

搜索树中一系列路径节点p₀,p₁,…,p_n的加权代价函数可表示为

(1)

J_{R R T^{*}}

=w_l

J_{R R T^{*}, l}

+w_o

J_{R R T^{*}, o}

= w_l

\overset{n - 1}{\sum_{i = 0}}

dis(p_i, p_i₊₁)+ w_o

\overset{n}{\sum_{i = 0}} \frac{1}{(o b s (p_{i}) - l_{0})^{2}}

式中: $J_{R R T^{*}, l}$ 为路径长度代价函数; $J_{R R T^{*}, o}$ 为障碍物间隙代价函数;w_l为路径长度代价函数权重; w_o为障碍物间隙代价函数权重;dis(p_i, p_i₊₁)为节点p_i与节点p_i₊₁之间的距离;obs(p_i)为节点p_i处的障碍物间隙;l₀为路径规划时的安全距离.

改进后RRT^*算法原理如图2(a)所示,其中:节点x_new为新加入节点;x_nea为初始父节点;x_ini为根节点;x_g为搜索过程中生成的节点,g=0,1,…,5; 节点中的数值为障碍物间隙对应的代价函数的值,边上数值为路径长度对应的代价函数.对于x_new,RRT^*算法会在其邻域内重新选择总代价最小的路径作为父节点.若只考虑路径长度,则所选路径为x_ini→x₂→x_new(见图2(b)中蓝线).采用加权代价函数,所选路径为x_ini→x₁→x₃→x_new(见图2(b)中红线).之后的重新布线步骤与该过程类似,相比于传统RRT^*算法,采用加权代价函数可使生成路径点处障碍物间隙增大,为之后的轨迹规划提供更好的初始条件.

图2

新窗口打开| 下载原图ZIP| 生成PPT

图2 RRT^*算法重选父节点过程

Fig.2 Rewiring operations of RRT^* algorithm

1.2 安全飞行通道构建

无人机轨迹在离散路径点基础上生成,轨迹优化过程中会改变路径点所在位置.为保障路径点的安全性,需在离散路径点基础上计算由凸多面体集合组成的安全飞行通道.安全飞行通道内部无障碍物点,代表栅格地图中的空闲区域集合,轨迹规划时可用于快速计算任意位置的障碍物间隙及其对路径点三维坐标的梯度.对于分段线性路径L_i,该处由凸多面体表示的局部安全飞行通道可表示为

(2)C_i=

\cap_{h = 1}^{m_{i}}

H_h

式中:m_i为构成第i个局部安全飞行通道所需的半平面个数;H_h为构成凸多面体的第h个半平面,可表示为

(3)H_h={s|n_h·(s-z_h)>0}

式中:s为半平面内一点;n_h为第h个半平面的法向量;z_h为第h个半平面上某点.对于分段线性路径L_i经轨迹规划后得到的多项式轨迹Φ_i,为保障巡检轨迹的安全性,其上任意一点s,应满足该点到局部安全飞行通道C_i的距离dis(s, C_i)>l₀,dis(s, C_i)可表示为

(4)dis(s, C_i)=min(n_h·(s-z_h))

h=1, 2, …, m_i

安全飞行通道的构建方法如图3所示,其中:o_j为第j(j=0, 1, 2, 3)次迭代时最近障碍物的位置;s_i_,_j为第i段路径上距o_j最近的点;l₁、l'₁为前两次迭代时的对应障碍物间隙;H'₁为H₁经法向量调整后生成的半平面.对于分段线性路径L_i,寻找距离L_i最近的障碍物位置o₀及L_i中距离o₀最近的点s_i_,0,构建半平面H₀={s|(s_i_,0-o₀)·(s-o₀)>0},将位于半平面H₀外部的障碍物排除.重复该过程,直至半平面的交集内无障碍物为止,即可得到局部安全通道.为保障算法的时间性能,采用K-d树存储占据栅格地图的节点,可实现最近障碍物位置的快速查找,并设置一个最大包围盒,只对包围盒内的障碍物信息进行迭代处理.

图3

新窗口打开| 下载原图ZIP| 生成PPT

图3 安全通道构造

Fig.3 Construction of safe flight corridor

以H₁为例,采用法向量n_j=s_i_,_j-o_j构建半平面会压缩分段路径中其余位置的可规划区域.调整该处对应半平面为H'₁可增大分段路径的平均障碍物间隙,即局部安全通道C_i的体积,但H'₁与分段路径的最小间隙l'₁减小.均衡考虑平均障碍物间隙和最小障碍物间隙对轨迹规划的影响,H'₁的法向量n'应优化如下目标函数:

(5)J_H=

\frac{1}{[n' \cdot (p_{i} - o_{j}) - l_{0}]^{2}}

\frac{1}{[n' \cdot (s_{i, j} - o_{j}) - l_{0}]^{2}}

\frac{1}{[n' \cdot (p_{i + 1} - o_{j}) - l_{0}]^{2}}

调整半平面为H'₁可能使排除的障碍物节点减少,如o₂及o₃处,不利于安全飞行通道的构建.故仅当新加入半平面H_j使得平均障碍物间隙减小量达到一定阈值时进行半平面调整工作.安全飞行通道构建完成后,任意位置处的障碍物间隙可近似为该段轨迹对应凸多面体中半平面的最小距离,障碍物间隙对路径点三维坐标的梯度向量可近似为使距离取最小处的半平面法向量.

2 飞行轨迹优化

2.1 问题描述

无人机轨迹规划需将初始路径中的离散路径点转化为时域连续轨迹,本文采用分段多项式函数描述轨迹:

(6)Φ(t)=

\{\begin{array}{l} Φ_{0} (t - t_{0}) = \overset{N}{\sum_{q = 0}} η_{0, q} (t - t_{0})^{q} \\ t_{0} \leq t < t_{1} \\ Φ_{1} (t - t_{1}) = \overset{N}{\sum_{q = 0}} η_{1, q} (t - t_{1})^{q} \\ t_{1} \leq t < t_{2} \\ ︙ \\ Φ_{n - 1} (t - t_{n - 1}) = \overset{N}{\sum_{q = 0}} η_{n - 1, q} (t - t_{n - 1})^{q} \\ t_{n - 1} \leq t < t_{n} \end{array}

式中:N为多项式轨迹的阶数;η_i_,_q为第i段轨迹中第q次多项式系数;t_i为离散路径点p_i处的时间戳.记Δt_i=t_i₊₁-t_i为第i段轨迹分配的时间,轨迹连续性约束条件可表示为

(7)

\frac{d^{k}}{d t^{k}}

Φ_i(Δt_i)=

\frac{d^{k}}{d t^{k}}

Φ_i₊₁(Δt_i₊₁), k=0, 1, 2

轨迹规划问题可描述为选取合适的分段轨迹多项式系数η_i_,_q及分配时间Δt_i,优化如下目标函数:

(8)J_opt=w_sJ_s+w_cJ_c+w_dJ_d+w_tJ_t

式中:J_s为平滑项目标函数;J_c为安全项目标函数;J_d为动力学项目标函数;J_t为时间项目标函数;w_s、w_c、w_d、w_t分别为其对应目标函数的权重系数.

2.2 目标函数

为使规划轨迹更为平滑,轨迹的高阶导数项在时域上的积分常被用为平滑项目标函数.本文选用轨迹三阶导数项的积分,则J_s可表示为

(9)J_s=

\overset{n - 1}{\sum_{i = 0}} \int_{0}^{Δ t_{i}} {‖\frac{d^{3}}{d t^{3}} Φ_{i} (t)‖}^{2}

记分段轨迹系数向量η_i=[η_i_,0η_i_,1 … η_i_,_N]^T,η为η_i组成的列向量,则有:

(10)J_s=η^TQη=

{[\begin{array}{l} η_{0} \\ η_{1} \\ ︙ \\ η_{n - 1} \end{array}]}^{T}

[\begin{array}{l} Q_{0} (Δ t_{0}) \\ Q_{1} (Δ t_{1}) \\ ⋱ \\ Q_{n - 1} (Δ t_{n - 1}) \end{array}] [\begin{array}{l} η_{0} \\ η_{1} \\ ︙ \\ η_{n - 1} \end{array}]

式中:Q_i(Δt_i)为第i段轨迹的Hessian矩阵,由式(8)中轨迹导数项时域积分得到.记e为分段轨迹所有端点处的位置、速度、加速度组成的列向量,e_F为e中的固定量,即起点和终点处的位置、速度、加速度组成的列向量,e_P为e中的自由变量,即除起点和终点外其余离散路径点处的位置、速度、加速度组成的列向量,则有:

e= ${[\begin{array}{l} e_{0} & e_{1} & \dots & e_{n - 1} \end{array}]}^{T}$

e_i=[Φ_i(t_i) Φ'_i(t_i) Φ″_i(t_i) Φ_i(t_i₊₁)

(11)Φ'_i(t_i₊₁) Φ″ _i(t_i₊₁)]^T

e_F=[Φ(t₀) Φ'(t₀) Φ″(t₀) Φ(t_n)

(12)Φ'(t_n) Φ″(t _n)]^T

e_P=[Φ(t₁) Φ'(t₁) Φ″(t₁) … Φ(t_n_-1)

(13)Φ'(t_n_-1) Φ″(t _n_-1)]^T

采用文献[11]中介绍的闭式求解方法,连续性约束条件可转化为

(14)η=M^-1e=M^-1S

[\begin{array}{l} e_{F} \\ e_{P} \end{array}]

式中:M为映射矩阵,将分段轨迹端点处的位置、速度、加速度映射为轨迹多项式系数组成的向量,与分段轨迹分配的时间Δt_i有关;S为常量选择矩阵,用于消除e中的重复变量并分离其中的固定量和自由变量.M为分块对角矩阵,可写成如下形式:

(15)M=

[\begin{array}{l} M_{0} \\ M_{1} \\ ⋱ \\ M_{n - 1} \end{array}]

, M_iη_i=e_i

根据文献[11]中的实验测试可知,该类闭式求解方法可在轨迹段数大于50时获得稳定的数值解,满足本文轨迹优化的应用场景.将式(14)带入式(10)中,可得:

(16)J_s=

{[\begin{array}{l} e_{F} \\ e_{P} \end{array}]}^{T}

S^TM^-TQM^-1S

[\begin{array}{l} e_{F} \\ e_{P} \end{array}]

为保障生成轨迹的安全性,将安全飞行通道对轨迹的约束dis(s, C_i)>l₀转化为安全项目标函数:

(17)J_c=∫_Sc(Φ(t))ds=

\int_{t_{0}}^{t_{n}}

c(Φ(t))‖v(t)‖dt=

\overset{(t_{n} - t_{0}) / T_{S}}{\sum_{k = 0}}

c(Φ(t_k))‖v(t_k)‖T_S

式中:t_k=t₀+kT_S; T_S为时域离散采样间隔;v(t_k)为t_k时刻无人机的速度矢量;c(Φ(t_k))为Φ(t_k)处的障碍物间隙惩罚项.针对轨迹中任意一点p,设计如下障碍物间隙惩罚项^[16]:

(18)c(p)=αexp[-(obs(p)-l₀)/r]

式中:α控制惩罚项函数的幅度;r控制惩罚项函数变化的幅度.对于第i段轨迹上的点p,obs(p)可近似为dis(p, C_i),减少因大规模栅格地图数据处理带来的时间开销.

动力学目标函数J_d的目的是防止优化后轨迹在某时刻的速度、加速度超过无人机的许用最大速度、加速度.J_d包含速度项J_dv与加速度项J_da:

(19)J_d=J_dv+J_da=

\overset{(t_{n} - t_{0}) / T_{S}}{\sum_{k = 0}}

c_v(‖v(t_k)‖)‖v(t_k)‖T_S+

\overset{(t_{n} - t_{0}) / T_{S}}{\sum_{k = 0}}

c_a(‖a(t_k)‖)‖v(t_k)‖T_S

式中:a(t_k)为t_k时刻无人机的加速度矢量;c_v(‖v(t_k)‖)和c_a(‖a(t_k)‖)分别为速度惩罚项及加速度惩罚项,采用式(18)类似的指数形式,不再赘述.

将分段轨迹耗时Δt_i作为自由变量进行优化可防止不合理时间分配导致的轨迹打结、存在尖锐拐点等现象的发生.但平滑项目标函数及动力学目标函数的迭代优化过程会使得轨迹分配时间不断增大,导致轨迹总耗时过长,不满足轨迹规划的要求.选取如下时间项目标函数作为惩罚项,可获得更为合理的分段轨迹时间分配:

(20)J_t=

\overset{n - 1}{\sum_{i = 0}}

t_{i}^{2}

2.3 凸优化

本文采用基于梯度下降的凸优化算法进行轨迹优化.对于选取的多个目标函数,分别计算其相对于自由变量的Jacobian行列式.为方便后续计算过程,选取e_P和Δt_i作为优化变量,通过式(14)可间接求出轨迹系数,得到优化后轨迹.

记R=S^TM^-TQM^-1S,由式(16)可得:

(21)J_s=

{[\begin{array}{l} e_{F} \\ e_{P} \end{array}]}^{T} [\begin{array}{l} R_{F F} & R_{F P} \\ R_{P F} & R_{P P} \end{array}] [\begin{array}{l} e_{F} \\ e_{P} \end{array}]

e_{F}^{T}

R_FFe_F+

e_{F}^{T}

R_FPe_P+

e_{P}^{T}

R_PFe_F+

e_{P}^{T}

R_PPe_P

式中:R_FF、R_FP、R_PF和R_PP分别为R左上方、右上方、左下方和右下方的分块矩阵.

对上式求偏导,可得:

(22)

\frac{\partial J_{s}}{\partial e_{P}}

e_{F}^{T}

R_FP+2

e_{P}^{T}

R_PP

记K=S $[\begin{array}{l} e_{F} \\ e_{P} \end{array}]$ ,则有:

(23)

\frac{\partial J_{s}}{\partial Δ t_{i}}

=K^T

{(\frac{\partial M^{- 1}}{\partial Δ t_{i}})}^{T}

QM^-1K+K^TM^-T

\frac{\partial Q}{\partial Δ t_{i}}

M^-1K+K^TM^-TQ

\frac{\partial M^{- 1}}{\partial Δ t_{i}}

式中: $\frac{\partial M^{- 1}}{\partial Δ t_{i}}$ =-M^-1 $\frac{\partial M}{\partial Δ t_{i}}$ M^-1.矩阵M与Q均为分块对角矩阵,其对Δt_i的偏导数可写成如下形式:

(24)

\frac{\partial M}{\partial Δ t_{i}}

[\begin{array}{l} 0 \\ ⋱ \\ \frac{\partial M (Δ t_{i})}{\partial Δ t_{i}} \\ ⋱ \\ 0 \end{array}]

(25)

\frac{\partial Q}{\partial Δ t_{i}}

[\begin{array}{l} 0 \\ ⋱ \\ \frac{\partial Q (Δ t_{i})}{\partial Δ t_{i}} \\ ⋱ \\ 0 \end{array}]

在确定式(14)中的映射关系及分段多项式轨迹的阶数后, $\frac{\partial M_{i} (Δ t_{i})}{Δ t_{i}}$ 与 $\frac{\partial Q_{i} (Δ t_{i})}{Δ t_{i}}$ 均可较易获得.

记e_P=[ $e_{P_{x}}$ , $e_{P_{y}}$ , $e_{P_{z}}$ ], μ为x,y,z这3个坐标轴分量,则有:

(26)

\frac{\partial J_{c}}{\partial e_{P_{μ}}}

\overset{(t_{n} - t_{0}) / T_{s}}{\sum_{k = 0}}

(v(t_k)

\frac{\partial c}{\partial o b s (Φ)}

n_μtN_PP+ c

\frac{v_{k} (t_{k})}{‖v (t_{k}) ‖}

tVN_PP)T_S

式中:n_μ为局部安全飞行通道中距离当前位置最近的半平面的法向量;t=[ $t_{k}^{0}$ $t_{k}^{1}$ … $t_{k}^{n}$ ]为时间向量;N=M^-1S,N_PP为N右下方的分块矩阵;V为将位置对应的多项式系数映射到速度对应的多项式系数; c为式(18)所述的障碍物间隙惩罚项函数.安全项目标函数Δt_i对的偏导数如下:

(27)

\frac{\partial J_{c}}{\partial Δ t_{i}}

\overset{(t_{n} - t_{0}) / T_{S}}{\sum_{k = 0}}

(v(t_k)‖

\frac{\partial c}{\partial o b s (Φ)}

n_jt

\frac{\partial M^{- 1}}{\partial Δ t_{i}}

K+c

\frac{v (t_{k})}{‖v (t_{k}) ‖}

\frac{\partial M^{- 1}}{\partial Δ t_{i}}

K)T_S

动力学目标函数J_d对自由变量的梯度推导与安全项目标函数类似,不再赘述.对于时间项目标函数, $\frac{\partial J_{t}}{\partial e_{P}}$ =0, $\frac{\partial J_{t}}{\partial Δ t_{i}}$ =2Δt_i.采用如下初值作为迭代优化过程的初始条件:

(28)

e_{P}^{*}

R_{P P}^{- 1} R_{F P}^{T} e_{F}^{*}

(29)Δ

t_{i}^{0}

\frac{d i s (p_{i}, p_{i + 1})}{\bar{v}}

式中: $\bar{v}$ 为预设的轨迹平均速度; $e_{F}^{*}$ 将初始路径中所有离散路径点的位置视为固定量; $e_{P}^{*}$ 为除起点和终点外所有路径端点处的速度、加速度,由 $e_{F}^{*}$ 和 $e_{P}^{*}$ 即可得到 $e_{P}^{0}$ .在确定初始条件及Jacobian行列式的计算公式后,采用文献[18]中介绍的凸优化算法进行轨迹优化,将优化后的e_P和Δt_i带入式(14)中即可得到分段多项式的轨迹系数及分配时间,生成所需的时域连续飞行轨迹.

3 仿真实验与分析

3.1 离散路径点生成算法仿真实验

本文中仿真实验采用CPU为R5-3600,内存为16 GB的主机进行,由C++编写实现,并通过机器人操作系统(ROS)下的Rviz软件进行可视化.通过随机生成的三维栅格地图对离散路径点生成算法进行仿真测试,并将选用加权代价函数和路径长度代价函数剪枝优化后的RRT^*算法规划结果进行对比,随机地图大小为20 m×20 m×4 m,以左下角为坐标系原点,起点设置为(2 m,2 m,2 m),终点设置为(18 m,18 m,2 m),无人机的安全距离为0.45 m. $J_{R R T^{*}, o}$ 在路径点靠近障碍物时数值远大于 $J_{R R T^{*}, l}$ ,为防止 $J_{R R T^{*}, o}$ 占主导地位导致生成路径过长,选取路径长度代价函数权重w_l为15,障碍物间隙代价函数权重w_o为1.两种情况下离散路径点构成的初始分段线性路径如图4所示.其中:蓝色路径为优化加权代价函数的结果;红色路径为优化路径长度的结果,采用加权代价函数可使生成路径远离障碍物.

图4

新窗口打开| 下载原图ZIP| 生成PPT

图4 RRT^*算法生成的初始路径

Fig.4 Initial path generated by RRT^* algorithm

上述两种代价函数生成的初始路径的总长度及平均障碍物间隙随障碍物密度的变化如图5所示,其中:ρ为障碍物密度, $\bar{o}$ 为平均障碍物间隙.由图5可知,采用加权代价函数在路径总长度增加约6.6%的情况下,使生成路径点处的平均障碍物间隙增大约22.0%,为后续轨迹优化过程提供了一个更好的初始条件.

图5

新窗口打开| 下载原图ZIP| 生成PPT

图5 两种代价函数生成的初始路径对比

Fig.5 Comparison of initial path generated by two optimization methods

3.2 安全飞行通道生成算法仿真实验

为验证安全飞行通道生成算法的有效性,在随机生成的三维栅格地图中进行测试,并与现有算法中较为常用且实时性较好的SFC算法^[8](一种安全飞行通道算法)进行对比,如图6所示.随机地图及起点、终点的设置与3.1节相同,先采用1.1节中介绍的RRT^*算法生成初始离散路径点,在此基础上分别采用本文算法及SFC算法计算凸多面体集合组成的安全飞行通道.其中,最大包围盒设置为距分段路径的间隙为安全距离的2倍,当新加入半平面使得当前平均障碍物间隙减小50%时重新调整该处的法向量方向,该类参数选取可以较好地平衡算法运行时间与安全飞行通道的综合性能.

图6

新窗口打开| 下载原图ZIP| 生成PPT

图6 障碍物密度为0.08时两种算法构建的安全飞行通道

Fig.6 Comparison of SFC and proposed algorithms at an obstacle density of 0.08

改变随机地图中障碍物密度,测试多组数据,算法对比结果如表1所示,其中:t_u为算法耗时;o_min为最小障碍物间隙.相比于SFC算法,本文提出的算法在 $\bar{o}$ 上略有下降,约2.2%,但最小障碍物间隙o_min有较大提升,约21.2%.对于狭窄区域较多的复杂环境, o_min决定无人机是否能够安全通过,生成可行轨迹.本文算法在牺牲少量平均障碍物间隙指标的前提下,增大了路径的最小障碍物间隙,提高了无人机在复杂环境中轨迹规划的可能性.另外,本文算法耗时缩短约16.0%,对于计算资源有限且实时性要求较高的无人机更为适用.

表1 不同障碍物密度下SFC与所提算法的性能对比

Tab.1 Comparison of SFC and proposed algorithms at different obstacle densities

ρ	本文算法				SFC算法
ρ	$\bar{o}$ /m	o_min/m	$\frac{o_{m i n}}{\bar{o}}$	t_u/s	$\bar{o}$ /m	o_min/m	$\frac{o_{m i n}}{\bar{o}}$	t_u/s
0.02	1.286	0.510	0.397	1.137	1.302	0.473	0.363	1.190
0.04	1.048	0.555	0.530	1.214	1.069	0.454	0.425	1.320
0.06	1.059	0.528	0.499	1.540	1.085	0.446	0.411	1.756
0.08	0.837	0.461	0.551	1.912	0.867	0.342	0.394	2.665
0.10	0.781	0.462	0.592	1.994	0.802	0.359	0.448	2.355
平均值	1.002	0.503	0.513	1.559	1.025	0.415	0.408	1.857

新窗口打开| 下载CSV

利用安全飞行通道可快速计算任意位置的障碍物间隙及其对三维坐标的梯度.障碍物密度为0.08时,采用安全飞行通道和栅格地图计算出的部分路径点处相应数值如表2所示,其中:g为障碍物梯度.由表2可知,该方法计算出的结果与栅格地图计算结果相差不大,可实现较好的近似效果.

表2 通过安全飞行通道计算障碍物间隙及梯度的结果

Tab.2 Calculated results of obstacle clearance and gradient using safe flight corridors

p^T/m	安全飞行通道计算结果		栅格地图计算结果
p^T/m	o_min/m	g	o_min/m	g
(-7.34,-6.02, 2.00)	1.15	(-0.22,-0.98, 0.00)	1.16	(-0.18,-0.98, 0.00)
(-3.20,-1.51, 1.65)	0.54	(0.08, 0.99,-0.04)	0.63	(-0.31, 0.95, 0.00)
(1.02,-1.29, 1.18)	0.93	(0.98,-0.22, 0.05)	0.94	(0.97,-0.21, 0.01)
(4.28, 1.19, 1.09)	0.90	(-0.51, 0.86, 0.03)	0.92	(-0.49, 0.87, 0.01)

新窗口打开| 下载CSV

3.3 轨迹优化算法仿真实验

本文采用随机地图、走廊地图及煤矿井下地图3种环境对轨迹优化算法进行仿真测试,并与同样基于多目标函数进行凸优化的GTOP算法^[16](一种基于梯度的轨迹优化算法)进行对比.GTOP算法包含平滑项、安全项、动力学项目标函数,对于权重系数、最大速度等公共参数,两种算法选取相同数值.最大速度、加速度等参数根据无人机的动力学性能进行选取,为保障多目标函数中各项均能在迭代求解过程得到优化,需合理选取目标函数对应的权重系数.首先对2.3节中介绍的初始轨迹计算各目标函数未加权时的数值大小,再选取适当的权重系数使得各项加权后的数值相等,实际应用中可根据需求进行适当调整.两种算法均使用本文介绍的RRT^*算法得到的离散路径点进行初始化,轨迹优化结果如图7所示.其中:红色轨迹为本文算法规划结果;蓝色轨迹为GTOP算法规划结果.由图7可知,在3组实验结果中均可以看出本文算法得出的轨迹更为平缓,而GTOP算法得出的轨迹则整体变化幅度更大.

图7

新窗口打开| 下载原图ZIP| 生成PPT

图7 两种算法轨迹规划效果

Fig.7 Trajectory generation effects of two algorithms

选用平滑项数值J_s、障碍物平均间隙 $\bar{o}$ 、轨迹总耗时t_w、轨迹长度l_w等指标评价两种算法,对比结果如表3所示.本文算法在平滑项数值及轨迹总耗时上远优于GTOP算法,在障碍物平均间隙和轨迹长度指标上也有少量提升.相比于GTOP算法仅对描述空间位置信息的e_P进行优化,本文算法将分段轨迹分配时间Δt_i也加入到自由变量中,调整轨迹空间位姿的同时进行时域优化,防止由于初始时间分配不当引起的轨迹曲率增大、出现尖点等状况,使得到的轨迹更为平缓.另外,Δt_i的引入也增大了轨迹规划的自由度,优化后轨迹能够适应更为复杂的环境,从而增大障碍物的平均间隙.时间项目标函数则可在保障轨迹综合性能的前提下缩短轨迹总耗时.

表3 轨迹规划算法性能对比

Tab.3 Comparison of trajectory planning algorithms

地图类型	本文算法				GTOP算法
地图类型	J_s/(m²·s^-5)	$\bar{o}$ /m	t_w/s	l_w/m	J_s/(m²·s^-5)	$\bar{o}$ /m	t_w/s	l_w/m
随机地图	69.106	1.063	21.514	23.690	108.871	1.011	29.714	23.714
走廊地图	368.889	0.775	76.659	69.231	624.186	0.738	86.244	69.432
矿井地图	309.090	2.157	59.893	56.435	337.129	2.139	70.404	56.948

新窗口打开| 下载CSV

4 结论

(1) 针对时域连续轨迹生成过程中存在碰撞风险的问题,以RRT^*算法得到的初始离散路径点为基础,提出了一种凸多面体集合表示的安全飞行通道构建算法,通过对半平面法向量的调整均衡平均障碍物间隙和最小障碍物间隙指标,提高了无人机在复杂环境下完成轨迹规划的可能性.

(2) 针对无人机轨迹规划算法在复杂环境下综合性能较差的问题,提出了一种基于多目标优化的轨迹规划算法,通过对无人机的轨迹平滑性、安全性、动力学特性及时间性能进行优化,生成分段多项式表示的轨迹.对比实验表明,该算法相比于现有算法在部分性能指标上有一定提升,能够在复杂环境下生成具有较好综合性能的飞行轨迹.

(3) 本文研究的轨迹规划算法主要用于无人机的全局轨迹规划.当无人机按照预设轨迹进行飞行时,可能会出现未知障碍物,如何在已有的全局安全飞行通道和飞行轨迹基础上根据传感器采集的障碍物信息完成局部安全飞行通道和轨迹的快速更新,实现实时避障功能,将是下一步的研究重点.

致谢

本研究和投稿发表等工作均由全体作者独立完成.感谢天地科技股份有限公司提供的资金资助及中煤科工集团上海有限公司提供的煤矿井下环境相关资料.

参考文献

原文顺序

文献年度倒序

文中引用次数倒序

被引期刊影响因子

[1]

ZHOU

, RUI

, LI

Y R

, et al.

A UAV patrol system using panoramic stitching and object detection

[J]. Computers & Electrical Engineering, 2019, 80: 106473.

[本文引用: 1]

[2]

MANSOURI

S S

, KANELLAKIS

, KOMINIAK

, et al.

Deploying MAVs for autonomous navigation in dark underground mine environments

[J]. Robotics and Autonomous Systems, 2020, 126: 103472.

DOI:10.1016/j.robot.2020.103472 URL [本文引用: 1]

[3]

赵建霞, 段海滨, 赵彦杰, 等.

基于鸽群层级交互的有人/无人机集群一致性控制

[J]. 上海交通大学学报, 2020, 54(9): 973-980.

[本文引用: 1]

ZHAO

Jianxia

, DUAN

Haibin

, ZHAO

Yanjie

, et al.

Consensus control of manned-unmanned aerial vehicle swarm based on hierarchy interaction of pigeons

[J]. Journal of Shanghai Jiao Tong University, 2020, 54(9): 973-980.

[本文引用: 1]

[4]

CHANDLER

, GOODRICH

M A

Online RRT and online FMT: Rapid replanning with dynamic cost

[C]// 2017 IEEE/RSJ International Conference on Intelligent Robots and Systems. Vancouver, Canada: IEEE, 2017: 6313-6318.

[本文引用: 1]

[5]

CAI

Y Z

, XI

Q B

, XING

X J

, et al.

Path planning for UAV tracking target based on improved A-star algorithm

[C]// 2019 1st International Conference on Industrial Artificial Intelligence. Shenyang, China: IEEE, 2019: 1-6.

[本文引用: 1]

[6]

郝钏钏, 方舟, 李平.

基于Q学习的无人机三维航迹规划算法

[J]. 上海交通大学学报, 2012, 46(12): 1931-1935.

[本文引用: 1]

HAO

Chuanchuan

, FANG

Zhou

, LI

Ping

A 3-D route planning algorithm for unmanned aerial vehicle based on Q-learning

[J]. Journal of Shanghai Jiao Tong University, 2012, 46(12): 1931-1935.

[本文引用: 1]

[7]

CHEN

, LIU

T B

, SHEN

S J

Online generation of collision-free trajectories for quadrotor flight in unknown cluttered environments

[C]// 2016 IEEE International Conference on Robotics and Automation. Stockholm, Sweden: IEEE, 2016: 1476-1483.

[本文引用: 1]

[8]

LIU

S K

, WATTERSON

, MOHTA

, et al.

Planning dynamically feasible trajectories for quadrotors using safe flight corridors in 3-D complex environments

[J]. IEEE Robotics and Automation Letters, 2017, 2(3): 1688-1695.

DOI:10.1109/LRA.2017.2663526 URL [本文引用: 2]

[9]

STOICAN

, PRODAN

, POPESCU

, et al.

Constrained trajectory generation for UAV systems using a B-spline parametrization

[C]// 2017 25th Mediterranean Conference on Control and Automation. Valletta, Malta: IEEE, 2017: 613-618.

[本文引用: 1]

[10]

SATAI

H A

, ZAHRA

M M A

, RASOOL

Z I

, et al.

Bézier curves-based optimal trajectory design for multirotor UAVs with any-angle pathfinding algorithms

[J]. Sensors, 2021, 21(7): 2460.

DOI:10.3390/s21072460 URL [本文引用: 1]

[11]

RICHTER

, BRY

, ROY

Polynomial trajectory planning for aggressive quadrotor flight in dense indoor environments

[M]. New York, NY, USA: Springer, 2016: 649-666.

[本文引用: 3]

[12]

INGERSOLL

B T

, INGERSOLL

J K

, DEFRANCO

, et al.

UAV path-planning using bezier curves and a receding horizon approach

[C]// AIAA Modeling and Simulation Technologies Conference. Reston, Virginia: AIAA, 2016: 3675.

[本文引用: 1]

[13]

LOPEZ

B T

, HOW

J P

Aggressive 3-D collision avoidance for high-speed navigation

[C]// 2017 IEEE International Conference on Robotics and Automation. Singapore: IEEE, 2017: 5759-5765.

[本文引用: 1]

[14]

MELLINGER

, KUMAR

Minimum snap trajectory generation and control for quadrotors

[C]// 2011 IEEE International Conference on Robotics and Automation. Shanghai, China: IEEE, 2011: 2520-2525.

[本文引用: 1]

[15]

OLEYNIKOVA

, BURRI

, TAYLOR

, et al.

Continuous-time trajectory optimization for online UAV replanning

[C]// 2016 IEEE/RSJ International Conference on Intelligent Robots and Systems. Daejeon, South Korea: IEEE, 2016: 5332-5339.

[本文引用: 1]

[16]

GAO

, LIN

, SHEN

S J

Gradient-based online safe trajectory generation for quadrotor flight in complex environments

[C]// 2017 IEEE/RSJ International Conference on Intelligent Robots and Systems. Vancouver, Canda: IEEE, 2017: 3681-3688.

[本文引用: 3]

[17]

HORNUNG

, WURM

K M

, BENNEWITZ

, et al.

OctoMap: An efficient probabilistic 3D mapping framework based on octrees

[J]. Autonomous Robots, 2013, 34(3): 189-206.

DOI:10.1007/s10514-012-9321-0 URL [本文引用: 1]

[18]

SVANBERG

A class of globally convergent optimization methods based on conservative convex separable approximations

[J]. SIAM Journal on Optimization, 2002, 12(2): 555-573.

DOI:10.1137/S1052623499362822 URL

A UAV patrol system using panoramic stitching and object detection

2019

... 无人机具备灵活多变、飞行速度快及检测范围广等特点,正被大规模运用于军事和民用领域.因此,无人机在复杂环境的轨迹规划问题受到国内外学者的广泛关注^[1⇓-3].尤其在如煤矿井下等复杂环境中,如何对安全飞行通道以及飞行轨迹进行优化,确保飞行安全性,更具有广泛的研究价值. ...

Deploying MAVs for autonomous navigation in dark underground mine environments

2020

基于鸽群层级交互的有人/无人机集群一致性控制

2020

基于鸽群层级交互的有人/无人机集群一致性控制

2020

Online RRT and online FMT: Rapid replanning with dynamic cost

2017

... 无人机巡检轨迹规划一般可分为初始路径规划和轨迹优化两部分.初始路径规划是指针对飞行环境,找到一条从初始位置到目标位置的无碰撞离散路径,常用的方法有随机树搜索^[4]、A^*算法^[5]、强化学习^[6]等.由离散路径点生成时域连续轨迹的过程存在碰撞风险,一般在初始路径基础上建立安全飞行通道,通过约束生成轨迹在通道内部以保障安全性.文献[7]使用立方体实现安全飞行通道的快速构建,但通道的体积受限,可用于轨迹规划的空闲区域较小.文献[8]采用凸多面体表示安全飞行通道,在最大化通道体积和计算效率之间实现了较好的平衡,但得到的通道在路径端点处间隙较小,复杂环境下性能较差.轨迹优化是指在初始路径规划得到的离散路径点基础上,生成一条时域连续的飞行轨迹.常采用的时域连续轨迹表示方式包括B样条^[9]、Bezier曲线^[10]、多项式曲线^[11]等.为使生成的巡检轨迹具备更好的综合性能,一般还需结合无人机的动力学特性对轨迹进行优化.文献[12]采用Bezier曲线拟合离散路径点,并对轨迹长度和能量消耗等目标函数进行优化.文献[13]在满足动力学可行性和安全性约束的条件下,对最小化轨迹耗时进行了研究.常用于轨迹优化的性能指标还包括轨迹的高阶导数项^[14]、安全性^[15]、动力学可行性^[16]等. ...

Path planning for UAV tracking target based on improved A-star algorithm

2019

基于Q学习的无人机三维航迹规划算法

2012

基于Q学习的无人机三维航迹规划算法

2012

Online generation of collision-free trajectories for quadrotor flight in unknown cluttered environments

2016

Planning dynamically feasible trajectories for quadrotors using safe flight corridors in 3-D complex environments

2017

... 为验证安全飞行通道生成算法的有效性,在随机生成的三维栅格地图中进行测试,并与现有算法中较为常用且实时性较好的SFC算法^[8](一种安全飞行通道算法)进行对比,如图6所示.随机地图及起点、终点的设置与3.1节相同,先采用1.1节中介绍的RRT^*算法生成初始离散路径点,在此基础上分别采用本文算法及SFC算法计算凸多面体集合组成的安全飞行通道.其中,最大包围盒设置为距分段路径的间隙为安全距离的2倍,当新加入半平面使得当前平均障碍物间隙减小50%时重新调整该处的法向量方向,该类参数选取可以较好地平衡算法运行时间与安全飞行通道的综合性能. ...

Constrained trajectory generation for UAV systems using a B-spline parametrization

2017

Bézier curves-based optimal trajectory design for multirotor UAVs with any-angle pathfinding algorithms

2021

Polynomial trajectory planning for aggressive quadrotor flight in dense indoor environments

2016

... 采用文献[11]中介绍的闭式求解方法,连续性约束条件可转化为 ...

... 根据文献[11]中的实验测试可知,该类闭式求解方法可在轨迹段数大于50时获得稳定的数值解,满足本文轨迹优化的应用场景.将式(14)带入式(10)中,可得: ...

UAV path-planning using bezier curves and a receding horizon approach

2016

Aggressive 3-D collision avoidance for high-speed navigation

2017

Minimum snap trajectory generation and control for quadrotors

2011

Continuous-time trajectory optimization for online UAV replanning

2016

Gradient-based online safe trajectory generation for quadrotor flight in complex environments

2017

... 式中:t_k=t₀+kT_S; T_S为时域离散采样间隔;v(t_k)为t_k时刻无人机的速度矢量;c(Φ(t_k))为Φ(t_k)处的障碍物间隙惩罚项.针对轨迹中任意一点p,设计如下障碍物间隙惩罚项^[16]: ...

... 本文采用随机地图、走廊地图及煤矿井下地图3种环境对轨迹优化算法进行仿真测试,并与同样基于多目标函数进行凸优化的GTOP算法^[16](一种基于梯度的轨迹优化算法)进行对比.GTOP算法包含平滑项、安全项、动力学项目标函数,对于权重系数、最大速度等公共参数,两种算法选取相同数值.最大速度、加速度等参数根据无人机的动力学性能进行选取,为保障多目标函数中各项均能在迭代求解过程得到优化,需合理选取目标函数对应的权重系数.首先对2.3节中介绍的初始轨迹计算各目标函数未加权时的数值大小,再选取适当的权重系数使得各项加权后的数值相等,实际应用中可根据需求进行适当调整.两种算法均使用本文介绍的RRT^*算法得到的离散路径点进行初始化,轨迹优化结果如图7所示.其中:红色轨迹为本文算法规划结果;蓝色轨迹为GTOP算法规划结果.由图7可知,在3组实验结果中均可以看出本文算法得出的轨迹更为平缓,而GTOP算法得出的轨迹则整体变化幅度更大. ...

OctoMap: An efficient probabilistic 3D mapping framework based on octrees

2013

... 环境的地图信息一般可通过激光雷达、深度相机等传感器以三维点云格式获取,通过设置一定的空间分辨率可将点云地图转化为以八叉树数据结构存储的占据栅格地图Octomap^[17],每个树节点均表示一个空间立方体,可实现对点云地图的压缩,本文采用该类地图进行路径规划.首先,在栅格地图基础上,使用RRT^*算法生成初始离散路径点p₀, p₁, …, p_n.每两个相邻路径点依次连接,可得到初始路径P={p₀→p₁→…→p_n},其中p_i→p_i₊₁表示起点为p_i,终点为p_i₊₁的线段L_i,在L_i基础上计算局部安全飞行通道C_i,在通道内部可规划得到分段多项式轨迹Φ_i.无人机轨迹规划得到的分段多项式轨迹如图1所示. ...

A class of globally convergent optimization methods based on conservative convex separable approximations

2002

〈

〉