CN113296536A - 一种基于a*与凸优化算法的无人机三维避障算法 - Google Patents

Abstract

本发明公开了一种基于A*与凸优化算法的无人机三维避障算法，所述算法包括如下步骤：第一步、明确轨迹规划算法的设计任务；第二步、通过A*算法得到无人机的参考路径；第三步、通过基于半定规划的迭代区域膨胀方法给出一系列凸多面体组成的避障约束；第四步、针对无人机***，给出无人机的三维避障轨迹规划模型，并通过解算该模型得到无人机的状态序列以及控制序列。本发明的算法不仅能够避开场地中的凸多面体障碍物，而且还能避免无人机在离散的时间步之间撞击障碍物的可能。本发明的算法相比于传统的凸优化避障算法减少了计算量，并最终得到一组符合要求的状态序列以及一组燃料消耗最少的控制序列。

Description

一种基于A*与凸优化算法的无人机三维避障算法

技术领域

本发明属于航空航天领域，涉及一种无人机三维避障算法，具体涉及一种适用于无人机，可以快速生成飞行轨迹到达指定地点并避开场地中的凸多面体障碍的算法。

背景技术

传统的无人机轨迹规划通常被建模为一个离散的线性最优控制问题，其本质上是一个混合整数的线性规划问题。这种模型往往由于障碍物形状的不规则以及障碍物数量的增多，会导致模型中出现大量的整数变量与不等式约束，从而导致求解时间过长失去实时性，甚至直接超出一般解算器的解算能力，此外无人机在离散的时间步之间也有可能发生撞击障碍物的可能。而常用的启发式路径规划算法例如A*算法，也具有路径不平滑、难以考虑无人机本身所带来的约束等问题。

发明内容

针对现有技术存在的上述问题，本发明提供了一种基于A*与凸优化算法的无人机三维避障算法。该算法能够快速在三维场地中给出从起始点到目标点的飞行轨迹，避开场地中存在的凸多面体障碍物，同时生成一组燃料消耗最少的控制序列。

本发明的目的是通过以下技术方案实现的：

一种基于A*与凸优化算法的无人机三维避障算法，包括如下步骤：

第一步、明确轨迹规划算法的设计任务，所述轨迹规划算法的设计任务是：给定一张包含多个凸多面体障碍的三维地图，标记起始点x₀与目标点x_f，针对给定的无人机以及规定的飞行时间t_final，给出最省燃料的飞行路径以及相对应的控制序列；

第二步、通过A*算法得到无人机的参考路径，设计代价函数如下：

f(n)＝g(n)+h(n)；

其中，f(n)为从起始点x₀经由节点x(n)移动到目标点x_f的移动代价，g(n)为从起始点x₀沿着到达该方格而生成的路径移动到指定方格的移动代价，h(n)为从指定的方格移动到目标点x_f的估算成本；

第三步、通过基于半定规划的迭代区域膨胀方法给出一系列凸多面体组成的避障约束，凸区域避障约束表示为：

L＝{x|A_ix≤b_i}；

其中，约束方程矩阵A_i∈R^m×n，b_i∈R^m，i＝[1,2，...，N_c]，N_c为凸区域个数，m为约束元素数，n为维数，本发明中n＝3；

第四步、针对无人机***，给出无人机的三维避障轨迹规划模型，并通过解算该模型得到无人机的状态序列以及控制序列，其中无人机的三维避障轨迹规划模型如下：

s.t.X_i(j+1)＝X_i(j)+Δt(A_dX_i(j)+B_du_i(j))；

A_iP_i(j)≤b_i；

X₁(1)＝X₀；

X_Nc(Step)＝X_f；

X_i(Step)＝X_i+1(1)；

||u_i(j)||₂≤u_max；

|u_i(Step,n)-u_i+1(1,n)|＜Δu；

