基于STM32F103的树莓派ROS小车——全局路径规划之Dijkstra算法

这篇具有很好参考价值的文章主要介绍了基于STM32F103的树莓派ROS小车——全局路径规划之Dijkstra算法。希望对大家有所帮助。如果存在错误或未考虑完全的地方,请大家不吝赐教,您也可以点击"举报违法"按钮提交疑问。

Dijkstra

Dijkstra算法概念:
基本思想:由近到远把所有点的最短路径算出来。
算法解析:从起点向四周辐射,由近到远一层一层遍历所有的点,直到包含目标点所在层级。然后将所有可行路径进行计算比较,筛选出绝对最佳路径。
优点:最终得到的路径一定是最佳路径。
缺点:因为Dijkstra算法需要遍历所有的点,所以程序运行时间较长。

小车路径规划,python,ubuntu

Dijkstra算法过程讲解:
情景假设:从起点开始

1.每次走一格,不能跨格
2.共有8个相邻栅格可以走
3.若走(上/下/左/右)计路程为:2
4.若走(左上/左下/右下/右上)计路程为:3

小车路径规划,python,ubuntu小车路径规划,python,ubuntu

5.设置两个列表(或者数组)
openlist=[ ]        ——存放待确定路径的点
closedlist=[ ]    ——存放已确定路径的点
6.每个点都记录自己的父节点,下标表示
7.为了方便讲解 起点称S点 相邻8个点以A-H称呼
8.已放进closedlist的点不走,因已确定路径

具体过程:

①:起点为S点,计算周围点,父节点为S
②:取一个最小值的点作第二步中心点;从openlist取出,放入closedlist计算中心点周围8个点,累计路程;若点已存在openlist,取路程小的方案。
③:取一个最小值的点作第三步中心点,重复第二步步骤。
④:取一个最小值的点作第四步中心点,重复步骤。
⑤:取一个最小值的点作第五步中心点,重复步骤。
⑥:取一个最小值的点作第六步中心点,重复步骤。

小车路径规划,python,ubuntu

 ......重复步骤

 伪代码:

