链接：https://atsushisakai.github.io/PythonRobotics/modules/0_getting_started/0_getting_started.html#

docs：PythonRobotics

PythonRobotics 是一个综合性的机器人算法实现项目，演示了机器人如何通过栅格地图等表征方式感知环境，借助定位滤波器精确估计自身状态/位姿，利用运动模型预测移动轨迹。

其核心展示了机器人如何通过基于搜索节点的多样化路径规划算法制定目标路线，并通过路径跟踪控制器生成控制指令精准执行规划方案。

架构总览

章节导航

1. 机器人状态/位姿
2. 环境表示（栅格地图）
3. 机器人运动模型
4. 定位滤波器
5. 路径规划算法
6. 搜索节点（路径规划用）
7. 路径跟踪控制器

---

第一章：机器人状态/位姿

欢迎进入PythonRobotics系列！

本章我们将深入探讨机器人学中最基础的概念——机器人状态（State）或**位姿**（Pose）。

这相当于机器人的"身份证"和"实时状态简报"，精确记录着机器人的空间位置与运动特性。

机器人状态为何如此重要？

想象家用扫地机器人执行清洁任务时，需要实时掌握以下信息：

• 已清洁区域定位（X/Y坐标）
• 行进方向判断（偏航角）
• 移动速度调节（速度参数）

这些参数构成的完整状态描述，是机器人执行路径规划、避障等高级功能的基石。若缺乏状态感知，机器人将如同盲人失去空间认知能力

状态参数组成解析

针对地面移动机器人（如车辆、无人机），典型状态包含四个核心维度：

参数	物理意义	生活类比
X坐标	东西轴向位置（米）	街道门牌号
Y坐标	南北轴向位置（米）	楼层编号
偏航角	朝向方位（弧度制，0通常指东方）	人体面部朝向
速度	移动速率（米/秒）	步行/驾驶速度表显

代码实现：状态类结构

在PythonRobotics项目中，机器人状态通过面向对象编程封装。以下代码节选自PathPlanning/ClosedLoopRRTStar/unicycle_model.py，展示典型实现：

import math  # 数学运算库

class State:
    def __init__(self, x=0.0, y=0.0, yaw=0.0, v=0.0):
        self.x = x      # 东向坐标（米）
        self.y = y      # 北向坐标（米）
        self.yaw = yaw  # 偏航角（弧度）
        self.v = v      # 线速度（米/秒）

关键解析：

• 类构造函数初始化默认坐标原点
• 采用国际单位制保证计算一致性
• 偏航角采用弧度制便于三角函数计算

实例化演示：

# 创建初始状态：坐标(10,5)，45度朝向，速度2m/s
my_robot = State(x=10.0, y=5.0, yaw=math.radians(45), v=2.0)

# 输出格式化状态信息
print(f"X坐标: {my_robot.x:.2f}m")
print(f"Y坐标: {my_robot.y:.2f}m")
print(f"航向角: {math.degrees(my_robot.yaw):.2f}°")
print(f"瞬时速度: {my_robot.v:.2f}m/s")

状态更新机制

机器人运动通过运动模型实现状态迭代更新。以下代码段来自unicycle_model.py，演示单步状态预测：

dt = 0.05   # 时间步长（秒）
L = 0.9     # 轴距参数（米）

def update(state, accel, delta):
    # 位置更新
    state.x += state.v * math.cos(state.yaw) * dt
    state.y += state.v * math.sin(state.yaw) * dt
    
    # 方向更新
    state.yaw += state.v/L * math.tan(delta) * dt
    state.yaw = pi_2_pi(state.yaw)  # 角度归一化
    
    # 速度更新
    state.v += accel * dt
    
    return state

参数说明：

• accel: 加速度（m/s²）
• delta: 前轮转向角（弧度）
• pi_2_pi(): 将角度约束在[-π, π]的辅助函数

运动模拟示例：

# 角度规约函数（utils/angle.py）
def pi_2_pi(angle):
    return (angle + math.pi) % (2*math.pi) - math.pi

# 初始状态：坐标(10,5)，45度航向，2m/s
current = State(10.0, 5.0, math.radians(45), 2.0)

# 施加0.5m/s²加速，5度右转
updated = update(current, 0.5, math.radians(5))

# 输出状态变化
print(f"X位移增量: {updated.x - current.x:.3f}m")
print(f"航向修正量: {math.degrees(updated.yaw - current.yaw):.3f}°")

状态表征多样性

不同应用场景需扩展状态参数：

1. 时序路径规划（TimeBasedPathPlanning/Node.py）：

   @dataclass
   class Node:
       pos: tuple    # (x,y)坐标
       time: int     # 到达时刻
       heuristic: float  # 启发式代价
   

   强调时间维度而非瞬时运动状态

2. 纯追踪控制（pure_pursuit.py）：

   class EnhancedState(State):
       def __init__(self, is_reverse=False, **kwargs):
           super().__init__(**kwargs)
           self.direction = -1 if is_reverse else 1
           # 计算后轴中心（控制算法需要）
           self.rear_x = self.x - self.direction*(L/2)*math.cos(self.yaw)
           self.rear_y = self.y - self.direction*(L/2)*math.sin(self.yaw)
   

   增加逆向行驶标志位和后轴坐标计算

小结

机器人状态/位姿是构建智能移动系统的基石，包含空间坐标、运动方向与速度等核心参数。

通过状态类封装和运动模型迭代，实现了机器人对自身运动状态的持续认知。这种基础感知能力为后续环境建模、路径规划等高级功能奠定基础。

