一种巡航任务下无人机避障运动规划方法与流程

本发明属于无人机空中避障技术领域，尤其涉及一种巡航任务下无人机避障运动规划方法。

背景技术：

近来，小型固定翼无人机在军用和民用领域展现出巨大潜力，广泛的应用于巡航监视和搜索中，这些任务需要固定翼无人机在陌生未知的环境中能够自主规划出无障碍碰撞路径，并且能够最大化搜索巡航范围。

传统的三维路径规划技术例如快速探索随机树、可视图法和人工势场法都计算复杂，并且无人机周围环境改变时需要重新计算。

技术实现要素：

发明目的：针对上述现有技术中无人机避障方法计算复杂、应对突发障碍能力不足等缺点，本发明提供一种巡航任务下无人机避障运动规划方法。

技术方案：本发明提供一种巡航任务下无人机避障运动规划方法，具体包括如下步骤：

步骤1：根据巡航任务的空间范围和已知障碍物的大小和坐标；利用栅格法将巡航任务的空间范围进行单元化分割，划分出若干个栅格；

步骤2：在数字地图的基础上对划分出的各个栅格进行碰撞风险评分；并删除风险评分大于0.8的栅格；

步骤3：利用k近邻算法在剩下的栅格中选择安全位置最集中的若干个栅格，并将该安全位置最集中的若干个栅格的顶点作为无人机航迹的里程碑；

步骤4：以无人机速度和最大偏航角速率为约束，利用改进的a星算法确定相邻的里程碑之间的航迹点；并根据航迹点生成全局避障航路；

步骤5：利用杜宾路径算法对相邻的航迹点之间的避障航路进行路径平滑；并输出全局平滑路径；

步骤6：在进行巡航任务时，判断是否有突发障碍物，如果没有，则无人机按照平滑路径飞行；如果在相邻的两个航迹点之间有突发障碍物，则根据突发障碍物的大小和坐标，利用杜宾路径算法重新调整该两个航迹点之间的平滑路径，从而构成避障平滑路径；并由无人机的位置以及无人机与障碍物之间的位置关系生成偏航角指令，将该偏航角指令转化为无人机的控制输入；同时采用马尔科夫决策模型判断出无人机的最佳避障机动动作。

进一步的，所述步骤1采用栅格法时采用2倍的无人机最小转弯半径作为栅格的大小。

进一步的，所述步骤2中具体风险评分的方法为：

步骤2.1：对每个栅格利用如下公式进行全局评分：

其中，k是已知障碍物的数量，是坐标为(x，y，z)的栅格与第i个障碍物之间的欧式距离，ri是第i个障碍物的半径，所述栅格的坐标为栅格顶点的坐标；

步骤2.2：根据每个栅格相邻的栅格的全局评分；对每个栅格利用如下公式进行局部评分：

其中为坐标为(i，j，q)的栅格的全局评分；

步骤2.3：利用如下公式对每个栅格进行风险评分：

进一步的，所述步骤4的具体方法为：

步骤4.1：将无人机的起点坐标和终点坐标作为输入；将栅格的顶点作为节点；并初始化open表和closed表；所述起点坐标为相邻两个里程碑中的起点，终点坐标为相邻两个里程碑中的终点；

步骤4.2：判断终点是否指向当前父节点；如果是，则根据open表中的父节点由终点向起点方向推算出相邻的里程碑之间的航迹点；如果不是，则将当前节点设为父节点,并将不存在open表中的当前父节点的可达到节点放入open表中，计算当前父节点的所有的可到达节点的f(x，y，z)的值，选择目标函数值最小的可到达节点作为下一个父节点,并将该下一个父节点同时保存在closed表中；所述可到达节点指与当前父节点相邻、与当前父节点之间满足最大偏航角速率和无人机速度的限制且不在closed表中的节点；所述满足最大偏航角速率和无人机速度的限制为当无人机从当前父节点飞行至可到达节点时无人机的偏航角速率和速度满足最大偏航角速率和无人机速度的限制；所述当前父节点的可达到节点均指向当前父节点；

所述f(x，y，z)的表达式如下所示：

其中，vcount是可到达节点被查询的次数，d^(x，y，z)是可到达节点与终点的欧式距离；

