全国大学生数学建模大赛模拟测试选拔题——移动机器人路径规划

移动机器人路径规划是机器人学的一个重要研究领域。 它要求机器人依据某个或某些优化原则(如最小能量消耗、最 短行走路线、最短行走时间等),在其工作空间中找到一条从 起始状态到目标状态能避开障碍物的最优路径。

机器人路径规划问题可以建模为一个有约束的优化问 题，都要完成路径规划、定位和避障等任务。

如上图所示正方形地板边长为1*1,整体为20*20块

地板的地面上，深蓝色为障碍物无法通行，浅色为可通行 部分，从1*1入，从20*20出：

(1)选择合适算法计算出最优路径，论证路径的优越 性。(30分)

(2)利用绘图工具在地图上绘制所求得的最优路径。 (10分)

[提示词：矩阵、蚁群]

基于蚁群算法的最优路径规划在20x20网格地图的应用研究

摘  要

本文研究了移动机器人在具有障碍物的工作空间中的路径规划问题。针对一个20x20的正方形地板环境，其中深蓝色区域代表障碍物，浅色区域代表可通行区域，我们提出了一种基于蚁群算法（Ant Colony Optimization, ACO）的路径规划方法。该方法旨在找到一条从起点（1,1）到终点（20,20）的最优路径，同时满足避开所有障碍物的约束条件。通过仿真实验，我们验证了算法的有效性，并分析了所得路径的优越性。

• 问题分析
  1. 问题背景

移动机器人路径规划是机器人学中的核心问题之一，它对于提高机器人的自主导航能力和工作效率具有重要意义。在复杂环境中，机器人需要能够快速、准确地规划出一条无碰撞的最优路径。本文采用蚁群算法来解决这一问题，该算法以其强大的全局搜索能力和鲁棒性在路径规划领域得到了广泛应用。

1. 1. 问题提出

将20x20的正方形地板划分为等大的网格，每个网格代表一个单位面积。深蓝色网格表示障碍物，机器人无法通行；浅色网格表示可通行区域。起点位于网格（1,1），终点位于网格（20,20）。

• 模型假设与符号说明

2.1模型基本假设

1. 假设地图环境是静态的，即障碍物位置固定不变，且机器人在移动过程中不会改变环境状态。
2. 整个20x20的地图被均匀划分为400个网格，每个网格的大小相同，且只有两种状态：可通行（浅色）和不可通行（深蓝色，即障碍物）。

（3）假设只有一个移动机器人在地图中执行路径规划任务。

（4）机器人能够感知到其所在网格及其相邻网格的状态（是否可通行）。

（5）蚁群算法参数固定：在仿真实验中，蚁群算法的相关参数（如信息素蒸发率、信息素强度、蚂蚁数量等）保持固定，以便于结果的可比性和分析。

2.2符号说明

表1 符号说明

• 问题一模型建立与求解

3.1求解思路

采用蚁群算法进行路径规划，主要思路是模拟蚂蚁觅食的行为，通过信息素的正反馈机制来引导蚂蚁找到从起点到终点的最优路径。每只蚂蚁在移动过程中会释放信息素，并倾向于选择信息素浓度较高的路径。同时，为了避免陷入局部最优解，引入信息素蒸发机制来降低旧路径上的信息素浓度。

3.2模型建立

通过蚁群算法建立模型，在每个迭代开始之前，所有路径上的信息素都会按照一定比例减少，以模拟信息素的自然消散。

当移动机器人完成一次路径构建后，它们会根据路径的长度或质量在路径上沉积一定量的信息素。

对于

第k次路径，其沉积的信息素量可以定义为：

移动机器人在选择下一个移动方向时，会根据当前位置的信息素浓度和启发式信息来计算选择概率：

3.3得出结果

通过多次迭代，蚁群算法会逐渐收敛到一条或几条较优的路径。在每次迭代后，可以记录当前找到的最短路径长度和对应的路径。当算法满足终止条件（如达到最大迭代次数、最短路径长度不再显著变化等）时，输出最终的最优路径。

生成对应的地图：

表一：模型结果

图一：路径结果地图（左边不考虑斜线、右边考虑斜线）

• 模型评价与推广

4.1 模型评价

本文提出的基于蚁群算法的最优路径规划方法在20x20网格地图中得到了有效验证。通过仿真实验，我们发现该算法能够快速找到从起点到终点的无碰撞最优路径，并且路径长度较短，满足了路径规划的基本要求。此外，蚁群算法的全局搜索能力和鲁棒性也使其在处理复杂环境时表现出色。

然而，该算法也存在一些不足之处。例如，算法参数的选择对结果影响较大，需要通过大量实验来优化；算法在初期可能收敛速度较慢，需要较多的迭代次数才能找到较优解。

4.2 推广

本文的研究成果可以进一步推广到更复杂的地图环境和更多的应用场景中。例如，可以将算法应用于三维空间中的路径规划问题；可以结合其他优化算法来提高算法的性能；还可以考虑加入动态障碍物和不确定性因素等复杂情况，使算法更加实用和可靠。

代码展示

import matplotlib.pyplot as plt
import numpy as np
from queue import PriorityQueue
import math
import time
from matplotlib import rcParams
rcParams['font.sans-serif'] = ['SimHei']  # 使用黑体
rcParams['axes.unicode_minus'] = False  # 用来正常显示负号