下一章：环境表示（栅格地图）

---

第二章：环境表示（栅格地图）

在第一章：机器人状态/位姿中，我们学习了机器人如何跟踪自身位置和运动状态。

这相当于机器人的"身份证"。但机器人如何感知周围环境？如何知道墙面位置或障碍物分布？

这正是环境表示的核心价值！本章将聚焦机器人理解环境的常用方法——栅格地图。

为何需要环境地图？

以扫地机器人为例，仅知道自身坐标不足以有效工作。它需要：

• 避障功能：避免碰撞墙壁、家具或宠物
• 导航能力：规划房间穿越路径
• 清洁路径：设计全覆盖无死角的移动轨迹

栅格地图如同数字化的环境蓝图，将空间划分为若干单元格，每个单元存储特定位置的环境信息。

---

栅格地图核心要素

栅格地图可类比为电子表格或像素图像，其构成要素包括：

要素	描述	类比
单元格	构成地图的基本单元，代表真实世界的微小区域	棋盘方格或低分辨率图像的像素
分辨率	单元格对应的真实尺寸（如0.1m×0.1m），数值越小细节越丰富	图像缩放级别
数值编码	单元格存储的环境信息：	绘画中的色彩
	- 占用(1.0)：存在障碍物	黑色像素代表墙面
	- 自由(0.0)：可通行区域	白色像素代表开放区域
	- 未知(0.5)：未探索区域	灰色像素表示未知区域
坐标原点	地图坐标系与真实世界坐标系的对应关系	大地图上的起始定位点

---

PythonRobotics栅格地图实践

PythonRobotics的GridMap类（位于Mapping/grid_map_lib/grid_map_lib.py）是构建栅格地图的核心工具。以下示例展示如何创建基础地图 [7]：

import numpy as np
import matplotlib.pyplot as plt
from Mapping.grid_map_lib.grid_map_lib import GridMap, FloatGrid

# 1. 定义地图参数
width_cells = 100       # 横向单元格数
height_cells = 120      # 纵向单元格数
resolution = 0.5        # 单元格分辨率（米）
center_x = 10.0         # 地图中心真实坐标X
center_y = -0.5         # 地图中心真实坐标Y

# 2. 创建GridMap对象
my_room_map = GridMap(width_cells, height_cells, resolution,
                      center_x, center_y, init_val=FloatGrid(0.0))

print(f"创建地图尺寸：{width_cells}x{height_cells}单元格")
print(f"地图分辨率：{resolution} 米/单元格")

技术解析：

• FloatGrid封装浮点数值，支持后续扩展复杂数据类型
• init_val=FloatGrid(0.0)初始化全图单元格为自由状态
• 坐标系转换模块实现真实坐标与栅格索引的双向映射 [4]

---

障碍物标注方法

通过真实坐标标注障碍物示例：

# 标注障碍物坐标（1.0表示占用状态）
my_room_map.set_value_from_xy_pos(10.1, -1.1, FloatGrid(1.0))
my_room_map.set_value_from_xy_pos(10.1, -0.1, FloatGrid(1.0))
my_room_map.set_value_from_xy_pos(11.1, 0.1, FloatGrid(1.0))

# 查询特定坐标值
value = my_room_map.get_value_from_xy_pos(10.1, 0.1)
print(f"坐标(10.1,0.1)值：{value.get_float_data()}")

注意事项：

• 越界坐标操作返回False，地图不会自动扩展边界
• 批量标注建议使用set_value_from_xy_index提升效率

---

地图可视化实现

matplotlib可视化工具展示栅格地图：

my_room_map.plot_grid_map()
plt.title("房间栅格地图")
plt.xlabel("X坐标(米)")
plt.ylabel("Y坐标(米)")
plt.axis("equal")  # 保持单元格方形比例
plt.grid(True)
plt.show()

输出特征：

• 蓝色单元格表示障碍物（1.0）
• 浅色区域为自由空间（0.0）
• 坐标系自动对齐真实世界尺度 [13]

---

底层技术解析

坐标转换算法

核心转换函数实现真实坐标到栅格索引的精确映射：

class GridMap:
    def calc_xy_index_from_position(self, pos, lower_pos, max_index):
        """计算坐标对应的栅格索引"""
        ind = int(np.floor((pos - lower_pos) / self.resolution))
        return ind if 0 <= ind <= max_index else None

计算过程：

1. 计算坐标相对于地图左下角的偏移量
2. 除以分辨率获得单元格数量
3. 取整确保索引为整数
4. 边界检查防止越界访问

数据存储优化

采用一维数组存储二维栅格数据，通过行列计算快速定位：

grid_ind = y_ind * self.width + x_ind  # 二维转一维索引
self.data[grid_ind] = value  # 更新存储

---

高级栅格地图变体

动态障碍物地图

TimeBasedPathPlanning/GridWithDynamicObstacles.py实现时序栅格堆栈，支持：

• 预测未来时段的障碍物分布
• 四维数据存储（x,y,time,value）
• 动态路径规划避障

传感器建图技术

lidar_to_grid_map.py展示激光雷达建图流程：

1. 激光射线投射（ray casting）标记障碍
2. 区域生长算法填充未知区域
3. 概率更新机制处理传感器噪声

---

小结

本章深入探讨了栅格地图作为环境表示的核心方法，涵盖地图创建、障碍标注、可视化及底层实现原理。

结合第一章的机器人状态跟踪，现已具备环境认知基础能力。下一章将解锁机器人的运动奥秘！

下一章：机器人运动模型