步骤4.3：比较下一个父节点与当前父节点的函数值g(x，y，z)的大小，如果下一个父节点的函数值g(x，y，z)小于等于当前父节点的函数值；则下一个父节点替换当前父节点，并转步骤4.2；如果下一个父节点的函数值g(x，y，z)大于当前父节点的函数值，则保留该当前父节点和下一个父节点，并把下一个父节点设为当前节点，转步骤4.2；

所述函数g(x，y，z)的表达式为：

进一步的，所述步骤5的具体方法为：沿起点速度方向的顺时针和逆时针以无人机的最小转弯半径为半径做起点速度方向的两个切圆n、n′；根据终点的坐标，找出与切圆n/n′相切且过终点的直线，从而找到两个切圆上的切点；根据切点的坐标、终点坐标、切圆的半径，利用余弦定理计算终点与切点之间的距离，选择与终点距离最短的切点h；按顺序连接起点、切点h、终点，从而构成一条由圆弧-直线组成的平滑路径，根据该路径在空间的导数生成终点的终点速度方向，将该终点速度方向作为下一个起点的起点速度方向。

进一步的，所述步骤6中重新调整该两个航迹点之间的平滑路径的具体方法为：沿起点速度方向的顺时针和逆时针做两个切圆s、s′；以障碍物的中心为圆心，离障碍物中心最小的安全距离为半径做障碍物的圆m；沿终点速度方向的顺时针和逆时针做两个切圆g、g′；根据切圆s/s′与圆m、圆m与切圆g/g′之间的公切线，找到切圆s、切圆s′、圆m、切圆g、切圆g′上的切点；根据所有切点的坐标以及每个圆的半径，利用余弦定理计算预设路径的长度：并选择最短的预设路径；从而确定切圆s/s′的切点q1，圆m的切点q1，切圆g/g′的切点q3；按顺序连接起点、切点q1、切点q2、切点q3、终点，从而构成了一条由圆弧-直线-圆弧组成的平滑路径；所述预设路径为按顺序连接s/s′上的切点、圆m上的切点、g/g′上的切点的路径；所述切圆s、s′、g、g′的半径为无人机的最小转弯半径。

进一步的，所述偏航角为其中xi,yi是无人机的横侧向位置，xf,yf是避开障碍物的切点位置。

有益效果：

(1)采用栅格化评分，剔除高风险区域，并利用k近邻方法分割空间设置里程碑，在降低了搜索空间提升规划速度的同时，保证了无人机巡航面积最大。

(2)局部平滑和实时避障航路调整采用的是杜宾路径算法，并直接由突发障碍物位置生成无人机的偏航角控制指令，具有响应速度快的特点，满足面对突发障碍的实时避障要求。

(3)采用马尔可夫决策模型，根据每个时刻无人机的状态和以及无人机与障碍物之间的距离选择最佳的控制策略，可以满足各种不同环境的避障任务。

附图说明

图1为本发明的流程图；

图2为本发明的杜宾路径算法的流程图；

图3为本发明实时避障几何原理图；

图4为本发明的无人机轨迹规划仿真；

图5为本发明的杜宾路径算法实时避障仿真图。

具体实施方式

构成本发明的一部分的附图用来提供对本发明的进一步理解，本发明的示意性实施例及其说明用于解释本发明，并不构成对本发明的不当限定。

本实施例提供一种巡航任务下无人机避障运动规划方法，该方法如附图1所示，首先对任务环境(巡航任务的范围)进行建模，采用栅格法将任务环境进行单元化分割，选取2倍的无人机最小转弯半径长度作为栅格大小，这样做一方面可以满足无人机的运动学约束，另一方面则可以使生成的路径更加平滑可行。

在数字地图的基础上对于划分出的各个栅格进行碰撞风险评分，并提出了综合全局和局部的评分政策。

其中全局评分如下：

其中，k是已知障碍物的数量，是坐标为(x，y，z)的栅格与第i个障碍物之间的欧式距离，ri是第i个障碍物的半径，所述栅格的坐标为栅格顶点的坐标；

此评分方法具体描述了栅格与障碍物的关系，与障碍物越近，风险值越高(最高值为1)，这种评分方法提供了与障碍物接近程度的准确估计，但会导致障碍物周围占用网格中的值的分布有些粗略。因此，还应该考虑局部风险得分，以便于网格的风险值分布更加均匀。

局部评分方法如下：

其中为坐标为(i，j，q)的栅格的全局评分；

用如下公式对每个栅格进行风险评分：