#%%
# 创建网格 (0 = 空地, 1 = 障碍物)
grid = np.zeros((20, 20))
grid[1:3, 1:3] = 1  # 示例障碍物
grid[0:8, 6:9] = 1
grid[5:9, 1:4] = 1
grid[14:16, 0:4] = 1
grid[7:12, 10:14] = 1
grid[10:12, 7:10] = 1
grid[12:15, 11:14] = 1
grid[12:15, 15:19] = 1
grid[15:17, 6:12] = 1
grid[17:20, 10:12] = 1
grid[16:18, 17:19] = 1
grid[18:19, 14:15] = 1
#%%
# 定义起点和终点
start = (0, 0)
goal = (19, 19)
#%%
#考虑斜线
def a_star_search(start, goal, grid):
    def heuristic(a, b):
        return abs(a[0] - b[0]) + abs(a[1] - b[1])

    def reconstruct_path(came_from, current):
        path = []
        while current in came_from:
            path.append(current)
            current = came_from[current]
        path.append(start)
        path.reverse()
        return path

    rows, cols = len(grid), len(grid[0])
    open_set = PriorityQueue()
    open_set.put((0, start))
    came_from = {}
    g_score = {(r, c): float('inf') for r in range(rows) for c in range(cols)}
    g_score[start] = 0
    f_score = {(r, c): float('inf') for r in range(rows) for c in range(cols)}
    f_score[start] = heuristic(start, goal)
    
    iterations = 0

    while not open_set.empty():
        current = open_set.get()[1]
        iterations += 1

        if current == goal:
            return reconstruct_path(came_from, current), iterations

        for dx, dy in [(-1, 0), (1, 0), (0, -1), (0, 1)]:
            neighbor = (current[0] + dx, current[1] + dy)
            if 0 <= neighbor[0] < rows and 0 <= neighbor[1] < cols and grid[neighbor[0]][neighbor[1]] == 0:
                tentative_g_score = g_score[current] + 1
                if tentative_g_score < g_score[neighbor]:
                    came_from[neighbor] = current
                    g_score[neighbor] = tentative_g_score
                    f_score[neighbor] = g_score[neighbor] + heuristic(neighbor, goal)
                    if neighbor not in [i[1] for i in open_set.queue]:
                        open_set.put((f_score[neighbor], neighbor))

    return None, iterations
#%%
# 查找路径
path = a_star_search(start, goal, grid)
#%%
# 运行A*算法并测量时间
start_time = time.time()
path, iterations = a_star_search(start, goal, grid)
end_time = time.time()

# 计算时间
calc_time = end_time - start_time

# 打印结果
if path:
    print(f"路径长度: {len(path)}")
else:
    print("未找到路径")
print(f"收敛速度（迭代次数）: {iterations}")
print(f"计算时间: {calc_time} 秒")

# 绘制网格和路径
plt.imshow(grid, cmap='Greys', interpolation='none')
if path:
    path_x, path_y = zip(*path)
    plt.plot(path_y, path_x, 'r', linewidth=2)
plt.scatter([start[1], goal[1]], [start[0], goal[0]], c='blue')
plt.title('路径寻找')
plt.show()

def a_star_search2(start, goal, grid):
    def heuristic(a, b):
        # 曼哈顿距离启发式函数
        return abs(a[0] - b[0]) + abs(a[1] - b[1])

    def reconstruct_path(came_from, current):
        # 从目标节点回溯重建路径
        path = []
        while current in came_from:
            path.append(current)
            current = came_from[current]
        path.append(start)
        path.reverse()
        return path

    rows, cols = len(grid), len(grid[0])
    open_set = PriorityQueue()
    open_set.put((0, start))
    came_from = {}
    g_score = {(r, c): float('inf') for r in range(rows) for c in range(cols)}
    g_score[start] = 0
    f_score = {(r, c): float('inf') for r in range(rows) for c in range(cols)}
    f_score[start] = heuristic(start, goal)

    while not open_set.empty():
        current = open_set.get()[1]

        if current == goal:
            return reconstruct_path(came_from, current)

        for dx, dy in [(-1, 0), (1, 0), (0, -1), (0, 1), (-1, -1), (-1, 1), (1, -1), (1, 1)]:
            neighbor = (current[0] + dx, current[1] + dy)
            if 0 <= neighbor[0] < rows and 0 <= neighbor[1] < cols and grid[neighbor[0]][neighbor[1]] == 0:
                # 计算移动的代价，斜向移动代价更高
                tentative_g_score = g_score[current] + (math.sqrt(2) if dx != 0 and dy != 0 else 1)
                if tentative_g_score < g_score[neighbor]:
                    came_from[neighbor] = current
                    g_score[neighbor] = tentative_g_score
                    f_score[neighbor] = g_score[neighbor] + heuristic(neighbor, goal)
                    if neighbor not in [i[1] for i in open_set.queue]:
                        open_set.put((f_score[neighbor], neighbor))

    return None
#%%
# 查找路径
path = a_star_search2(start, goal, grid)

# # 运行A*算法并测量时间
# start_time = time.time()
# path, iterations = a_star_search(start, goal, grid)
# end_time = time.time()
# 
# # 计算时间
# calc_time = end_time - start_time
# 
# # 打印结果
# if path:
#     print(f"路径长度: {len(path)}")
# else:
#     print("未找到路径")
# print(f"收敛速度（迭代次数）: {iterations}")
# print(f"计算时间: {calc_time} 秒")

# 绘制网格和路径
plt.imshow(grid, cmap='Greys', interpolation='none')
if path:
    path_x, path_y = zip(*path)
    plt.plot(path_y, path_x, 'r', linewidth=2)
plt.scatter([start[1], goal[1]], [start[0], goal[0]], c='blue')

# 设置坐标轴刻度
plt.xticks(np.arange(grid.shape[1]))
plt.yticks(np.arange(grid.shape[0]))

# 设置坐标轴标签
plt.title('路径寻找')

# plt.grid(True)
plt.grid(False)
plt.show()