其中，N_c表示凸区域个数，Step＝100表示每个凸多面体区域的步数，u_i(j,n)表示第i个凸区域中第j步的推力向量中第n轴推力分量，i＝[1,2,...,N_c]，j＝[1,2,...,Step]，n＝[1,2,3]，|·|表示绝对值，||·||₂表示2-范数，X_i(j)＝[p_ix(j) p_iy(j) p_iz(j)v_ix(j) v_iy(j) v_iz(j)]^T，P_i(j)＝[p_ix(j) p_iy(j) p_iz(j)]^T，p_ix(j)、p_iy(j)、p_iz(j)分别为无人机在第i个避障约束区域内第j步中x、y、z轴的位置分量，v_ix(j)、v_iy(j)、v_iz(j)分别为无人机在第i个避障约束区域内第j步中x、y、z轴的速度分量，X₀、X_f分别表示无人机起始状态与目标状态，A_d表示无人机的***矩阵，B_d表示无人机的输入矩阵，u_i(j)表示无人机在第i个避障约束区域内第j步的推力向量，u_max表示无人机提供的最大推力，

t₀(i)为每个凸区域的起始时间，t_f(i)为每个凸区域的结束时间，Δu表示最大推力变化率。

相比于现有技术，本发明具有如下优点：

1、本发明的算法不仅能够避开场地中的凸多面体障碍物，而且还能避免无人机在离散的时间步之间撞击障碍物的可能。

2、本发明的算法相比于传统的凸优化避障算法减少了计算量，并最终得到一组符合要求的状态序列以及一组燃料消耗最少的控制序列。

附图说明

图1为基于A*与凸优化算法的无人机三维避障算法的设计流程图；

图2为A*算法的流程图；

图3为基于半定规划的迭代区域膨胀算法的流程图；

图4为生成一系列凸区域约束的流程图；

图5为三维地图数据，(a)三维视图，(b)俯视图；

图6为参考飞行轨迹，(a)三维视图，(b)俯视图；

图7为凸区域约束图，(a)三维视图，(b)俯视图；

图8为生成无人机位置轨迹，(a)三维视图，(b)俯视图；

图9为生成无人机推力。

具体实施方式

下面结合附图对本发明的技术方案作进一步的说明，但并不局限于此，凡是对本发明技术方案进行修改或者等同替换，而不脱离本发明技术方案的精神和范围，均应涵盖在本发明的保护范围中。

本发明提供了一种基于A*与凸优化的无人机三维避障算法，如图1所示，具体设计步骤如下：

第一步：明确轨迹规划算法的设计任务。

轨迹规划算法的设计任务是：给定一张包含多个凸多面体障碍的三维地图，标记起始点x₀与目标点x_f，针对给定的无人机以及规定的飞行时间t_final，给出最省燃料的飞行路径以及相对应的控制序列。

第二步：通过A*算法给出飞行器参考路径。

首先设计代价函数：

f(n)＝g(n)+h(n)；

其中，f(n)为从起始点x₀经由节点x(n)移动到目标点x_f的移动代价，g(n)为从起始点x₀沿着到达该方格而生成的路径移动到指定方格的移动代价，这里移动代价取两方格中心的直线距离，h(n)为从指定的方格移动到目标点x_f的估算成本，这里用当前指定方格中心点到目标点x_f所在的方格的中心点的直线距离作为估算成本。

A*算法的流程如图2所示，具体可以概括为如下流程：

1.将起点加入open list。

2.重复如下过程：

a.遍历open list，查找f(n)最小的节点，将其设置为当前节点。

b.把当前节点移到close list。

c.判断当前节点周围节点是否存在于list之中。

◆若它存在于close list中，则忽略该节点。

◆若它不在openlist中，将其加入openlist，并且把当前方格设置为它的父亲，记录该方格的f(n)值。

◆如果它已经在openlist中，检查这条路径是否更优，如果是，把它的父亲设置为当前方格，并重新计算它的f(n)值。

d.停止循环，当出现以下情况：

◆把终点加入到了openlist中，此时路径已经找到了，或者

◆查找终点失败，并且openlist是空的，此时没有路径。

3.保存路径。从终点开始，每个方格沿着父节点移动直至起点，就得到了参考路径。

第三步：通过基于半定规划的迭代区域膨胀方法给出一系列凸多面体组成的避障约束。