获取起点坐标、目标点坐标,把起点放进openlist
while (openlist不为空列表)
{    取openlist中路程最小的点为中心点,从openlist剔除,放入closedlist
    if (中心点是目标点)
        {从目标点开始通过父节点反推,提取出路径,break跳出循环}
    else         {遍历相邻的八个点
            {if(点已存在closedlist中  or  遇到障碍物 )
                {跳过,不计算这个点}
            else
                {累计路径
                if(点已存在openlist中)
                    if(累计路程<原来记录的路程)
                        {替换数据}
                else {加入openlist}
            }
        openlist从小到大排序
    }
}

完整代码送上:文章来源地址https://www.toymoban.com/news/detail-615790.html

# -- coding: utf-8 --
import math, sys, pygame, random
from math import *
from pygame import *
import time

Astar = 0
Dijkstra = 1
#模式选择: Dijkstra or Astar
mode = Astar

GAME_LEVEL =1  #障碍物等级,目前可选项为0或1
delay_sec = 0   #算法延时,单位:秒;可设置延时来观察学习路径规划的逐步实现过程
#颜色RGB值
grey = (169, 169, 169)
blue = (0, 0, 255)
red = (255, 0, 0)
black = (0, 0, 0)
green = (0, 255, 0)
yellow = (255, 255, 0)
orange = (255, 165, 0)
bg_color = (255, 255, 255)
purple = (160, 32, 240)
#初始化相关
pygame.init()
screen = pygame.display.set_mode((800, 600))
fpsClock = pygame.time.Clock()

#定义Node类,用于存储每个节点信息
class Node(object):
    def __init__(self, point, parent, d, f):
        super(Node, self).__init__()
        self.point = point      #节点坐标
        self.parent = parent    #父节点对象地址
        self.d = d      #Astar算法下:起点至该节点的确定距离;Dijkstra算法下:起点至该节点的估算距离
        self.f = f      #Astar算法下:f = d + h  d为上述确定距离,h为该节点到目标节点的估算距离(欧几里得距离或曼哈顿距离);Dijkstra算法下此参数置零

#初始化栅格
def init_grid():
    start_point = [0, 0]
    end_point = [0, 600]
    i = 20
    while start_point[0] <= 800:
        pygame.draw.line(screen, grey, start_point, end_point)
        start_point[0] += i
        end_point[0] += i 
    
    start_point = [0, 0]
    end_point = [800, 0]
    while start_point[1] <= 600:
        pygame.draw.line(screen, grey, start_point, end_point)
        start_point[1] += i
        end_point[1] += i 

#判断栅格
def judge_in_grid(p):
    p0 = int(p[0]/20)
    p1 = int(p[1]/20)
    rect1 = ((p0*20, p1*20), (20,20))
    return rect1

#初始化障碍物
def init_obstacles(configNum):
    global rectObs
    rectObs = []
    if configNum == 0:
        rectObs.append(pygame.Rect((300,200),(200,200)))
    if configNum == 1:
        rectObs.append(pygame.Rect((100,100),(100,340)))
        rectObs.append(pygame.Rect((320,240),(160,160)))
        rectObs.append(pygame.Rect((500,60),(200,100)))
    for rect in rectObs:
        pygame.draw.rect(screen, black, rect)

#节点与障碍物碰撞检测
def collides(p):    #check if point collides with the obstacle
    for rect in rectObs:
        if rect.collidepoint(p) == True:
            return True
    return False

#复位函数
def reset():
    screen.fill(bg_color)
    init_grid()
    init_obstacles(GAME_LEVEL)

#文本打印
def text_display(text0, rect, colour, Size):
    fontObj = pygame.font.Font(None,Size)
    textSurfaceObj = fontObj.render(text0,True,colour)
    textRectObj = textSurfaceObj.get_rect()
    textRectObj.center=(10+(rect[0][0]),10+(rect[0][1]))
    screen.blit(textSurfaceObj,textRectObj)

#欧几里得距离,计算量大
def Euclid(p1, p2):
    m = ((p1[0]-p2[0])*(p1[0]-p2[0])+(p1[1]-p2[1])*(p1[1]-p2[1]))**0.5
    return m

#曼哈顿距离,计算量小,简单
def Manhattan(p1, p2):
    m = abs(p1[0]-p2[0])+abs(p1[1]-p2[1])
    return m

#Astar估算函数
def evaluation_Astar(ynode, goalPoint):
    judge_open = False
    judge_close = False
    if collides(ynode.point) == True:
        return
    if ynode.point[0] < 0 or ynode.point[1] < 0:
        return
    ynode.f = ynode.d*1+0.1*Manhattan(ynode.point, goalPoint.point)
    for p in openlist:
        if p.point == ynode.point:
            judge_open = True
            index_open = openlist.index(p)
    for p in closelist:
        if p.point == ynode.point:
            judge_close = True
            index_close = closelist.index(p)
    if judge_close == True:
        return
    if judge_open == False:
        openlist.append(ynode)
        pygame.draw.rect(screen, (255*abs(1-float(ynode.d)*1.2/140), 255*abs(1-float(ynode.d)*1.2/140), 255), (ynode.point, (20, 20)))
        text_display(str(int(ynode.d)), (ynode.point, (20, 20)), black, 15)
        pygame.display.flip()
    else:
        if openlist[index_open].d > ynode.d:
            openlist[index_open] = ynode

#Dijkstra估算函数
def evaluation_Dijkstra(ynode, goalPoint):
    judge_open = False
    judge_close = False
    if collides(ynode.point) == True:
        return
    if ynode.point[0] < 0 or ynode.point[1] < 0:
        return
    for p in closelist:
        if p.point == ynode.point:
            judge_close = True
            index_close = closelist.index(p)
    if judge_close == True:
        return
    for p in openlist:
        if p.point == ynode.point:
            judge_open = True
            index_open = openlist.index(p)
    if judge_open == True:
        if openlist[index_open].d > ynode.d:
            openlist[index_open] = ynode
    else:
        openlist.append(ynode)
        pygame.draw.rect(screen, (255*abs(1-float(ynode.d)*1.2/140), 255*abs(1-float(ynode.d)*1.2/140), 255), (ynode.point, (20, 20)))
        text_display(str(ynode.d), (ynode.point, (20, 20)), black, 15)
        pygame.display.flip()

#程序主要函数
def run_game():
    currentState = 'init'
    initPoseSet = False
    goalPoseSet = False
    global openlist
    openlist = []
    global closelist
    closelist = []
    reset()
    #主循环
    while True:
        #处理不同currentState状态下的工作,其中包括规划路径的实现
        if currentState == 'init':
            print('goal point not yet set')
            pygame.display.set_caption('Select Starting Point and then Goal Point')
            fpsClock.tick(10)
        elif currentState == 'lookingForGoal':
            run = True
            while(len(openlist) != 0):
                for event in pygame.event.get():
                    if event.type == MOUSEBUTTONDOWN :
                        if run == True:
                            run = False
                        else:
                            run = True
                if run == True:
                    xnode = openlist[0]
                    if xnode.point == goalPoint.point:
                        pygame.draw.rect(screen, orange, (xnode.point, (20, 20)))
                        text_display("G",(xnode.point, (20, 20)), black, 32)
                        currentState = 'goalFound'
                        flag = 1
                        goalNode = xnode
                        break
                    closelist.append(xnode)
                    del openlist[0]
                    if delay_sec > 0:
                        pygame.draw.circle(screen, red, (xnode.point[0]+10, xnode.point[1]+10), 2)
                    i = 0
                    while i < 8:
                        if i == 0:
                            ynode = Node((xnode.point[0],xnode.point[1]-20), xnode, xnode.d+2, 0)
                        elif i == 1:
                            ynode = Node((xnode.point[0],xnode.point[1]+20), xnode, xnode.d+2, 0)
                        elif i == 2:
                            ynode = Node((xnode.point[0]-20,xnode.point[1]), xnode, xnode.d+2, 0)
                        elif i == 3:
                            ynode = Node((xnode.point[0]+20,xnode.point[1]), xnode, xnode.d+2, 0)
                        elif i == 4:
                            ynode = Node((xnode.point[0]-20,xnode.point[1]-20), xnode, xnode.d+3, 0)
                        elif i == 5:
                            ynode = Node((xnode.point[0]+20,xnode.point[1]-20), xnode, xnode.d+3, 0)
                        elif i == 6:
                            ynode = Node((xnode.point[0]+20,xnode.point[1]+20), xnode, xnode.d+3, 0)
                        elif i == 7:
                            ynode = Node((xnode.point[0]-20,xnode.point[1]+20), xnode, xnode.d+3, 0)
                        if mode == 0:
                            evaluation_Astar(ynode, goalPoint)
                        elif mode == 1:
                            evaluation_Dijkstra(ynode, goalPoint)
                        i += 1
                    for i in range(0, len(openlist)-1):
                        for j in range(0, len(openlist)-1-i):
                            if mode == 0:
                                data_j = openlist[j].f
                                data_j1 = openlist[j+1].f
                            elif mode == 1:
                                data_j = openlist[j].d
                                data_j1 = openlist[j+1].d
                            if data_j > data_j1:
                                temp = openlist[j]
                                openlist[j] = openlist[j+1]
                                openlist[j+1] = temp
                    pygame.display.flip()
                    for i in range(0, len(openlist)):
                        print(str(openlist[i].point)+"\t--- d = "+str(openlist[i].d)+"  &  f = "+str(openlist[i].f))
                        print("parent:"+str(openlist[i].parent.point))
                    print("-------------------")
                    time.sleep(delay_sec)
                if len(openlist) == 0:
                    print('False to find the path')
        elif currentState == 'goalFound':
            if flag == 1:
                print('goalFound!!!!!!')
                currNode = goalNode.parent
                print("******************")
                print("startPoint = "+(str(initialPoint.point)))
                print("goalPoint = "+(str(goalPoint.point)))
                print("******************")
                for i in range(0, len(closelist)):
                    print(str(closelist[i].point)+"\t--- d = "+str(closelist[i].d)+"  &  f = "+str(closelist[i].f))
                    if i > 0:
                        print("parent:"+str(closelist[i].parent.point))
            while currNode.parent != None:
                pygame.draw.rect(screen, green, (currNode.point, (20, 20)))
                text_display(str(currNode.d), (currNode.point, (20, 20)), black, 15)
                currNode = currNode.parent
                flag = 0
                pygame.display.flip()
                time.sleep(float(delay_sec)/2)
            fpsClock.tick(10)
        #处理鼠标事件
        for event in pygame.event.get():
            if event.type == pygame.QUIT or (event.type == KEYUP and event.key == K_ESCAPE):
                sys.exit()
            if event.type == MOUSEBUTTONDOWN:
                if currentState == 'init':
                    rect = judge_in_grid(event.pos)
                    if initPoseSet == False:
                        if collides(event.pos) == False:
                            initialPoint = Node(rect[0], None, 0, 0)
                            
                            pygame.draw.rect(screen, red, rect)
                            text_display("S",rect, black, 32)
                            initPoseSet = True
                    elif  goalPoseSet == False:
                        if collides(event.pos) == False:
                            if rect[0] != initialPoint.point:
                                goalPoint = Node(rect[0],None, 0, 0)
                                print("******************")
                                print("startPoint = "+(str(initialPoint.point)))
                                print("goalPoint = "+(str(goalPoint.point)))
                                print("******************")
                                if mode == 0:
                                    initialPoint.f = initialPoint.d+0.1*Manhattan(goalPoint.point, initialPoint.point)
                                openlist.append(initialPoint)
                                pygame.draw.rect(screen, orange, rect)
                                text_display("G",rect, black, 32)
                                goalPoseSet = True
                                currentState = 'lookingForGoal'
                else:
                    currentState = 'init'
                    initPoseSet = False
                    goalPoseSet = False
                    openlist = []
                    closelist = []
                    reset()
        init_grid()
        pygame.display.flip()
        
if __name__ == '__main__':
    run_game()

到了这里,关于基于STM32F103的树莓派ROS小车——全局路径规划之Dijkstra算法的文章就介绍完了。如果您还想了解更多内容,请在右上角搜索TOY模板网以前的文章或继续浏览下面的相关文章,希望大家以后多多支持TOY模板网!

本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如若转载,请注明出处: 如若内容造成侵权/违法违规/事实不符,请点击违法举报进行投诉反馈,一经查实,立即删除!

领支付宝红包 赞助服务器费用

相关文章

  • 从零开始制作STM32F103RCT6小车(一)

            仅以此系列给实验室的学弟学妹作为小车制作教程来使用,后续的内容我会在这个暑假陆续更新出来,本篇的内容是新建一个适用于STM32F103RCT6的工程         接下来的操作几乎是基于STM32F1xx系列的固件库,这里我给大家列出链接 STM32F1xx系列固件库               

    2023年04月08日
    浏览(43)
  • 移植FreeRTOS的STM32F103双轮平衡小车(开源,代码文末)

    耗时大概三四天吧,主要时间还是花在硬件方面上, 目录 引言 1、系统概述 1.1、设计任务 1.2、设计要求 1.3、硬件清单 2、方案设计与论证 2.1、芯片选择方案芯片 2.2 、系统概述 2.3、设计要求 2.4、系统总体设计 2.5、重要功能模块程序实现原理分析 2.5.1、MPU6050模块的介绍 小

    2024年01月20日
    浏览(35)
  • STM32超级蓝牙小车——基于STM32F103C8T6的多功能蓝牙小车(PID循迹、跟踪、有源蜂鸣器播放音乐、蓝牙遥控、AD采集+DMA转运等超多元素小车)

    一、项目时间:2023.7.24~11.26 二、实现效果:通过蓝牙控制小车运动与模式转换                         模式一:循迹模式                         模式二:跟踪模式                         模式三:音乐模式                         模式四:控制运动模式 三、使

    2024年02月04日
    浏览(45)
  • STM32F103C8T6智能小车舵机超声波避障

    目录 一、定时器 计数和定时器中断  输出比较(PWM) 二、 舵机 三、超声波测距 四、主函数 总结 推荐的STM32学习链接:  [6-1] TIM定时中断_哔哩哔哩_bilibili [6-1] TIM定时中断是STM32入门教程-2022持续更新中的第13集视频,该合集共计29集,视频收藏或关注UP主,及时了解更多相关视

    2024年02月15日
    浏览(38)
  • stm32f103 简易4路红外寻迹小车(2)----2023西南交大电赛校赛(pcb原理图,代码及分析)

    目录 一。材料准备。 二。PCB原理图  三。逻辑状态图 四。代码部分 五。文件下载: 接上:stm32f103 简易4路红外寻迹小车(1)----2023西南交大电赛校赛(含stm32中文资料) 小车测试视频: stm32小车寻迹小车 材料资料图片见上:stm32f103 简易4路红外寻迹小车(1)----2023西南交大

    2024年02月16日
    浏览(43)
  • 基于stm32F103的座面声控台灯

            设计一个放置在桌面使用的台灯,使用220v交流电供电。具备显示屏能够实时显示日期(年、月、日和星期),时间(小时、分钟、秒)和温度(摄氏度);能够通过语音交互播报实时日期、时间或者温度;能够通过语音交互控制桌面台灯的开启与关闭(或者明暗

    2024年04月29日
    浏览(58)
  • 基于STM32F103——AS608指纹模块+串口打印

    最近用STM32F103做一个智能门锁小玩意,其中用到指纹模块,我这里也单独的写一下笔记,不过我只是了解了基本的,就是我做门禁卡要用到的几个东西,其它还没了解。为了方便,做一下记录。我这里没有用到按键和显示屏,所以还是串口输出输入来控制了 哈哈哈哈 这里就

    2023年04月09日
    浏览(40)
  • 基于stm32F103的蜂鸣器周期发声实验

    蜂鸣器作为一种声音报警器件,应用广泛。本实验基于stm32F103单片机,通过控制蜂鸣器的IO口电平电压,使其周期性地进行电平翻转,从而驱动蜂鸣器发出周期性的鸣叫声。该实验主要运用了stm32的GPIO和定时器TIM的相关功能,不仅可以巩固这些外设的使用,也可以通过改变时间参数,控

    2024年02月21日
    浏览(36)
  • 2021校赛基于stm32f103多功能台灯

    起源 又到了一学期一次的校内电子设计大赛,又到了激动人心的时刻每次电子设计大赛都会出现各种大佬展现他们的作品,对于我这种菜鸟也只能默默观望,但是呢,积极参与还是要有的,记得上一次参加做的基于51的避障小车直接买的套件焊好 然后在烧入程序就直接上战场

    2023年04月20日
    浏览(43)
  • 基于STM32F103HAL库的声音定位系统

    这是一道学校出的电赛题目,要求在100*100cm的平面上实现定位实现声音定位。由于一米太大了,我们就做了40cm的,下面的讲解我按照40厘米的写。用到的处理器是stm32f103c8t6接下来分享一下调试心得。 硬件部分需要制作发声装置和接收装置,详细可以

    2024年02月14日
    浏览(46)

觉得文章有用就打赏一下文章作者

支付宝扫一扫打赏

博客赞助

微信扫一扫打赏

请作者喝杯咖啡吧~博客赞助

支付宝扫一扫领取红包,优惠每天领

二维码1

领取红包

二维码2

领红包