[发明专利]一种巡航任务下无人机避障运动规划方法

权利要求书

1.一种巡航任务下无人机避障运动规划方法，其特征在于，包括如下步骤：

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

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

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

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

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

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

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

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

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

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

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

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

2.根据权利要求1所述的一种巡航任务下无人机避障运动规划方法，其特征在于，所述步骤1采用栅格法时采用2倍的无人机最小转弯半径作为栅格的大小。

3.根据权利要求1所述的一种巡航任务下无人机避障运动规划方法，其特征在于，所述步骤4的具体方法为：

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

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

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

其中，V_count是可到达节点被查询的次数，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)的表达式为：