算法的整体流程图如图3和图4所示，其中图3给出了根据一个点通过基于半定规划的迭代区域膨胀算法得到与障碍物相切的凸区域方法，图4给出了根据参考路径坐标得到一系列凸区域的方法。首先根据图3介绍通过参考轨迹坐标点获得以坐标点为球心的椭球与障碍物相切的最优分割面集合方法。

凸区域的线性约束可表示为：

L＝{x|Ax≤b}；

其中，约束方程矩阵A∈R^m×n，b∈R^m，m为约束元素数，n为维数，本发明中n＝3。

初始椭球由单位圆经坐标变换后得到，可以表示为：

其中：C∈R^n×n，

d为无人机参考路径中的坐标。

求解给定椭球的最优线性约束就是在扩大内切椭球体积时，确保椭球不与障碍物相交，为解决这个问题，可以先找到障碍物中距离椭球最近的点。

定义椭球E和凸多面体障碍物O_j的顶点集合v_jp，p＝1,2,3...，根据上述椭球定义式可将最近距离问题映射到单位球上，即只需要找到映射后的障碍物

距离原点最近的点，该问题可以表述成：

该优化得到的点

经过原映射

即可求得原问题中障碍物中距离椭球最近的点x^*。

找到障碍物距离椭球最近的点位后即可通过求取过该点与椭球的垂直正交面得到一个最优分割面，求取垂直正交面的步骤如下：

其中，a_j是A的第j行，b_j是b的第j个元素。

这样，便得到了一个最优分割面，为减少计算量，每计算出一个最优分割面以后就遍历一次障碍物，判断其顶点是否都满足

如果满足，则说明障碍物与椭球被上述分割面分隔开，如果不满足，则计算出另一个分割面继续分割该障碍物与椭球。

得到给定椭球的最优分割面集合后，为了使凸多面体体积尽可能大，在已经求得的最优分割面集合中寻找体积最大的椭球，在给定分割面集合中寻找体积最大的椭球可表述为如下问题：

C≥1；

其中，N为障碍物个数，detC表示矩阵C的行列式。将上述问题改写为不包含

的格式如下：

C≥1；

该问题是一个包含半定优化与圆锥二次约束的凸优化问题，可以利用CVX或者Mosek解算器求解。然后按照图3所示流程反复迭代即可得到所求的最优分割面集合。具体迭代流程为：

1、初始化椭球，初始化位置以及障碍物，其中，d＝x₀，C＝10⁴×I^3×3，I^3×3表示三维单位阵。

2、求解给定椭圆的最优分割面集合组成的线性约束。

3、求解给定线性约束的最大内切椭球。

4、判断是否满足|detC-detC_max|/detC_max≤ε，其中，C_max表示迭代中得到的最大detC的矩阵C，ε表示给定误差，若是第一次进入循环则跳过判断直接将得到的C带回步骤2。

a、满足，则结束迭代。

b、不满足，则将得到的C带回步骤2。

然后按照图4的流程得到一系列的凸区域，具体步骤如下：

1、将参考路径起点设置为给定点。

2、根据给定点按照上述基于半定规划的迭代区域膨胀方法得到一个最优分割面集合组成的凸区域约束。

3、判断目标点是否符合步骤2求得的最优分割面集合组成的凸区域约束：

a、目标点满足凸约束，则结束循环。

b、目标点不满足凸约束，则遍历参考路径点，找到第一个不满足目前所有生成的凸约束的路径点，并将其设置成给定点，并返回步骤2。

这样就得到了一系列不包含障碍物的凸区域组成的避障约束，设其约束方程矩阵为A_i∈R^m×n，b_i∈R^m，i＝[1,2，...，N_c]，N_c为凸区域个数。

第四步：基于凸优化的无人机三维避障在线轨迹规划。

首先针对无人机轨迹生成问题，建立包括性能指标、运动学约束、避障约束、状态约束、推力约束的最优控制问题模型。

无人机的轨迹生成性能指标J如下式所示，通过最小化油门以得到最节省燃料的飞行轨迹。

