ROS中使用A星算法进行路径规划

前言

由于本人这学期修了一门《智能工程》的课程,课程的大作业要求大家自主编写机器人导航程序,包括:路径规划、轨迹规划、控制器、机器人建模等相关程序,因此特在此写下一篇博客,记录开发过程。

这是第一篇博客,用于记录如何将自己编写的A星算法用于ROS中

python编写A星算法

描述

先声明:在使用Python编写A*算法的过程中,深深的体会到了C++的链表、指针的便利性,但奈何已经1年多没使用C++写过程序了,所以还是用了Python来完成的。我编写的A星算法的时间复杂度和空间复杂度肯定都很,希望大家看了轻喷。

由于要用在ROS的map中寻找路径,所以我把A星算法写成了一个类,只需要传进去三个参数,一个是二维的ListROS中规定:可走区域的数值为0,障碍物数值为100,未知领域数值为-1),一个是起始点的坐标,一个是终点的坐标

最后程序会返回一个List,里面的内容是从终点到起点的最短路径,用每一个点的坐标表示。

比如下面是一张地图:

0 1 2 3 4 5 6 7
0 0 0 0 0 0 0 0 0
1 0 0 0 100 0 0 0 0
2 0 0 0 100 0 0 0 0
3 0 0 0 100 0 0 0 0
4 0 0 0 100 0 0 0 0
5 0 0 0 0 0 0 0 0
6 0 0 0 0 0 0 0 0

规定起点为[2,2],终点为[2,4],则返回的内容为:[(2,4),(1,4),(0,3),(1,2),(2,2)]

具体实现

对于地图中每一个像素点,如果这个像素点可到达,那就计算当前的G代价(self.cost_g),H代价(self.cost_h),F代价(self.cost_f)

这个类,相当于C++中的struct

class map_node():
    def __init__(self):
    	# x、y代表该像素点在地图中的横纵坐标
        self.x = 0
        self.y = 0
        # F = G + H
        self.cost_f = 0
        self.cost_g = 0
        self.cost_h = 0
        # 父节点的横纵坐标
        self.parent = [0,0]

主要类:

class find_path():

类初始化函数:

  • extend_map函数是将地图扩展一圈,加一圈障碍物(像素值为1)

  • 声明一个state_map用于保存map中每一个像素点的状态:

    • 2代表已经在open表中
    • 3代表已经在close表中
    • 0代表还没有处理过
  • 起点和终点自然横纵坐标都+=1

  • 初始化一些变量 openlist、closelist

    def __init__(self, map, start, goal):
        # map是一个二维地图, start是起点坐标[],goal是终点坐标[]
        self.map = self.extend_map(map)
        # 2代表在open表中 3代表在close表中
        self.state_map = np.zeros([len(map) + 2, len(map[0]) + 2])
        # print self.map

        self.start = start
        self.start[0] += 1
        self.start[1] += 1
        self.goal = goal
        self.goal[0] += 1
        self.goal[1] += 1
        self.open_list = []
        self.cloase_list = []
        self.path = []
        self.if_reach = False

扩展地图边界的函数

      def extend_map(self, map):
        new_row = np.ones(len(map[0]))
        new_col = np.ones(len(map) + 2)
        x = np.insert(map, 0, new_row, axis=0)
        x = np.insert(x, len(map) + 1, new_row, axis=0)
        x = np.insert(x, 0 , new_col, axis=1)
        x = np.insert(x, len(map[0]) + 1 , new_col, axis=1)
        return x

主要的寻找路径的函数

  • 如果设置的起始点和终点不可达,print 提示,然后直接退出
  • append_around_open函数是把该点周围的8个点加到open表中
  • 把起始点加到close表中
  • 进入循环,利用find_min_cost_f每次寻找open表中最小的cost_f,把它周围的8个点加到open表中,然后把这个节点加到close表,直到找到了到终点的路径,然后利用append_path函数回溯父节点直到回溯到起点。
    def start_find(self):
        #第一次操作,把起点的周围的点指向起点,起点和周围的点加到open list,
        # print "-----start point-----",self.start
        if self.map[self.start[0]][self.start[1]] != 0:
            print "\033[0;31m[E] : Please set the valid start point\033[0m"
            print "value = ", self.map[self.start[0]][self.start[1]]
            return "None"
        if self.map[self
  • 28
    点赞
  • 294
    收藏
    觉得还不错? 一键收藏
  • 16
    评论
智能车项目就是利用人工智能技术使车辆自主感知和决策,并能够安全地规划自己的行驶路线。在智能车项目,c语言是一种经典的编程语言,可以用于开发车辆控制系统。而ROS(机器人操作系统)是一种流行的开源软件平台,为智能车项目提供了许多功能和框架。 在智能车项目实现A*三维路径规划,可以通过结合c语言和ROS进行开发。A*算法是一种常用的启发式搜索算法,可以用于求解最短路径问题。三维路径规划需要考虑车辆在空间的移动和环境的障碍物。 首先,我们可以使用c语言编写A*算法的源码。该源码可以接受起始点和目标点的坐标,并根据环境的障碍物情况进行路径搜索。A*算法通过使用启发式函数来评估路径的优劣,以找到最短路径。 然后,我们将这个A*算法源码整合到ROSROS提供了许多用于机器人感知、决策和控制的功能包和工具。我们可以使用ROS来构建车辆模型、实现对车辆的感知和控制,以及可视化路径规划结果。 在ROS,我们可以定义车辆的运动模型、地图信息和障碍物信息,并使用A*算法源码计算出最佳路径。随着车辆根据路径进行移动,我们可以将实时的位置数据传递给ROS,以便进行路径跟踪和自主导航。 总之,基于c语言实现A*三维路径规划源码,并结合ROS进行智能车项目的开发,可以实现车辆的自主导航和路径规划功能。这样的系统可以使车辆根据实时感知的环境信息,安全、高效地找到最优路径,并完成任务。

“相关推荐”对你有帮助么?

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论 16
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值