【技术实现步骤摘要】
本专利技术涉及一种基于双向apf-rrt*-cpo算法的三维航迹规划方法。
技术介绍
1、随着技术的进步和成本的降低，无人机在民用领域的有优势越来越突出，如在航拍摄影、环境监测、农业植保、电力巡检、森林防火、应急通信以及无人机物流等多个方面得到广泛应用。无人机的航迹规划是无人机任务规划的重要环节。结合无人机自身的性能指标和特点，采用一定的航迹规划方法，制定最优或者次优路径是无人机安全飞行的重要保障。尤其是在复杂的山地环境之中，优质的航迹会大大降低无人机损毁的风险，从而提高无人机执行任务的效率，所以复杂山地环境下无人机的航迹规划具有重要的现实意义。
2、现有技术中传统rrt*算法，dijkstra算法等在复杂环境下会生成大量节点，计算复杂度大，运行时间较长。智能优化算法遗传算法、蚁群算法等广泛应用于无人机的航迹规划中。但在复杂环境下智能优化算法容易陷入局部最优，无法准确找到可供无人机安全飞行的航迹。具有一定的局限性。
技术实现思路
1、本专利技术的目的是提供一种复杂三维环境下提高无人机航迹规划搜索速度和航迹可行性的基于双向apf-rrt*-cpo算法的三维航迹规划方法。
2、本专利技术采用如下技术方案：
3、一种基于双向apf-rrt*-cpo算法的三维航迹规划方法，其包括如下步骤：
4、（1）构建三维地图，设置无人机的性能约束；创建两棵随机树t1，t2；
5、（2）采用双向搜索；随机树t1，t2分别生成采样点qrand1、
6、（3）以新节点qnew1、qnew2为中心，重新寻找距离新节点qnew1、qnew2相连接距离更小的节点，若存在该节点且与新节点qnew1、qnew2相连接之间不存在障碍，则将qnew1加入t1，将qnew2加入t2；否则，不改变原航迹，继续向下执行；
7、（4）判断t1，t2中是否存在直接相连的节点，若是则继续步骤（5），若否则重复步骤（2）；
8、（5）将t1，t2中所有节点相连，得到初始航迹；
9、（6）初始化cpo算法参数；
10、（7）以初始航迹的航迹点设置临时起点和终点，利用位置更新公式进行航迹点更新，找到两个节点之间的最短距离；
11、（8）计算总代价函数计算无人机航迹总成本，找到代价最小的航迹；
12、（9）根据最小代价位置信息，连接生成最优航迹。
13、更近一步的，步骤（1）中，t1以qstart为根节点，t2以qgoal为根节点，并为根节点标记索引。
14、更近一步的，步骤（2）中，当节点qnear1处于无障碍区域时，采用目标点节点qgoal对节点qnear1产生的引力fatt1（qnear1）引导树t1的生长方向生成新节点qnew1。
15、
16、其中，为尺度因子，表示节点qnear1到目标点qgoal的欧式距离。
17、当节点qnear1处于障碍区域时，用目标点qgoal、采样点qrand1对节点qnear1产生的引力fatt2（qnear1）以及障碍物中心点qobs对节点qnear1产生斥力frepi（qnear1）的合力f（qnear1）来引导树t1的生长方向生成新节点qnew1。
18、
19、
20、
21、其中，为尺度因子，表示节点qnear1到采样点qrand1的欧式距离；为节点qnear1到障碍物的欧式距离；为斥力尺度因子；为障碍物节点的影响半径；是节点qnear1到障碍物qobsi方向矢量，i=1,2,…,m，m为障碍物个数。
22、更近一步的，步骤（2）中，当节点qnear2处于无障碍区域时，采用起点qstart对节点qnear2产生的引力fatt1（qnear2）引导树t2的生长方向生成新节点qnew2；
23、
24、其中，表示节点qnear2到起点qstart的欧式距离。
25、当节点qnear2处于障碍区域时，用起点qstart、采样点qrand2对节点qnear2产生的引力fatt2（qnear2）以及障碍物中心点qobs对节点qnear2产生斥力frepi（qnear2）的合力f（qnear2）来引导树t2的生长方向生成新节点qnew2；
26、
27、
28、
29、其中，表示节点qnear2到采样点qrand2的欧式距离；为节点qnear1到障碍物的欧式距离；为斥力尺度因子；为障碍物节点的影响半径；是节点qnear2到障碍物qobsi方向矢量。
30、更近一步的，步骤（3）中，障碍物检测方法为：将节点与节点连线分成n+1个坐标点，判断坐标点与障碍物中心坐标点的欧氏距离，若此距离小于障碍物的半径，即节点之间存在障碍物；若此距离大于障碍物的半径，节点之间不存在障碍物。
31、更近一步的，步骤（7）中，先判定算法处于勘探阶段或开发阶段，再进行位置更新公式的判断。
32、所述算法处于勘探阶段或开发阶段的判定方法为：首先生成两个在0~1之间的随机值r1、r2；若r1小于r2则算法进入算法勘探阶段；若r1大于r2则算法进入算法开发阶段。
33、更近一步的，当算法进入算法勘探阶段时，再次生成两个在0~1之间的随机值r3、r4；若r3小于r4则位置更新采用位置更新公式1；若r3大于r4则位置更新采用位置更新公式2。
34、位置更新公式1：
35、
36、
37、
38、其中，是第i个种群个体第t次迭代位置， rand是0~1之间的随机值，是最佳个体位置，是种群的一个随机个体的位置；i和 是一个大于等于1小于等于n的整数随机值；f是柯西分布的逆累积分布数，p是0~1间的概率值；γ为设定参数。
39、位置更新公式2：
40、
41、
42、
43、其中，是第i个种群个体第t次迭代位置；t是迭代次数；rand是0~1之间的随机值， 是一个二进制向量，，，是种群中三个个不同的随机个体的位置；popr，popr1，popr2分别是是种群中的一个大于等于1小于等于n的整数随机值；u、v是服从正态分布的随机数。
44、更近一步的，当算法进入算法开发阶段时，设置一个0~1之间的自定义阈值，同时再次生成一个在0~1内的随机值r5；若r5小于自定义阈值则位置更新公式采用位置更新公式3；若r5大于自定义阈值则位置更新公式采用位置更新公式4。
45、位置更新公式3：
46、
47、
48、。
49、位置更新公式4：
50、
51、
52、
本文档来自技高网...
【技术保护点】
1.一种基于双向APF-RRT*-CPO算法的三维航迹规划方法，其特征在于，其包括如下步骤：
2.根据权利要求1所述的一种基于双向APF-RRT*-CPO算法的三维航迹规划方法，其特征在于，步骤（1）中，T1以qstart为根节点，T2以qgoal为根节点，并为根节点标记索引。
3.根据权利要求2所述的一种基于双向APF-RRT*-CPO算法的三维航迹规划方法，其特征在于，步骤（2）中，当节点qnear1处于无障碍区域时，采用目标点节点qgoal对节点qnear1产生的引力Fatt1（qnear1）引导树T1的生长方向生成新节点qnew1；
4.根据权利要求2所述的一种基于双向APF-RRT*-CPO算法的三维航迹规划方法，其特征在于，步骤（2）中，当节点qnear2处于无障碍区域时，采用起点qstart对节点qnear2产生的引力Fatt1（qnear2）引导树T2的生长方向生成新节点qnew2；
5.根据权利要求3或4所述的一种基于双向APF-RRT*-CPO算法的三维航迹规划方法，其特征在于，步骤（3）中，障碍物检测方法为：将
6.根据权利要求5所述的一种基于双向APF-RRT*-CPO算法的三维航迹规划方法，其特征在于，步骤（7）中，先判定算法处于勘探阶段或开发阶段，再进行位置更新公式的判断；
7.根据权利要求6所述的一种基于双向APF-RRT*-CPO算法的三维航迹规划方法，其特征在于，当算法进入算法勘探阶段时，再次生成两个在0~1之间的随机值r3、r4；若r3小于r4则位置更新采用位置更新公式1；若r3大于r4则位置更新采用位置更新公式2；
8.根据权利要求7所述的一种基于双向APF-RRT*-CPO算法的三维航迹规划方法，其特征在于，当算法进入算法开发阶段时，设置一个0~1之间的自定义阈值，同时再次生成一个在0~1内的随机值r5；若r5小于自定义阈值则位置更新公式采用位置更新公式3；若r5大于自定义阈值则位置更新公式采用位置更新公式4。
9.根据权利要求8所述的一种基于双向APF-RRT*-CPO算法的三维航迹规划方法，其特征在于，步骤（8）中的总代价函数为：
...【技术特征摘要】
1.一种基于双向apf-rrt*-cpo算法的三维航迹规划方法，其特征在于，其包括如下步骤：
2.根据权利要求1所述的一种基于双向apf-rrt*-cpo算法的三维航迹规划方法，其特征在于，步骤（1）中，t1以qstart为根节点，t2以qgoal为根节点，并为根节点标记索引。
3.根据权利要求2所述的一种基于双向apf-rrt*-cpo算法的三维航迹规划方法，其特征在于，步骤（2）中，当节点qnear1处于无障碍区域时，采用目标点节点qgoal对节点qnear1产生的引力fatt1（qnear1）引导树t1的生长方向生成新节点qnew1；
4.根据权利要求2所述的一种基于双向apf-rrt*-cpo算法的三维航迹规划方法，其特征在于，步骤（2）中，当节点qnear2处于无障碍区域时，采用起点qstart对节点qnear2产生的引力fatt1（qnear2）引导树t2的生长方向生成新节点qnew2；
5.根据权利要求3或4所述的一种基于双向apf-rrt*-cpo算法的三维航迹规划方法，其特征在于，步骤（3）中，障碍物检测方法为：将节点与节点连线分成n+1个坐标点，判断坐标...
【专利技术属性】
技术研发人员：甄然，王新卓，武晓晶，孟凡华，郝雷雷，阎鑫英，
申请(专利权)人：河北科技大学，
类型：发明
国别省市：
还没有人留言评论。发表了对其他浏览者有用的留言会获得科技券。