其中，N_c表示凸区域个数，Step＝100表示每个凸多面体区域的步数，u_i(j,n)表示第i个凸区域中第j步的推力向量中第n轴推力分量，i＝[1,2,...,N_c]，j＝[1,2,...,Step]，n＝[1,2,3]，|·|表示绝对值。

无人机的运动学约束表示为下式所示的微分方程组：

其中，X_i＝[p_ix p_iy p_iz v_ix v_iy v_iz]^T，p_ix、p_iy、p_iz分别为无人机在第i个避障约束区域内x、y、z轴的位置分量，v_ix、v_iy、v_iz分别为无人机在第i个避障约束区域内x、y、_z轴的速度分量，u_i表示无人机在第i个避障约束区域内的推力向量，A_d表示无人机的***矩阵，B_d表示无人机的输入矩阵。为了能够将其转化为凸优化问题，故需要将其离散化处理，其状态转移矩阵取状态矩阵的一阶近似，离散化后的形式为：

X_i(j+1)＝X_i(j)+Δt(A_dX_i(j)+B_du_i(j))；

其中，X_i(j)＝[p_ix(j) p_iy(j) p_iz(j) v_ix(j) v_iy(j) v_iz(j)]^T，p_ix(j)、p_iy(j)、p_iz(j)分别为无人机在第i个避障约束区域内第j步中x、y、z轴的位置分量，v_ix(j)、v_iy(j)、v_iz(j)分别为无人机在第i个避障约束区域内第j步中x、y、z轴的速度分量，u_i(j)表示无人机在第i个避障约束区域内第j步的推力向量，A_d表示无人机的***矩阵，B_d表示无人机的输入矩阵，

t₀(i)为每个凸区域的起始时间，t_f(i)为每个凸区域的结束时间。

无人机的避障约束可以表示为：

A_iP_i(j)≤b_i；

其中，P_i(j)＝[p_ix(j) p_iy(j) p_iz(j)]^T，上述约束表示无人机的轨迹始终在凸区域内，这样就能保证无人机全程不与障碍接触。

无人机的状态约束可以表示为：

X₁(1)＝X₀；

X_Nc(Step)＝X_f；

X_i(Step)＝X_i+1(1)。

其中，X₀、X_f分别表示无人机起始状态与目标状态，上式约束了无人机的起始状态与目标状态，并保证了不同凸区域切换时无人机状态的连续性。

无人机的推力约束由下式给出：

||u_i(j)||₂≤u_max；

|u_i(Step,n)-u_i+1(1,n)|＜Δu；

其中，||·||₂表示2-范数，u_max表示无人机提供的最大推力，Δu表示最大推力变化率。

上式给出了无人机的最大推力约束以及最大推力变化约束。

综上，得到无人机的三维避障轨迹规划模型如下：

s.t.X_i(j+1)＝X_i(j)+Δt(A_dX_i(j)+B_du_i(j))；

A_iP_i(j)≤b_i；

X₁(1)＝X₀；

X_Nc(Step)＝X_f；

X_i(Step)＝X_i+1(1)；

||u_i(j)||₂≤u_max；

|u_i(Step,n)-u_i+1(1,n)|＜Δu。

解算上述凸优化问题，即可得到满足要求的状态序列以及控制序列。

实施例：

下面通过介绍一个具有一定代表性的实施例，来进一步说明本发明技术方案中的相关设计。

第一步：明确轨迹规划算法的设计任务。

轨迹规划算法的设计任务是：给定一张包含多个凸多面体障碍的三维地图，标记起始点x₀与目标点x_f，针对给定的无人机以及规定的飞行时间t_final，给出最省燃料的飞行路径以及相对应的控制序列u。

地图数据以及起始点目标点数据如图5所示，其中起始点坐标为x₀＝(4 7 7)，目标点坐标为x_f＝(17 17 8)，飞行时间t_final＝25s，地图三个轴的边界均为[-1,21]。

第二步：通过A*算法给出飞行器参考路径。

首先设计代价函数：

f(n)＝g(n)+h(n)；

