CN112484726B - 一种基于三维模型的无人机路径规划方法 - Google Patents

一种基于三维模型的无人机路径规划方法 Download PDF

Info

Publication number
CN112484726B
CN112484726B CN202011057008.3A CN202011057008A CN112484726B CN 112484726 B CN112484726 B CN 112484726B CN 202011057008 A CN202011057008 A CN 202011057008A CN 112484726 B CN112484726 B CN 112484726B
Authority
CN
China
Prior art keywords
path
point
candidate
aerial vehicle
unmanned aerial
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN202011057008.3A
Other languages
English (en)
Other versions
CN112484726A (zh
Inventor
佘换林
赵满超
王志敏
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Tianjin Base Technology Co ltd
Original Assignee
Tianjin Base Technology Co ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Tianjin Base Technology Co ltd filed Critical Tianjin Base Technology Co ltd
Priority to CN202011057008.3A priority Critical patent/CN112484726B/zh
Publication of CN112484726A publication Critical patent/CN112484726A/zh
Application granted granted Critical
Publication of CN112484726B publication Critical patent/CN112484726B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Management, Administration, Business Operations System, And Electronic Commerce (AREA)

Abstract

本发明提供一种基于三维模型的无人机路径规划方法，具体步骤如下：步骤一，通过3DVFH+算法计算出候选路径及各候选路径点的位置；步骤二，计算出步骤一得到的全部候选路径点的代价值；步骤三，找出最大的代价值及其对应的候选路径点，将该候选路径点选择为下一个路径点，并且将上一次选择的路径点设置为上上个选择的路径点路径，将当前所在的路径点设置为上一个候选点，无人机执行当前规划的路径后回到步骤一。通过本发明方法对3DVFH+算法的候选路径点进行更加合理的选择，能够在三维场景下使无人机以更加合理的路径躲避、绕开墙壁等巨大障碍物，且计算速度较快，经对比测试，躲避、绕开墙壁的成功率得到显著提升。

Description

一种基于三维模型的无人机路径规划方法
技术领域
本发明涉及无人机路径规划方法技术领域，尤其涉及一种基于三维模型的无人机路径规划方法。
背景技术
目前为止PX4 autopilot这样的开源无人机自动驾驶系统采用的局部路径规划依然使用的是3DVFH+的路径规划算法，但是在实际测试当中发现此算法对于给定目标点需要无人机躲避绕开墙壁等庞大的平面障碍物到达目标点时，这个算法难以行之有效，很多情况下无人机会在一个较小的范围内持续震荡，无法进一步接近目标点，实际测试效果欠佳。
发明内容
本发明旨在解决现有技术的不足，而提供一种基于三维模型的无人机路径规划方法。
本发明为实现上述目的，采用以下技术方案：
一种基于三维模型的无人机路径规划方法，具体步骤如下：
步骤一，通过3DVFH+算法计算出候选路径及各候选路径点的位置；
步骤二，计算出步骤一得到的全部候选路径点的代价值，代价值由以下公式得出
Figure BDA0002711115390000011
其中，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+算法计算出候选路径及各候选路径点的位置；
步骤二，计算出步骤一得到的全部候选路径点的代价值，代价值由以下公式得出
Figure BDA0002711115390000031
其中，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+算法计算出候选路径及各候选路径点的位置；
步骤二，计算出步骤一得到的全部候选路径点的代价值，代价值由以下公式得出
Figure BDA0002711115390000032
其中，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+算法计算出候选路径及各候选路径点的位置；
步骤二，计算出步骤一得到的全部候选路径点的代价值，代价值由以下公式得出
Figure BDA0002711115390000041
其中，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+算法计算出候选路径及各候选路径点的位置；
步骤二，计算出步骤一得到的全部候选路径点的代价值，代价值由以下公式得出
Figure BDA0002711115390000051
其中，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+算法的候选路径点进行更加合理的选择，能够在三维场景下使无人机以更加合理的路径躲避、绕开墙壁等巨大障碍物，且计算速度较快，经对比测试，躲避、绕开墙壁的成功率得到显著提升。
上面结合具体实施例对本发明进行了示例性描述，显然本发明具体实现并不受上述方式的限制，只要采用了本发明的方法构思和技术方案进行的各种改进，或未经改进直接应用于其它场合的，均在本发明的保护范围之内。

Claims (2)

1.一种基于三维模型的无人机路径规划方法，其特征在于，具体步骤如下：
步骤一，通过3DVFH+算法计算出候选路径及各候选路径点的位置；
步骤二，计算出步骤一得到的全部候选路径点的代价值，代价值由以下公式得出
Figure FDA0002711115380000011
其中，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。
CN202011057008.3A 2020-09-30 2020-09-30 一种基于三维模型的无人机路径规划方法 Active CN112484726B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202011057008.3A CN112484726B (zh) 2020-09-30 2020-09-30 一种基于三维模型的无人机路径规划方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202011057008.3A CN112484726B (zh) 2020-09-30 2020-09-30 一种基于三维模型的无人机路径规划方法

Publications (2)

Publication Number Publication Date
CN112484726A CN112484726A (zh) 2021-03-12
CN112484726B true CN112484726B (zh) 2022-08-30

Family

ID=74921016

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202011057008.3A Active CN112484726B (zh) 2020-09-30 2020-09-30 一种基于三维模型的无人机路径规划方法

Country Status (1)

Country Link
CN (1) CN112484726B (zh)

Families Citing this family (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115016546B (zh) * 2022-08-10 2022-10-28 中国科学院自动化研究所 无人机三维路径规划方法、装置、电子设备和存储介质
CN115930967A (zh) * 2023-01-03 2023-04-07 浙江大华技术股份有限公司 一种路径规划方法、装置以及计算机存储介质
CN118392195A (zh) * 2024-07-01 2024-07-26 天津攸信智能科技有限公司 一种基于改进3dvfh算法的无人机路径数据规划方法

Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103926925A (zh) * 2014-04-22 2014-07-16 江苏久祥汽车电器集团有限公司 一种基于改进的vfh算法的定位与避障方法及机器人
CN105807769A (zh) * 2016-03-09 2016-07-27 哈尔滨工程大学 无人水下航行器ivfh避碰方法
CN107289938A (zh) * 2017-05-27 2017-10-24 中国北方车辆研究所 一种地面无人平台局部路径规划方法
CN108444482A (zh) * 2018-06-15 2018-08-24 东北大学 一种无人机自主寻路避障方法及系统
CN108549385A (zh) * 2018-05-22 2018-09-18 东南大学 一种结合a*算法和vfh避障算法的机器人动态路径规划方法
CN109828566A (zh) * 2019-01-30 2019-05-31 华南理工大学 一种水面无人艇自主航行方法
CN109919026A (zh) * 2019-01-30 2019-06-21 华南理工大学 一种水面无人艇局部路径规划方法

Patent Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103926925A (zh) * 2014-04-22 2014-07-16 江苏久祥汽车电器集团有限公司 一种基于改进的vfh算法的定位与避障方法及机器人
CN105807769A (zh) * 2016-03-09 2016-07-27 哈尔滨工程大学 无人水下航行器ivfh避碰方法
CN107289938A (zh) * 2017-05-27 2017-10-24 中国北方车辆研究所 一种地面无人平台局部路径规划方法
CN108549385A (zh) * 2018-05-22 2018-09-18 东南大学 一种结合a*算法和vfh避障算法的机器人动态路径规划方法
CN108444482A (zh) * 2018-06-15 2018-08-24 东北大学 一种无人机自主寻路避障方法及系统
CN109828566A (zh) * 2019-01-30 2019-05-31 华南理工大学 一种水面无人艇自主航行方法
CN109919026A (zh) * 2019-01-30 2019-06-21 华南理工大学 一种水面无人艇局部路径规划方法

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
3DVFH+: Real-time three-dimensional obstacle avoidance using an Octomap;Vanneste S, Bellekens B, Weyn M;《STAF 2014》;20141231;第1-12页 *
VFH*: Local Obstacle Avoidance with Look-Ahead Verification;Iwan Ulrich， Johann Borenstein;《Proceedingsof the 2000 IEEE international Conference on Robotics& Automation》;20001231;第2505-2551页 *
一种移动机器人路径规划的优化方法;李燊等;《新 型 工 业 化》;20200531;第10卷(第5期);第93-86页 *

Also Published As

Publication number Publication date
CN112484726A (zh) 2021-03-12

Similar Documents

Publication Publication Date Title
CN112484726B (zh) 一种基于三维模型的无人机路径规划方法
Faust et al. Prm-rl: Long-range robotic navigation tasks by combining reinforcement learning and sampling-based planning
Hwangbo et al. Efficient two-phase 3D motion planning for small fixed-wing UAVs
CN110850873B (zh) 无人船路径规划方法、装置、设备和存储介质
Fan et al. Crowdmove: Autonomous mapless navigation in crowded scenarios
CN109657863B (zh) 一种基于萤火虫算法的无人船全局路径动态优化方法
CN112304314B (zh) 一种分布式多机器人的导航方法
CN113296514B (zh) 一种基于稀疏带状结构的局部路径优化方法及系统
Gunawan et al. Smoothed a-star algorithm for nonholonomic mobile robot path planning
CN115061499B (zh) 无人机控制方法及无人机控制装置
CN113433937A (zh) 基于启发式探索的分层导航避障系统、分层导航避障方法
CN112965471A (zh) 一种考虑角速度约束和改进斥力场的人工势场路径规划方法
Dharmasiri et al. Novel implementation of multiple automated ground vehicles traffic real time control algorithm for warehouse operations: djikstra approach
CN113359808A (zh) 一种无人机电力巡检多级路径规划方法及相关装置
CN112857370A (zh) 一种基于时序信息建模的机器人无地图导航方法
CN113467483B (zh) 动态环境中基于时空栅格地图的局部路径规划方法及装置
CN117359643A (zh) 一种多模态元学习机器人自我定位系统及其方法
Regier et al. Improving navigation with the social force model by learning a neural network controller in pedestrian crowds
Helble et al. 3-d path planning and target trajectory prediction for the oxford aerial tracking system
CN111752295B (zh) 无人机飞行轨迹规划方法及相关装置
CN117419716A (zh) 无人机三维路径规划方法、系统、存储介质和电子设备
Uçan et al. Using genetic algorithms for navigation planning in dynamic environments
CN114995400B (zh) 基于混合微粒群算法的移动机器人路径规划方法及系统
CN113959446A (zh) 一种基于神经网络的机器人自主物流运输导航方法
Han et al. Pseudospectral method-based safe motion planning for quadrotors in a cluttered environment

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
CB02 Change of applicant information

Address after: 300467-6f-055, area B2, animation building, No. 126, animation Middle Road, Zhongxin Tianjin eco city, Binhai New Area, Tianjin

Applicant after: Tianjin Base Technology Co.,Ltd.

Address before: Room 507, door 1, building 18, Ruihang Plaza, northeast of Central Avenue and East 2nd Road, Tianjin Binhai New Area pilot free trade zone (Airport Economic Zone) (Tianjin Jincai shaojiao Business Secretary Service Co., Ltd. trusteeship number jcsj-297)

Applicant before: Tianjin Base Technology Co.,Ltd.

CB02 Change of applicant information
GR01 Patent grant
GR01 Patent grant