此评分方法可以较好的表示出障碍物的具体位置，并且通过对栅格的评分，剔除距离障碍物较近的点，一方面可以保证无人机的安全飞行，留出冗余，另一方面可以降低路径规划算法的搜索空间，提高算法效率。

根据栅格化后的任务环境，利用改进的a*算法；所述改进的a*算法的搜索空间为一个矩阵，矩阵的值则是栅格风险的评分，那些由障碍物占据的栅格会被评分为1，在应用改进的a*算法进行路径搜索时，首先剔除高风险栅格，即风险评分大于0.8的栅格(接近障碍物)。固定翼无人机的飞行约束主要表现在最大偏航角速率、最小转弯半径、最大里程约束三个方面，在栅格法中，最小转弯半径跟栅格划分的大小密切相关；为满足最大里程约束则需要在搜索过程中对规划出的路径长度做出判断；根据最大偏航角速率的约束，对可达到节点的选取进一步筛选，去除掉搜索空间中的不符合要求的结点，来减少搜索量，从而降低搜索时间。

改进的a*搜索算法流程如下：

步骤1：将任务空间离散化的点以及无人机的起始位置(起点坐标)和目标位置(终点坐标)作为输入；设置两个列表分别存放待查询节点和已访问的节点，起始时待查询open表置空，将无人机起点放入待查询列表open的表头，closed表用于存放已经处理过的节点。

步骤2：判断终点是否指向当前父节点；如果是，则根据open表中的父节点由终点向起点方向推算出相邻的里程碑之间的航迹点；如果不是，则将当前节点设为父节点,并将不存在open表中的当前父节点的可达到节点放入open表中，计算当前父节点的所有的可到达节点的f(x，y，z)的值，选择目标函数值最小的可到达节点作为下一个父节点，并将该下一个父节点同时保存在closed表中；所述可到达节点指与当前父节点相邻、与当前父节点之间满足最大偏航角速率和无人机速度的限制且不在closed表中的节点；所述满足最大偏航角速率和无人机速度的限制为当无人机从当前父节点飞行至可到达节点时无人机的偏航角速率和速度满足最大偏航角速率和无人机速度的限制；所述当前父节点的可达到节点均指向当前父节点；

所述f(x，y，z)的表达式如下所示：

其中，vcount是可到达节点被查询的次数，d^(x,y,z)是可到达节点与终点的欧式距离；

步骤3：比较下一个父节点与当前父节点的函数值g(x，y，z)的大小；如果下一个父节点的函数值g(x，y，z)小于等于当前父节点的函数值，则下一个父节点替换当前父节点，并转步骤2；如果下一个父节点的函数值g(x，y，z)大于当前父节点的函数值，则保留该当前父节点和下一个父节点，并把下一个父节点设为当前节点，转步骤2；

所述函数g(x，y，z)的表达式为：

在生成航迹点(航路点)后，用模值为1、方向为速度方向的向量来代替航迹点，构成航迹点向量，采用杜宾路径连接各个航迹点：在最小转弯半径的限制下，同一平面内两航迹点之间的最短路径可以由圆弧—直线—圆弧(csc)、圆弧—圆弧—圆弧(ccc)(或者是它们的一部分)组成。当满足最小转弯半径约束的情况下，杜宾路径为两个位姿点之间的最短路径。这里的最大曲率约束与固定翼无人机的最小转弯半径相对应：

式中，rmin为固定翼无人机的最小转弯半径，1/ρmax为杜宾路径中的最大曲率约束，杜宾路径中的两个位姿点恰好与航迹点向量相同，可以利用杜宾路径平滑的特点，对生成的航路进行平滑。对于前面已生成的航路，主要问题是存在拐角点，若利用csc型杜宾曲线进行圆滑，需要两个航迹点之间的距离足够大，而两个航迹点间的距离与栅格大小相关，如果栅格划分过大，则会出现地图信息不准确的情况。

针对航路中拐角点的问题，基于杜宾路径算法的思想，提出了使用部分csc曲线的方法，即在构造杜宾路径时，不考虑目标点的向量方向，在已知起点、起点速度方向、终点的情况下构造出一个由圆弧和直线(cs)组成的路径，并生成终点方向，终点方向将作为下一航路段起点的起点速度方向，具体算法流程如附图2所示：沿起点速度方向的顺时针和逆时针以无人机的最小转弯半径为半径做起点速度方向的两个切圆n、n′；根据终点的坐标，找出与切圆n/n′相切且过终点的直线，从而找到两个切圆上的切点；根据切点的坐标、终点坐标、切圆的半径，利用余弦定理计算终点与切点之间的距离，选择与终点距离最短的切点h(本实施例中选择切圆n上的切点h)；按顺序连接起点、切点h、终点，从而构成一条由圆弧-直线组成的平滑路径，根据该路径在空间的导数生成终点的终点速度方向(终点向量)，将该终点速度方向作为下一个起点的起点速度方向。