其中，f(n)为从起始点x₀经由节点x(n)移动到目标点x_f的移动代价，g(n)为从起始点x₀沿着到达该方格而生成的路径移动到指定方格的移动代价，这里移动代价取两方格中心的直线距离，h(n)为从指定的方格移动到目标点x_f的估算成本，这里用当前指定方格中心点到目标点x_f所在的方格的中心点的直线距离作为估算成本。

按照图2流程得到的参考飞行轨迹如图6所示，具体参考路径坐标见表1。

表1无人机参考飞行轨迹

第三步：通过基于半定规划的迭代区域膨胀方法给出一系列凸多面体组成的避障约束。

按照图3和图4所示流程通过迭代区域膨胀算法得到的一系列凸约束区域如图7所示，取给定误差ε＝0.02，具体凸区域避障约束矩阵A_i、b_i如下：

第四步：基于凸优化的无人机三维避障在线轨迹规划。

首先针对无人机轨迹生成问题，建立包括性能指标、运动学约束、避障约束、状态约束、推力约束的最优控制问题模型。

无人机的轨迹生成性能指标J如下式所示，通过最小化油门以得到最节省燃料的飞行轨迹。

其中，N_c表示凸区域个数，Step＝100表示每个凸多面体区域的步数，u_i(j,n)表示第i个凸区域中第j步的推力向量中第n轴推力分量，i＝[1,2,...,N_c]，j＝[1,2,...,Step]，n＝[1,2,3]，|·|表示绝对值。

无人机的运动学约束表示为下式所示的微分方程组：

其中，X_i＝[p_ix p_iy p_iz v_ix v_iy v_iz]^T，p_ix、p_iy、p_iz分别为无人机在第i个避障约束区域内x、y、z轴的位置分量，v_ix、v_iy、v_iz分别为无人机在第i个避障约束区域内x、y、z轴的速度分量，u_i表示无人机在第i个避障约束区域内的推力向量，A_d表示无人机的***矩阵，B_d表示无人机的输入矩阵，不失一般性，将***矩阵与输入矩阵的具体参数定义如下：

为了能够将其转化为凸优化问题，故需要将其离散化处理，其状态转移矩阵取状态矩阵的一阶近似，离散化后的形式为：

X_i(j+1)＝X_i(j)+Δt(A_dX_i(j)+B_du_i(j))；

其中，X_i(j)＝[p_ix(j) p_iy(j) p_iz(j) v_ix(j) v_iy(j) v_iz(j)]^T，p_ix(j)、p_iy(j)、p_iz(j)分别为无人机在第i个避障约束区域内第j步中x、y、z轴的位置分量，v_ix(j)、v_iy(j)、v_iz(j)分别为无人机在第i个避障约束区域内第j步中x、y、z轴的速度分量，u_i(j)表示无人机在第i个避障约束区域内第j步的推力向量，A_d表示无人机的***矩阵，B_d表示无人机的输入矩阵，

t₀(i)为每个凸区域的起始时间，t_f(i)为每个凸区域的结束时间。设t₀(i)＝0，t_f(i)的数值按照凸区域中的参考路径节点数量给出，具体数值为t_f(1)＝12，t_f(2)＝1，t_f(3)＝2，t_f(4)＝3，t_f(5)＝3，t_f(6)＝4。

无人机的避障约束可以表示为：

A_iP_i(j)≤b_i；

其中，P_i(j)＝[p_ix(j) p_iy(j) p_iz(j)]^T，上述约束表示无人机的轨迹始终在凸区域内，这样就能保证无人机全程不与障碍接触。

无人机的状态约束可以表示为：

X₁(1)＝X₀；

X_Nc(Step)＝X_f；

X_i(Step)＝X_i+1(1)；

其中，起始点状态为X₀＝[4 9 1 0 0 0]^T，目标点状态为X_f＝[17 12 8 0 0 0]^T。

无人机的推力约束由下式给出：

||u_i(j)||₂≤u_max；

|u_i(Step,n)-u_i+1(1,n)|＜Δu；

其中，||·||₂表示2-范数，u_max＝10，Δu＝1。

