python无人机路径规划算法_RRT算法在Python中的实现,快速,拓展,随机,树

"""

《基于智能优化与RRT算法的无人机任务规划方法研究》博士论文

《基于改进人工势场法的路径规划算法研究》硕士论文

"""

import matplotlib.pyplot as plt

import random

import math

import copy

show_animation = True

class Node(object):

"""

RRT Node

"""

def __init__(self, x, y):

self.x = x

self.y = y

self.parent = None

class RRT(object):

"""

Class for RRT Planning

"""

def __init__(self, start, goal, obstacle_list, rand_area):

"""

Setting Parameter

start:Start Position [x,y]

goal:Goal Position [x,y]

obstacleList:obstacle Positions [[x,y,size],...]

randArea:random sampling Area [min,max]

"""

self.start = Node(start[0], start[1])

self.end = Node(goal[0], goal[1])

self.min_rand = rand_area[0]

self.max_rand = rand_area[1]

self.expandDis = 1.0

self.goalSampleRate = 0.05 # 选择终点的概率是0.05

self.maxIter = 500

self.obstacleList = obstacle_list

self.nodeList = [self.start]

def random_node(self):

"""

产生随机节点

:return:

"""

node_x = random.uniform(self.min_rand, self.max_rand)

node_y = random.uniform(self.min_rand, self.max_rand)

node = [node_x, node_y]

return node

@staticmethod

def get_nearest_list_index(node_list, rnd):

"""

:param node_list:

:param rnd:

:return:

"""

d_list = [(node.x - rnd[0]) ** 2 + (node.y - rnd[1]) ** 2 for node in node_list]

min_index = d_list.index(min(d_list))

return min_index

@staticmethod

def collision_check(new_node, obstacle_list):

a = 1

for (ox, oy, size) in obstacle_list:

dx = ox - new_node.x

dy = oy - new_node.y

d = math.sqrt(dx * dx + dy * dy)

if d <= size:

a = 0 # collision

return a # safe

def planning(self):

"""

Path planning

animation: flag for animation on or off

"""

while True:

# Random Sampling

if random.random() > self.goalSampleRate:

rnd = self.random_node()

else:

rnd = [self.end.x, self.end.y]

# Find nearest node

min_index = self.get_nearest_list_index(self.nodeList, rnd)

# print(min_index)

# expand tree

nearest_node = self.nodeList[min_index]

# 返回弧度制

theta = math.atan2(rnd[1] - nearest_node.y, rnd[0] - nearest_node.x)

new_node = copy.deepcopy(nearest_node)

new_node.x += self.expandDis * math.cos(theta)

new_node.y += self.expandDis * math.sin(theta)

new_node.parent = min_index

if not self.collision_check(new_node, self.obstacleList):

continue

self.nodeList.append(new_node)

# check goal

dx = new_node.x - self.end.x

dy = new_node.y - self.end.y

d = math.sqrt(dx * dx + dy * dy)

if d <= self.expandDis:

print("Goal!!")

break

if True:

self.draw_graph(rnd)

path = [[self.end.x, self.end.y]]

last_index = len(self.nodeList) - 1

while self.nodeList[last_index].parent is not None:

node = self.nodeList[last_index]

path.append([node.x, node.y])

last_index = node.parent

path.append([self.start.x, self.start.y])

return path

def draw_graph(self, rnd=None):

"""

Draw Graph

"""

print('aaa')

plt.clf() # 清除上次画的图

if rnd is not None:

plt.plot(rnd[0], rnd[1], "^g")

for node in self.nodeList:

if node.parent is not None:

plt.plot([node.x, self.nodeList[node.parent].x], [

node.y, self.nodeList[node.parent].y], "-g")

for (ox, oy, size) in self.obstacleList:

plt.plot(ox, oy, "sk", ms=10*size)

plt.plot(self.start.x, self.start.y, "^r")

plt.plot(self.end.x, self.end.y, "^b")

plt.axis([self.min_rand, self.max_rand, self.min_rand, self.max_rand])

plt.grid(True)

plt.pause(0.01)

def draw_static(self, path):

"""

画出静态图像

:return:

"""

plt.clf() # 清除上次画的图

for node in self.nodeList:

if node.parent is not None:

plt.plot([node.x, self.nodeList[node.parent].x], [

node.y, self.nodeList[node.parent].y], "-g")

for (ox, oy, size) in self.obstacleList:

plt.plot(ox, oy, "sk", ms=10*size)

plt.plot(self.start.x, self.start.y, "^r")

plt.plot(self.end.x, self.end.y, "^b")

plt.axis([self.min_rand, self.max_rand, self.min_rand, self.max_rand])

