本发明涉及无人机路径规划方法技术领域，尤其涉及一种基于三维模型的无人机路径规划方法。
背景技术：
目前为止px4autopilot这样的开源无人机自动驾驶系统采用的局部路径规划依然使用的是3dvfh 的路径规划算法，但是在实际测试当中发现此算法对于给定目标点需要无人机躲避绕开墙壁等庞大的平面障碍物到达目标点时，这个算法难以行之有效，很多情况下无人机会在一个较小的范围内持续震荡，无法进一步接近目标点，实际测试效果欠佳。
技术实现要素：
本发明旨在解决现有技术的不足，而提供一种基于三维模型的无人机路径规划方法。
本发明为实现上述目的，采用以下技术方案：
一种基于三维模型的无人机路径规划方法，具体步骤如下：
步骤一，通过3dvfh 算法计算出候选路径及各候选路径点的位置；
步骤二，计算出步骤一得到的全部候选路径点的代价值，代价值由以下公式得出
其中，ki表示3dvfh 算法得出的第i条候选路径的候选路径点的代价值，μ1，μ2分别表示公式第一项和第二项的权重，2*μ2≥|μ1|≥μ2，μ1∈[-100,0)，μ2∈(0,100]，设pc表示无人机上一次选择的路径点计算出的三维移动点位置，pl表示相对于pc的上一次选择的路径点计算出的三维移动点位置，pi表示3dvfh 算法得出的第i条候选路径所计算出的三维移动点位置，pt表示目标点的三维位置，δ(x1,x2)表示三维位置x2与x1的欧式距离，p表示通过所有候选路径计算出的所有候选位置点的集合，minx1∈pδ(x1,x2)表示取每个集合p当中的元素x1与一个x2进行δ(x1,x2)运算并取当中的极小值，maxx1∈pδx1,x2)表示取每个集合p当中的元素x1与一个x2进行δ(x1,x2)运算并取当中的极大值，初始规划阶段没有上上个候选点的信息，则代价函数的第一项取值为0；
步骤三，从步骤二的计算结果中找出最大的代价值及其对应的候选路径点，将该候选路径点选择为下一个路径点，并且将上一次选择的路径点设置为上上个选择的路径点路径，将当前所在的路径点设置为上一个候选点，无人机执行当前规划的路径后回到步骤一，如此往复，直到抵达目标点。
进一步的，步骤二中μ1＝-40，μ2＝20。
本发明的有益效果是：通过本发明方法对3dvfh 算法的候选路径点进行更加合理的选择，能够在三维场景下使无人机以更加合理的路径躲避、绕开墙壁等巨大障碍物，且计算速度较快，经对比测试，躲避、绕开墙壁的成功率得到显著提升。
附图说明
图1为本发明执行时的流程图；
图2为完全采用3dvfh 方法时的模拟效果图；
图3为采用本发明方法时的模拟效果图；
以下将结合本发明的实施例参照附图进行详细叙述。
具体实施方式
下面结合实施例对本发明作进一步说明：
实施例一
采用现有技术中的3dvfh 算法中的候选路径选择方法进行无人机翻越5m*0.2m*3m的墙壁测试10次，翻越成功率为30％。
实施例二
一种基于三维模型的无人机路径规划方法，具体步骤如下：
步骤一，通过3dvfh 算法计算出候选路径及各候选路径点的位置；
步骤二，计算出步骤一得到的全部候选路径点的代价值，代价值由以下公式得出
其中，ki表示3dvfh 算法得出的第i条候选路径的候选路径点的代价值，μ1，μ2分别表示公式第一项和第二项的权重，μ1＝-100，μ2＝50，设pc表示无人机上一次选择的路径点计算出的三维移动点位置，pl表示相对于pc的上一次选择的路径点计算出的三维移动点位置，pi表示3dvfh 算法得出的第i条候选路径所计算出的三维移动点位置，pt表示目标点的三维位置，δ(x1,x2)表示三维位置x2与x1的欧式距离，p表示通过所有候选路径计算出的所有候选位置点的集合，minx1∈pδ(x1,x2)表示取每个集合p当中的元素x1与一个x2进行δ(x1,x2)运算并取当中的极小值，maxx1∈pδ(x1,x2)表示取每个集合p当中的元素x1与一个x2进行δ(x1,x2)运算并取当中的极大值，初始规划阶段没有上上个候选点的信息，则代价函数的第一项取值为0；
步骤三，从步骤二的计算结果中找出最大的代价值及其对应的候选路径点，将该候选路径点选择为下一个路径点，并且将上一次选择的路径点设置为上上个选择的路径点路径，将当前所在的路径点设置为上一个候选点，无人机执行当前规划的路径后回到步骤一，如此往复，直到抵达目标点。
采用本方法进行无人机翻越5m*0.2m*3m的墙壁测试10次，翻越成功率为100％。
实施例三
一种基于三维模型的无人机路径规划方法，具体步骤如下：
步骤一，通过3dvfh 算法计算出候选路径及各候选路径点的位置；
步骤二，计算出步骤一得到的全部候选路径点的代价值，代价值由以下公式得出
其中，ki表示3dvfh 算法得出的第i条候选路径的候选路径点的代价值，μ1，μ2分别表示公式第一项和第二项的权重，μ1＝-1，μ2＝1，设pc表示无人机上一次选择的路径点计算出的三维移动点位置，pl表示相对于pc的上一次选择的路径点计算出的三维移动点位置，pi表示3dvfh 算法得出的第i条候选路径所计算出的三维移动点位置，pt表示目标点的三维位置，δ(x1,x2)表示三维位置x2与x1的欧式距离，p表示通过所有候选路径计算出的所有候选位置点的集合，minx1∈pδ(x1,x2)表示取每个集合p当中的元素x1与一个x2进行δ(x1,x2)运算并取当中的极小值，maxx1∈pδ(x1,x2)表示取每个集合p当中的元素x1与一个x2进行δ(x1,x2)运算并取当中的极大值，初始规划阶段没有上上个候选点的信息，则代价函数的第一项取值为0；
步骤三，从步骤二的计算结果中找出最大的代价值及其对应的候选路径点，将该候选路径点选择为下一个路径点，并且将上一次选择的路径点设置为上上个选择的路径点路径，将当前所在的路径点设置为上一个候选点，无人机执行当前规划的路径后回到步骤一，如此往复，直到抵达目标点。
采用本方法进行无人机翻越5m*0.2m*3m的墙壁测试10次，翻越成功率为80％。
实施例四
一种基于三维模型的无人机路径规划方法，具体步骤如下：
步骤一，通过3dvfh 算法计算出候选路径及各候选路径点的位置；
步骤二，计算出步骤一得到的全部候选路径点的代价值，代价值由以下公式得出
其中，ki表示3dvfh 算法得出的第i条候选路径的候选路径点的代价值，μ1，μ2分别表示公式第一项和第二项的权重，μ1＝-100，μ2＝100，设pc表示无人机上一次选择的路径点计算出的三维移动点位置，pl表示相对于pc的上一次选择的路径点计算出的三维移动点位置，pi表示3dvfh 算法得出的第i条候选路径所计算出的三维移动点位置，pt表示目标点的三维位置，δ(x1,x2)表示三维位置x2与x1的欧式距离，p表示通过所有候选路径计算出的所有候选位置点的集合，minx1∈pδ(x1,x2)表示取每个集合p当中的元素x1与一个x2进行δ(x1,x2)运算并取当中的极小值，maxx1∈pδ(x1,x2)表示取每个集合p当中的元素x1与一个x2进行δ(x1,x2)运算并取当中的极大值，初始规划阶段没有上上个候选点的信息，则代价函数的第一项取值为0；
步骤三，从步骤二的计算结果中找出最大的代价值及其对应的候选路径点，将该候选路径点选择为下一个路径点，并且将上一次选择的路径点设置为上上个选择的路径点路径，将当前所在的路径点设置为上一个候选点，无人机执行当前规划的路径后回到步骤一，如此往复，直到抵达目标点。
采用本方法进行无人机翻越5m*0.2m*3m的墙壁测试10次，翻越成功率为80％。
实施例五
一种基于三维模型的无人机路径规划方法，具体步骤如下：
步骤一，通过3dvfh 算法计算出候选路径及各候选路径点的位置；
步骤二，计算出步骤一得到的全部候选路径点的代价值，代价值由以下公式得出
其中，ki表示3dvfh 算法得出的第i条候选路径的候选路径点的代价值，μ1，μ2分别表示公式第一项和第二项的权重，μ1＝-40，μ2＝20，设pc表示无人机上一次选择的路径点计算出的三维移动点位置，pl表示相对于pc的上一次选择的路径点计算出的三维移动点位置，pi表示3dvfh 算法得出的第i条候选路径所计算出的三维移动点位置，pt表示目标点的三维位置，δ(x1,x2)表示三维位置x2与x1的欧式距离，p表示通过所有候选路径计算出的所有候选位置点的集合，minx1∈pδ(x1,x2)表示取每个集合p当中的元素x1与一个x2进行δ(x1,x2)运算并取当中的极小值，maxx1∈pδ(x1,x2)表示取每个集合p当中的元素x1与一个x2进行δ(x1,x2)运算并取当中的极大值，初始规划阶段没有上上个候选点的信息，则代价函数的第一项取值为0；
步骤三，从步骤二的计算结果中找出最大的代价值及其对应的候选路径点，将该候选路径点选择为下一个路径点，并且将上一次选择的路径点设置为上上个选择的路径点路径，将当前所在的路径点设置为上一个候选点，无人机执行当前规划的路径后回到步骤一，如此往复，直到抵达目标点。
采用本方法进行无人机翻越5m*0.2m*3m的墙壁测试10次，翻越成功率为100％。
如图2所示，完全采用3dvfh 进行候选路径点选择时，无人机大多数情况下仅能在一个范围内持续震荡而无法翻越障碍；如图3所示，采用本发明方法进行候选路径点选择时，大多数情况下无人机都可以成功翻越障碍物。因此，通过本发明方法对3dvfh 算法的候选路径点进行更加合理的选择，能够在三维场景下使无人机以更加合理的路径躲避、绕开墙壁等巨大障碍物，且计算速度较快，经对比测试，躲避、绕开墙壁的成功率得到显著提升。
上面结合具体实施例对本发明进行了示例性描述，显然本发明具体实现并不受上述方式的限制，只要采用了本发明的方法构思和技术方案进行的各种改进，或未经改进直接应用于其它场合的，均在本发明的保护范围之内。
1.一种基于三维模型的无人机路径规划方法，其特征在于，具体步骤如下：
步骤一，通过3dvfh 算法计算出候选路径及各候选路径点的位置；
步骤二，计算出步骤一得到的全部候选路径点的代价值，代价值由以下公式得出
其中，ki表示3dvfh 算法得出的第i条候选路径的候选路径点的代价值，μ1，μ2分别表示公式第一项和第二项的权重，2*μ2≥|μ1|≥μ2，μ1∈[-100,0)，μ2∈(0,100]，设pc表示无人机上一次选择的路径点计算出的三维移动点位置，pl表示相对于pc的上一次选择的路径点计算出的三维移动点位置，pi表示3dvfh 算法得出的第i条候选路径所计算出的三维移动点位置，pt表示目标点的三维位置，δ(x1,x2)表示三维位置x2与x1的欧式距离，p表示通过所有候选路径计算出的所有候选位置点的集合，minx1∈pδx1,x2)表示取每个集合p当中的元素x1与一个x2进行δ(x1,x2)运算并取当中的极小值，maxx1∈pδ(x1,x2)表示取每个集合p当中的元素x1与一个x2进行δ(x1,x2)运算并取当中的极大值，初始规划阶段没有上上个候选点的信息，则代价函数的第一项取值为0；
步骤三，从步骤二的计算结果中找出最大的代价值及其对应的候选路径点，将该候选路径点选择为下一个路径点，并且将上一次选择的路径点设置为上上个选择的路径点路径，将当前所在的路径点设置为上一个候选点，无人机执行当前规划的路径后回到步骤一，如此往复，直到抵达目标点。
2.根据权利要求1所述的基于三维模型的无人机路径规划方法，其特征在于，步骤二中μ1＝-40，μ2＝20。
技术总结