数学建模基础：栅格地图构建、障碍物表示、起点终点定义
做路径规划，第一步不是写算法，而是把现实世界翻译成计算机能懂的语言。这个过程，就是数学建模。
我个人习惯把这一步叫做「搭舞台」。舞台搭不好，再好的算法也白搭。今天我们就聊聊这个舞台怎么搭——栅格地图、障碍物表示、还有起点终点的定义。
1. 栅格地图：把连续世界变成离散格子
现实世界是连续的。机器人可以走任意角度，任意距离。但计算机处理连续数据太费劲了。怎么办？
说白了，就是「切格子」。把环境切成一个个小方格，每个格子要么能走，要么不能走。这就是栅格地图的核心思想。
核心概念：栅格地图（Grid Map）将环境离散化为 M×N 的网格单元。每个网格单元称为一个栅格（Cell），用坐标 (i, j) 唯一标识。
我在项目中遇到过一个问题：栅格大小怎么定？切太细，地图大、计算慢；切太粗，路径不精确。我的经验是——
- 栅格尺寸 = 机器人直径 × 0.5，这是经验值
- 如果环境复杂，可以缩小到机器人直径 × 0.3
- 千万别小于机器人直径 × 0.2，否则计算量爆炸
举个例子。假设机器人直径 0.5 米，环境是 10m × 10m 的房间。栅格尺寸取 0.25 米，那么地图就是 40 × 40 = 1600 个格子。嗯，这个量级对蚁群算法来说很舒服。
2. 障碍物表示：哪些格子不能走？
地图建好了，接下来要告诉算法：哪些地方不能去。
障碍物表示其实就两种思路：
- 二值法：0 表示空闲，1 表示障碍。简单粗暴。
- 概率法：0~1 之间的值，表示该格子被占用的概率。更精细，但复杂。
做课程项目，我建议用二值法。为什么？因为蚁群算法本身已经够复杂了，别在输入数据上给自己加戏。
我的小技巧：障碍物周围做一层「膨胀处理」。就是把障碍物边缘的格子也标记为障碍。这样机器人不会贴着墙走，安全系数高很多。膨胀半径取机器人半径对应的栅格数就行。
我曾经在一个仓库项目里吃过亏。障碍物没做膨胀，结果规划的路径贴着货架走，机器人稍微偏一点就撞上了。后来加了 2 格膨胀，再也没出过问题。
代码实现很简单：
# 栅格地图初始化
import numpy as np
# 地图尺寸：10m x 10m，栅格尺寸0.25m
grid_size = 0.25
width = int(10 / grid_size) # 40
height = int(10 / grid_size) # 40
# 0=空闲, 1=障碍
grid_map = np.zeros((height, width), dtype=int)
# 手动设置障碍物（矩形区域）
grid_map[10:15, 20:25] = 1 # 一个5x5的障碍物
# 膨胀处理（简单实现）
def inflate_obstacles(map, radius=2):
inflated = map.copy()
obstacles = np.argwhere(map == 1)
for (y, x) in obstacles:
for dy in range(-radius, radius+1):
for dx in range(-radius, radius+1):
ny, nx = y+dy, x+dx
if 0 <= ny < map.shape[0] and 0 <= nx < map.shape[1]:
inflated[ny, nx] = 1
return inflated
grid_map = inflate_obstacles(grid_map, radius=2)
3. 起点终点定义：给算法一个目标
地图有了，障碍物标好了。接下来就是告诉算法：从哪出发，到哪去。
起点和终点在栅格地图里就是两个坐标 (sx, sy) 和 (ex, ey)。听起来简单，但有几个坑要注意。
注意：起点和终点不能落在障碍物上！这听起来是废话，但我见过太多人犯这个错。尤其是终点，有时候贴着墙放，膨胀后就被吞掉了。
我建议的做法：
- 起点选在空旷区域，周围至少 3 格没有障碍
- 终点同样要留出空间，方便机器人到达后调整姿态
- 如果终点在狭窄通道里，考虑放宽终点判定条件
代码里怎么定义？
# 起点和终点定义
start = (5, 5) # 第5行第5列
goal = (35, 35) # 第35行第35列
# 检查合法性
def check_valid(point, map):
y, x = point
if map[y, x] == 1:
print(f"错误：点 {point} 在障碍物上！")
return False
return True
assert check_valid(start, grid_map), "起点无效"
assert check_valid(goal, grid_map), "终点无效"
4. 知识体系总览
说了这么多，我把整个流程画了张图。你一看就明白了。
5. 避坑指南
最后，分享几个我踩过的坑。你遇到了可以直接跳过。
我曾经犯过的错：
- 坐标搞反：栅格地图的行列和笛卡尔坐标的 xy 是反的。行对应 y，列对应 x。我一开始没注意，路径规划出来全是斜线。
- 障碍物太密：如果障碍物占地图面积超过 60%，蚁群算法基本找不到路径。这时候要重新考虑环境布局。
- 起点终点太近：距离小于 5 格时，算法可能直接「瞬移」过去，路径看起来很奇怪。建议至少保持 10 格以上距离。
嗯，数学建模这块就这些。说白了就是把现实问题翻译成矩阵和坐标。翻译对了，后面的算法才能跑得顺。翻译错了，debug 能让你怀疑人生。
我个人习惯是先把地图画出来可视化看看，确认没问题再往下走。你也不妨试试。