考虑到在巡航任务中紧急突发障碍(就是在全局路径规划时不知道的障碍；在执行巡航任务时出现的无人机，客机，气球等)的存在，需要对已经平滑过的路径进行调整，主要是针对无人机的偏航角或俯仰角进行调整，使无人机避开与突发障碍物的碰撞；采用障碍物的横侧向外切圆定义为障碍物的数学表达形式，具体避障方法如图3所示：沿起点速度方向的顺时针和逆时针做两个切圆s、s′(附图3中未给出切圆s′)；以障碍物的中心为圆心，离障碍物中心最小的安全距离为半径做障碍物的圆m；沿终点速度方向的顺时针和逆时针做两个切圆g、g′(附图3中未给出切圆g′)；根据切圆s/s′与圆m、圆m与切圆g/g′之间的公切线，找到切圆s、切圆s′、圆m、切圆g、切圆g′上的切点；根据所有切点的坐标以及每个圆的半径，利用余弦定理计算预设路径的长度：并选择最短的预设路径；从而确定切圆s/s′的切点q1，圆m的切点q2，切圆g/g′的切点q3；按顺序连接起点、切点q1、切点q2、切点q3、终点，从而构成了一条由圆弧-直线-圆弧组成的平滑路径；所述预设路径为按顺序连接s/s′上的切点、圆m上的切点、g/g′上的切点的路径；所述切圆s、s′、g、g′的半径为无人机的最小转弯半径。

由无人机的位置以及无人机与障碍物之间的位置关系生成偏航角指令其中xi，yi是无人机的横侧向位置，xf，yf是由几何关系得到避开障碍物的切点位置，并结合杜宾路径进行无人机实时避障。

无人机避障时有三种机动动作可以选择，采用基于马尔可夫决策模型来进行机动决策，根据无人机当前的状态量以及与障碍之间的相对位置判断是否变换无人机速度、进行局部路径调整或者是全局航迹调整。将无人机的控制和决策问题建模为有限状态马尔可夫决策过程，利用策略搜索方法学习出最佳控制策略的参数，通过马尔可夫模型得到当前状态下无人机的最佳避障机动动作。

仿真

对本实施例提出的无人机避障控制器在matlab中进行仿真验证。选用的无人机是小型固定翼无人机，无人机采用改进a星算法对路径进行规划，仿真结果如附图4所示，圆代表先验已知的障碍区域，由图4可知无人机可以避开航路中的障碍，生成满足无人机动力学约束的平滑路径。针对紧急突发障碍，采用杜宾路径算法的实时避障仿真效果如附图5所示，图中黑色实心圆代表突发障碍物，生成了一条不与障碍物发生碰撞的无人机避障平滑路径。

综上，本实施例所采用的方法为，在环境建模部分采用了栅格法对任务环境进行离散化处理，并且对栅格进行风险评分。并且讨论了无人机的一些飞行约束，并构造了航路评价函数。在全局路径规划方面，主要针对a星算法做出改进，引入了最大偏航角约束，并且在a星算法的启发信息里加入了航路威胁信息，使生成的航路更加远离障碍物，并且matlab仿真结果表明该改进措施生成的航路威胁值明显低于传统a星算法，改进后的a星算法使得航路变得更加安全、可行、并且符合固定翼无人机的飞行约束。进一步利用杜宾路径算法对全局规划生成的航迹点进行平滑处理，针对在传统航路规划算法结果中航路有拐角点的情况进行改进，使得原来航路仅由航迹点连接而成变为由至直线、圆、圆弧组合而成，新的航路与旧航路相比更符合实际情况，同时考虑到突发障碍的情况，平滑路径进行调整绕开障碍物，进而根据当前无人机的状态和障碍物的相对位置关系，采用马尔可夫模型决策出最佳避障机动。

另外需要说明的是，在上述具体实施方式中所描述的各个具体技术特征，在不矛盾的情况下，可以通过任何合适的方式进行组合。为了避免不必要的重复，本发明对各种可能的组合方式不再另行说明。