plt.plot([data[0] for data in path], [data[1] for data in path], '-r')

plt.grid(True)

plt.show()

def main():

print("start RRT path planning")

obstacle_list = [

(5, 1, 1),

(3, 6, 2),

(3, 8, 2),

(1, 1, 2),

(3, 5, 2),

(9, 5, 2)]

# Set Initial parameters

rrt = RRT(start=[0, 0], goal=[8, 9], rand_area=[-2, 10], obstacle_list=obstacle_list)

path = rrt.planning()

print(path)

# Draw final path

if show_animation:

plt.close()

rrt.draw_static(path)

if __name__ == '__main__':

main()

ff805ef60d0ec3ab2b871a013e7dd4da.png

6ca8ca8462a1556e02e8500b6339b900.png

1dff98b62474d0fa368d8aab1394057b.png

本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如若转载,请注明出处:http://www.mzph.cn/news/532883.shtml

如若内容造成侵权/违法违规/事实不符,请联系多彩编程网进行投诉反馈email:809451989@qq.com,一经查实,立即删除!

相关文章

uc3842改可调电源教程_明纬开关电源改可调详细教程

1.拆除启动电阻&#xff0c;电路板是R12R59R58R9 四个150K的电阻。2.拆除原494供电&#xff0c;去掉J22一根跳线&#xff0c;12V辅助电源正极接J22到494一端&#xff0c;负极接地。3.去掉J15更换为20K电阻&#xff0c;同时拆掉变压器下面的ZD1过压保护&#xff0c;输出可达到50…

duino例程 stm32_stm32duino