综上，得到无人机的三维避障轨迹规划模型如下：

s.t.X_i(j+1)＝X_i(j)+Δt(A_dX_i(j)+B_du_i(j))；

A_iP_i(j)≤b_i；

X₁(1)＝X₀；

X_Nc(Step)＝X_f；

X_i(Step)＝X_i+1(1)；

||u_i(j)||₂≤u_max；

|u_i(Step,n)-u_i+1(1,n)|＜Δu；

解算上述凸优化问题，得到满足要求的位置轨迹如图8所示，推力序列如图9所示。

Claims (5)

1.一种基于A*与凸优化算法的无人机三维避障算法，其特征在于所述算法包括如下步骤：

第一步、明确轨迹规划算法的设计任务；

第二步、通过A*算法得到无人机的参考路径；

第三步、通过基于半定规划的迭代区域膨胀方法给出一系列凸多面体组成的避障约束；

第四步、针对无人机***，给出无人机的三维避障轨迹规划模型，并通过解算该模型得到无人机的状态序列以及控制序列。

2.根据权利要求1所述的基于A*与凸优化算法的无人机三维避障算法，其特征在于所述第一步中，轨迹规划算法的设计任务是：给定一张包含多个凸多面体障碍的三维地图，标记起始点x₀与目标点x_f，针对给定的无人机以及规定的飞行时间t_final，给出最省燃料的飞行路径以及相对应的控制序列。

3.根据权利要求1所述的基于A*与凸优化算法的无人机三维避障算法，其特征在于所述第二步中，设计代价函数如下：

f(n)＝g(n)+h(n)；

其中，f(n)为从起始点x₀经由节点x(n)移动到目标点x_f的移动代价，g(n)为从起始点x₀沿着到达该方格而生成的路径移动到指定方格的移动代价，h(n)为从指定的方格移动到目标点x_f的估算成本。

4.根据权利要求1所述的基于A*与凸优化算法的无人机三维避障算法，其特征在于所述第三步中，凸区域避障约束表示为：

L＝{x|A_ix≤b_i}；

其中，约束方程矩阵A_i∈R^m×n，b_i∈R^m，i＝[1,2，...，N_c]，N_c为凸区域个数，m为约束元素数，n为维数，其中n＝3。

5.根据权利要求1所述的基于A*与凸优化算法的无人机三维避障算法，其特征在于所述第四步中，无人机的三维避障轨迹规划模型如下：

s.t.X_i(j+1)＝X_i(j)+Δt(A_dX_i(j)+B_du_i(j))；

A_iP_i(j)≤b_i；

X₁(1)＝X₀；

X_Nc(Step)＝X_f；

X_i(Step)＝X_i+1(1)；

||u_i(j)||₂≤u_max；

|u_i(Step,n)-u_i+1(1,n)|＜Δu；

其中，N_c表示凸区域个数，Step＝100表示每个凸多面体区域的步数，u_i(j,n)表示第i个凸区域中第j步的推力向量中第n轴推力分量，i＝[1,2,...,N_c]，j＝[1,2,...,Step]，n＝[1,2,3]，|·|表示绝对值，||·||₂表示2-范数，X_i(j)＝[p_ix(j) p_iy(j) p_iz(j) v_ix(j) v_iy(j) v_iz(j)]^T，P_i(j)＝[p_ix(j) p_iy(j) p_iz(j)]^T，p_ix(j)、p_iy(j)、p_iz(j)分别为无人机在第i个避障约束区域内第j步中x、y、z轴的位置分量，v_ix(j)、v_iy(j)、v_iz(j)分别为无人机在第i个避障约束区域内第j步中x、y、z轴的速度分量，X₀、X_f分别表示无人机起始状态与目标状态，A_d表示无人机的***矩阵，B_d表示无人机的输入矩阵，u_i(j)表示无人机在第i个避障约束区域内第j步的推力向量，u_max表示无人机提供的最大推力，

t₀(i)为每个凸区域的起始时间，t_f(i)为每个凸区域的结束时间，Δu表示最大推力变化率。

Images