{"data":{"id":"8000-000000437045-0","name":"SEO专题页栏目分发组","type":"1","position":"8000-000000004003-0","status":1,"linkList":[{"id"…

java webservice ip_通过Web Service实现IP地址查询功能的示例

实例01 实现一个简单的Web服务访问本实例将实现IP地址查询接口服务&#xff0c;根据用户传入的IP地址返回IP所在的省、市、地区&#xff0c;实例中将会用到IP地址库用于查询信息&#xff0c;由于数据较多&#xff0c;所以读者可在光盘资源文件中直接附加数据库文件&#xff0c;…

java默认数组值_数组元素默认的初始值都是什么

在Java中&#xff0c;使用数组时&#xff0c;如果为数组分配了内存空间&#xff0c;但是没有为数组元素指定初始值&#xff0c;系统会自动为数组元素指定初始值。数组元素的初始值与数组的数据类型有关&#xff0c;对于不同数据类型的数组&#xff0c;其数组元素的初始值是不一…

java 异步通信处理_java异步通信

在Merlin之前,编写Socket程序是比较繁琐的工作.因为输入输出都必须同步.这样,对于多客户端客户/服务器模式,不得不使用多线程.即为每个连接的客户都分配一个线程来处理输入输出.由此而带来的问题是可想而知的.程序员不得不为了避免死锁,线程安全等问题,进行大量的编码和测试.很…

Java统计做题正确率_ResNet:训练期间的准确率为100%,但使用相同数据的预测准确率为33%...

我之前遇到过类似的问题&#xff0c;但解决方案非常简单 . 你需要增加时代数 . 这是1000个纪元后的输出[[ 9.99999881e-01 8.58182432e-08 9.54004670e-12][ 8.58779623e-20 9.99999881e-01 6.76907632e-08][ 2.12900631e-26 4.09224481e-34 1.00000000e00]]这是培训日志..Epoc…

java成员初始化顺序_Java成员初始化顺序

1. 初始化顺序在类的内部&#xff0c;变量定义的先后顺序决定了初始化的顺序。即使变量散布于方法定义之间&#xff0c;他们仍会在任何方法(包括构造器)被调用之前初始化。2. 静态成员初始化顺序1⃣️初始化类的静态成员或者静态块&#xff0c;静态初始化只在Class对象首次加载…

java五星好评点评器_亲,麻烦给个五星好评!—RatingBar

引言上一篇的CheckBox已经让大家越来越接近实战演练了&#xff0c;本章我们继续分享干货给大家&#xff0c;今天介绍一个实用的UI控件RatingBar(星级评分条)&#xff0c;对于使用过电商APP(某东&#xff0c;某宝等)的小伙伴们来说&#xff0c;应该不会陌生。在对商品进行评价时…

java激光推送ios_关于ios极光推送server端注意的地方

今天试用了极光推送API用它是因为&#xff0c;大多数人说它的文档是最全的&#xff0c;但是用过之后&#xff0c;发现关于IOS的文档&#xff0c;还是很不够&#xff0c;导致走了一点弯路&#xff01;特别是服务端的代码&#xff1a;https://github.com/jpush/jpush-api-java-cl…

日是这一年的等几天Java代码_java中计算指定日期是一年的第几天的方法

Java输入日期计算是这年的第几天&#xff1a;思路通过年份区分出是闰年还是平年&#xff0c;平年 2 月 28 天&#xff0c;闰年 2 月 29 天&#xff1b;1、3、5、7、8、10、12 月份 31 天其余月份均为 30 天&#xff1b;然后将每个月的天数相加即可&#xff0c;注意如果输入的是…

[2021-CVPR] Fine-grained Angular Contrastive Learning with Coarse Labels 论文简析

[2021-CVPR] Fine-grained Angular Contrastive Learning with Coarse Labels 论文简析 论文地址&#xff1a;https://arxiv.org/abs/2012.03515 代码地址&#xff1a;https://github.com/guybuk/ANCOR 首先通俗地介绍一下细粒度&#xff08;fine-grained&#xff09;&#…

orcle mysql 查询_Oracle与Mysql的高级查询与难点sql

一、连接查询 1. 内连接 内连接用于返回满足连接条件的所有记录。默认情况下&#xff0c;在执行连接查询时如果没有指定任何连接操作符&#xff0c;那么这些连接查询都属于内连接。 Sql 代码 1. SELECT a.dname,b.ename from depta,empb where a.deptnob.deptno and a.deptno10…

[2020-AAAI] Revisiting Image Aesthetic Assessment via Self-Supervised Feature Learning 论文简析

[2020-AAAI] Revisiting Image Aesthetic Assessment via Self-Supervised Feature Learning 论文简析 论文链接&#xff1a;https://arxiv.org/abs/1911.11419 本文探索从自监督的角度进行美学评估。基于一个基本的动机&#xff1a;一个好的美学特征表示应该能够辨别出不同的…

java9 堆外内存_java堆外内存泄漏排查

当考虑Java中的内存泄漏时&#xff0c;我们通常会考虑Java堆泄漏&#xff0c;即在堆中分配的对象没有被垃圾收集。这是我在处理一台服务器内存泄漏时的想法&#xff0c;但我即将经历的远超出我的想象。症状&#xff1a;运行Vertx应用程序(没有交换分区)的生产服务器被Linux内存…

[2020-CVPR] Dynamic Region-Aware Convolution 论文简析

[2020-CVPR] Dynamic Region-Aware Convolution 论文简析 论文地址&#xff1a;https://arxiv.org/abs/2003.12243 参考代码地址&#xff08;非官方&#xff09;&#xff1a;https://github.com/shallowtoil/DRConv-PyTorch 代码笔者自己试了一下&#xff0c;应该是可以的&…

java activity模式_Activity的启动模式

Android系统采用任务栈的方式来管理Activity实例。栈是后进先出的数据结构。通常一个应用程序对应一个任务栈&#xff0c;默认情况下&#xff0c;每启动一个Activity都会入栈&#xff0c;处于栈顶位置。用户操作的永远都是栈顶的Activity。Activity可以层叠摆放&#xff0c;每启…

Python 中的可执行对象 eval,exec 和 compile与其在深度学习训练中的应用实例

Python 中的可执行对象 eval&#xff0c;exec 和 compile 与其在深度学习训练中的应用实例 eval 计算指定表达式的值。也就是说它要执行的python代码只能是单个表达式&#xff08;注意eval不支持任何形式的赋值操作&#xff09;&#xff0c;而不能是复杂的代码逻辑。 eval(s…

php写简单接口_php写接口的日常

php写接口的日常/*评论列表*/public function commentListW(){$base new Base();$info $base->getUserByToken();$shop_id $info[shop_id];$page $this->data[page]?:1;$pagesize $this->data[pagesize]?:C(ROLLPAGE);$search $this->data[search];$and &…

mmdetection 使用笔记 01: 安装与简单的推理demo

mmdetection 使用笔记 01: 安装与简单的推理demo mmdetection是来自商汤和港中文联合实验室openmmlab推出的目标检测工具包&#xff0c;与其同系列的还有基础视觉包mmcv&#xff0c;图像分类mmclassification&#xff0c;还有mmaction&#xff0c;mmaction2等等。 今天第一次…

php无限评论回复_php实现无限级评论功能_后端开发

php去除数组的键名的方法_后端开发在php中可以使用“array_values()”函数去除数组的键名&#xff0c;该函数返回包含数组中所有的值的数组&#xff0c;其语法是“array_values(array)”&#xff0c;其参数“array”表示规定的数组&#xff0c;返回值是包含数组中所有的值的数组…