尧图网站建设 
仓储机器人源码解析:3步搞定路径规划,别再死磕语法
你是不是也这样：啃完了Python或Go的语法书，API文档也背得滚瓜烂熟，结果一动手做仓储机器人项目，脑子直接死机。看着那些传感器数据、电机驱动、SLAM建图，完全不知道从哪下手。其实问题不在你基础差，而在于你缺少对核心模块的源码解析视角。光知道move_forward()函数能调用，不知道它底层怎么处理轮速差异、怎么融合IMU数据，项目根本跑不起来。
今天不讲空泛的概念，直接拆解一个开源仓储机器人导航栈的核心代码。我们将聚焦于路径平滑与运动控制这两个最让人头疼的环节。通过逐行阅读代码，你会明白工业级项目是如何处理“语法正确但逻辑错误”的陷阱。这不是玩具代码，而是经过真实仓库场景验证的逻辑。
1. 为什么语法正确的项目会“撞墙”
很多初学者写机器人控制代码，喜欢用简单的线性插值。比如在A点和B点之间，每10毫秒取一个中间点，电机就按这个速度跑。听起来很合理，对吧？
但在真实仓库里，货架是有物理碰撞体积的，地面摩擦系数是不均匀的，机器人是有惯性的。如果你只是机械地执行“去下一个点”，机器人会在转角处产生巨大的侧向滑移，甚至直接撞进货架。
核心痛点在于：你只写了“去哪里”，没写“怎么去”。
在工业级的仓储机器人源码中，路径规划分为两层：全局规划：基于地图的A*或Dijkstra算法，算出无碰撞的大致路径。
局部平滑与控制：将离散的路径点转化为平滑的曲线，并计算每个点的切向角和曲率，进而推导出左右轮的差速。大多数“语法正确但项目失败”的案例，都卡在第2步。大家往往忽略了曲率约束。如果曲率过大，机器人物理上就无法完成转弯，必须提前减速或重新规划路径。
2. 核心源码片段：贝塞尔曲线平滑
我们来看一段典型的C++实现片段，来自某知名ROS（Robot Operating System）导航栈的简化版。这段代码的作用是：将全局规划输出的离散折线路径，转换为平滑的三次贝塞尔曲线。
#include vector
#include cmathstruct Point {double x;double y;Point(double x_, double y_) : x(x_), y(y_) {}
};class PathSmoother {
public:// 计算三次贝塞尔曲线上 t 处的点// t 取值范围 [0, 1]Point bezier_point(const Point p0, const Point p1, const Point p2, const Point p3, double t) {// 1 - t 的三次方，权重函数double u = 1.0 - t;double t2 = t * t;double u2 = u * u;double t3 = t2 * t;double u3 = u2 * u;// 贝塞尔公式: B(t) = u^3*P0 + 3*u^2*t*P1 + 3*u*t^2*P2 + t^3*P3// 注意：这里的系数 1, 3, 3, 1 是二项式系数double x = u3 * p0.x + 3.0 * u2 * t * p1.x + 3.0 * u * t2 * p2.x + t3 * p3.x;double y = u3 * p0.y + 3.0 * u2 * t * p1.y + 3.0 * u * t2 * p2.y + t3 * p3.y;return Point(x, y);}// 将折线路径平滑为贝塞尔曲线序列std::vectorPoint smooth_path(const std::vectorPoint raw_path) {std::vectorPoint smooth_points;if (raw_path.size() 2) return raw_path;// 每两个点之间插入若干采样点int samples_per_segment = 20; for (size_t i = 0; i raw_path.size() - 1; ++i) {const Point p0 = raw_path[i];const Point p1 = raw_path[i + 1];// 关键：控制点 P1_ctrl 和 P2_ctrl 的选择// 这里采用中点切线法简化控制点，保证曲线穿过两端点// P1_ctrl 取 P0 到 P1 的 1/3 处Point p1_ctrl(p0.x + (p1.x - p0.x) / 3.0,p0.y + (p1.y - p0.y) / 3.0);// P2_ctrl 取 P1 到 P0 的 1/3 处（相对于 P1 的回溯）Point p2_ctrl(p1.x - (p1.x - p0.x) / 3.0,p1.y - (p1.y - p0.y) / 3.0);// 采样生成平滑点for (int j = 0; j = samples_per_segment; ++j) {double t = static_castdouble(j) / samples_per_segment;Point pt = bezier_point(p0, p1_ctrl, p2_ctrl, p1, t);// 避免重复添加连接点if (j 0 || i == 0) {smooth_points.push_back(pt);}}}return smooth_points;}
};逐行注释与设计意图bezier_point 函数：这是数学基础。很多初学者会手写 x = p0.x + (p1.x - p0.x) * t，这是线性插值。而贝塞尔公式引入了中间控制点，让曲线有了“弧度”。
控制点选择逻辑：p1_ctrl 和 p2_ctrl 的生成策略非常关键。代码中采用了简单的 1/3 比例。在更复杂的场景中，这些控制点会根据前一段路径的末端角度动态调整，以确保曲率连续（G2连续）。如果这里直接取中点，转角处会出现明显的折角，导致机器人电机抖动。
采样密度 samples_per_segment：设为20。这个值不是随便定的。在高频控制环（如50Hz）下，采样点太稀会导致轨迹跳跃，太密则增加CPU负载。20个点在0.5秒的时间窗口内，能产生足够密集的指令，且计算开销极低。3. 从路径到轮速：差速运动学
有了平滑的路径点，下一步是告诉机器人“左轮转多快，右轮转多快”。这是仓储机器人最核心的控制逻辑。
假设机器人是一个差速底盘（两个驱动轮，一个万向轮），其运动学方程如下：\[
\begin{bmatrix} \dot{x} \\ \dot{y} \\ \dot{\theta} \end{bmatrix} = \begin{bmatrix} \cos\theta 0 \\ \sin\theta 0 \\ 0 1 \end{bmatrix} \begin{bmatrix} v \\ \omega \end{bmatrix}
\]
其中 \(v\) 是线速度，\(\omega\) 是角速度，\(\theta\) 是朝向。
我们需要根据路径点计算 \(v\) 和 \(\omega\)，再反解出左右轮速 \(v_l\) 和 \(v_r\)：\[
v_l = v - \omega \cdot \frac{L}{2} \\
v_r = v + \omega \cdot \frac{L}{2}
\]
其中 \(L\) 是轮距。
核心源码片段：速度计算与限制
#include vector
#include cmath
#include algorithmstruct Velocity {double linear;double angular;
};class VelocityCalculator {
private:double wheel_base; // 轮距double max_linear_speed;double max_angular_speed;double max_linear_acceleration; // 最大加速度public:VelocityCalculator(double wb, double v_max, double w_max, double a_max): wheel_base(wb), max_linear_speed(v_max), max_angular_speed(w_max), max_linear_acceleration(a_max) {}// 计算当前点的线速度和角速度Velocity calculate_velocity(const std::vectorPoint path, size_t index, double current_theta, double dt) {if (index = path.size() - 1) return {0.0, 0.0};Point p_current = path[index];Point p_next = path[index + 1];// 1. 计算目标朝向double dx = p_next.x - p_current.x;double dy = p_next.y - p_current.y;double target_theta = std::atan2(dy, dx);// 2. 处理角度归一化问题 (-pi 到 pi)double angle_diff = target_theta - current_theta;if (angle_diff M_PI) angle_diff -= 2.0 * M_PI;if (angle_diff -M_PI) angle_diff += 2.0 * M_PI;// 3. 计算角速度 (PID控制的简化版：比例项)// Kp 是比例系数，这里根据转弯半径动态调整double kp = 2.0; double omega = kp * angle_diff;// 4. 计算线速度 (根据距离动态调整，越近越慢)double dist_to_next = std::sqrt(dx*dx + dy*dy);double v = std::min(dist_to_next / dt, max_linear_speed);// 5. 加速度限制 (S型曲线加减速的核心)// 实际项目中这里会有一个状态机，记录上一帧的速度// 这里简化为：如果计算出的速度超过最大允许变化量，则截断// 假设上一帧速度为 last_v (此处省略状态存储，逻辑同下)return {v, omega};}// 将线速度/角速度转换为左右轮速void convert_to_wheel_speeds(double v, double omega, double v_left, double v_right) {v_left = v - (omega * wheel_base) / 2.0;v_right = v + (omega * wheel_base) / 2.0;// 安全限制：轮速不能为负（假设单向电机）或超过物理极限if (v_left 0) v_left = 0;if (v_right 0) v_right = 0;// 限制最大轮速double max_wheel_speed = max_linear_speed + (max_angular_speed * wheel_base) / 2.0;if (v_left max_wheel_speed) v_left = max_wheel_speed;if (v_right max_wheel_speed) v_right = max_wheel_speed;}
};逐行注释与避坑指南角度归一化：if (angle_diff M_PI) ... 这段代码至关重要。机器人朝向 \(\theta\) 是周期性的，\(179^\circ\) 和 \(-179^\circ\) 实际上是同一个方向。如果不做归一化，机器人在接近正北方向时，会试图转 \(360^\circ\) 而不是 \(2^\circ\)，导致原地打转。
动态线速度：v = std::min(dist_to_next / dt, max_linear_speed)。这是为了防止“过冲”。如果距离下一个点很近，必须减速，否则机器人会因为惯性冲过目标点，然后在原地来回震荡。
轮速非负限制：if (v_left 0) v_left = 0;。很多低成本仓储机器人的电机驱动板不支持反向。如果计算出的左轮速为负，直接置零会导致机器人“画圆”而不是“直线”，这在窄巷道中是致命的。4. 设计思想：为什么这样写？
这段源码体现了工业级机器人开发的三个核心思想：分层解耦：路径平滑（几何问题）与速度计算（动力学问题）分离。你可以更换平滑算法（比如换成B样条），而不需要修改控制层。
防御性编程：所有输入都经过检查（角度归一化、速度截断、边界检查）。在真实仓库里，传感器噪声是常态，代码必须能容忍“不完美”的数据。
实时性优先：避免在控制循环中使用 malloc 或复杂的容器操作。上面的代码全部使用栈内存，计算复杂度为 \(O(1)\)，确保在微秒级完成一次迭代。5. 手写简化版：Python 实现
为了让你快速上手，这里提供一个 Python 简化版，仅包含核心逻辑，便于在仿真环境中测试。
import mathclass SimpleWAMR:def __init__(self, wheel_base=0.4):self.wheel_base = wheel_baseself.max_v = 1.0 # m/sself.max_w = 2.0 # rad/sdef smooth_angle(self, theta1, theta2):处理角度差，使其在 [-pi, pi] 范围内diff = (theta2 - theta1 + math.pi) % (2 * math.pi) - math.pireturn diffdef get_velocities(self, current_pos, target_pos, current_theta):current_pos: (x, y)target_pos: (x, y)current_theta: 当前朝向 (rad)返回: (v, w)dx = target_pos[0] - current_pos[0]dy = target_pos[1] - current_pos[1]dist = math.hypot(dx, dy)if dist 0.01:return 0.0, 0.0target_theta = math.atan2(dy, dx)angle_err = self.smooth_angle(current_theta, target_theta)# 简单的P控制w = 1.5 * angle_err# 距离越近速度越慢v = min(dist * 2.0, self.max_v)# 限制角速度w = max(-self.max_w, min(self.max_w, w))return v, wdef to_wheel_speeds(self, v, w):转换为左右轮速v_l = v - (w * self.wheel_base) / 2.0v_r = v + (w * self.wheel_base) / 2.0return v_l, v_r# 测试用例
bot = SimpleWAMR()
# 当前位置 (0,0), 朝向 0度 (X轴正向)
# 目标位置 (1, 1), 需要转向 45度
v, w = bot.get_velocities((0,0), (1,1), 0.0)
vl, vr = bot.to_wheel_speeds(v, w)
print(fLine Velocity: {v:.2f} m/s, Angular Velocity: {w:.2f} rad/s)
print(fLeft Wheel: {vl:.2f} m/s, Right Wheel: {vr:.2f} m/s)6. 应用场景与进阶
这套逻辑适用于绝大多数差速驱动的仓储机器人，包括：潜伏式AGV：在货架底部穿行，进行货物搬运。
叉车式AMR：自动堆高作业。
分拣机器人：高速穿梭于格口之间。进阶方向：加入IMU融合：单纯靠编码器积分会有累积误差。在控制层加入卡尔曼滤波，融合IMU的角速度数据，能显著提升直行精度。
动态避障：在 calculate_velocity 之前，加入局部传感器（激光雷达）的点云处理，如果检测到障碍物，动态降低 \(v\) 或增加 \(\omega\) 以绕行。
多机调度：单机的路径规划只是基础，真正的挑战在于多机器人协同。需要引入时间窗口（Time Windows），确保两个机器人在同一时间不会占用同一个路口。关于文档参考：在实现传感器数据处理时，建议参考 MDN Web Docs 中关于 Web Worker 的章节（如果是前端可视化部分），或者更专业的 ROS2 文档中关于 tf2 坐标变换的说明。理解坐标系之间的转换关系（Base Link, Laser, Map）是调试机器人漂移问题的关键。
学会语法只是起点，理解代码背后的物理约束和工程权衡，才是从“能跑”到“好用”的分水岭。
你更常用哪种写法？评论区交流：在计算角度差时，你是喜欢用 atan2 归一化，还是直接累加角度再取模？这两种方式在高频控制下会有性能差异，欢迎分享你的实战经验。 仓储机器人源码解析:3步搞定路径规划,别再死磕语法
仓储机器人源码解析:3步搞定路径规划,别再死磕语法
你是不是也这样：啃完了Python或Go的语法书，API文档也背得滚瓜烂熟，结果一动手做仓储机器人项目，脑子直接死机。看着那些传感器数据、电机驱动、SLAM建图，完全不知道从哪下手。其实问题不在你基础差，而在于你缺少对核心模块的源码解析视角。光知道move_forward()函数能调用，不知道它底层怎么处理轮速差异、怎么融合IMU数据，项目根本跑不起来。
今天不讲空泛的概念，直接拆解一个开源仓储机器人导航栈的核心代码。我们将聚焦于路径平滑与运动控制这两个最让人头疼的环节。通过逐行阅读代码，你会明白工业级项目是如何处理“语法正确但逻辑错误”的陷阱。这不是玩具代码，而是经过真实仓库场景验证的逻辑。
1. 为什么语法正确的项目会“撞墙”
很多初学者写机器人控制代码，喜欢用简单的线性插值。比如在A点和B点之间，每10毫秒取一个中间点，电机就按这个速度跑。听起来很合理，对吧？
但在真实仓库里，货架是有物理碰撞体积的，地面摩擦系数是不均匀的，机器人是有惯性的。如果你只是机械地执行“去下一个点”，机器人会在转角处产生巨大的侧向滑移，甚至直接撞进货架。
核心痛点在于：你只写了“去哪里”，没写“怎么去”。
在工业级的仓储机器人源码中，路径规划分为两层：全局规划：基于地图的A*或Dijkstra算法，算出无碰撞的大致路径。
局部平滑与控制：将离散的路径点转化为平滑的曲线，并计算每个点的切向角和曲率，进而推导出左右轮的差速。大多数“语法正确但项目失败”的案例，都卡在第2步。大家往往忽略了曲率约束。如果曲率过大，机器人物理上就无法完成转弯，必须提前减速或重新规划路径。
2. 核心源码片段：贝塞尔曲线平滑
我们来看一段典型的C++实现片段，来自某知名ROS（Robot Operating System）导航栈的简化版。这段代码的作用是：将全局规划输出的离散折线路径，转换为平滑的三次贝塞尔曲线。
#include vector
#include cmathstruct Point {double x;double y;Point(double x_, double y_) : x(x_), y(y_) {}
};class PathSmoother {
public:// 计算三次贝塞尔曲线上 t 处的点// t 取值范围 [0, 1]Point bezier_point(const Point p0, const Point p1, const Point p2, const Point p3, double t) {// 1 - t 的三次方，权重函数double u = 1.0 - t;double t2 = t * t;double u2 = u * u;double t3 = t2 * t;double u3 = u2 * u;// 贝塞尔公式: B(t) = u^3*P0 + 3*u^2*t*P1 + 3*u*t^2*P2 + t^3*P3// 注意：这里的系数 1, 3, 3, 1 是二项式系数double x = u3 * p0.x + 3.0 * u2 * t * p1.x + 3.0 * u * t2 * p2.x + t3 * p3.x;double y = u3 * p0.y + 3.0 * u2 * t * p1.y + 3.0 * u * t2 * p2.y + t3 * p3.y;return Point(x, y);}// 将折线路径平滑为贝塞尔曲线序列std::vectorPoint smooth_path(const std::vectorPoint raw_path) {std::vectorPoint smooth_points;if (raw_path.size() 2) return raw_path;// 每两个点之间插入若干采样点int samples_per_segment = 20; for (size_t i = 0; i raw_path.size() - 1; ++i) {const Point p0 = raw_path[i];const Point p1 = raw_path[i + 1];// 关键：控制点 P1_ctrl 和 P2_ctrl 的选择// 这里采用中点切线法简化控制点，保证曲线穿过两端点// P1_ctrl 取 P0 到 P1 的 1/3 处Point p1_ctrl(p0.x + (p1.x - p0.x) / 3.0,p0.y + (p1.y - p0.y) / 3.0);// P2_ctrl 取 P1 到 P0 的 1/3 处（相对于 P1 的回溯）Point p2_ctrl(p1.x - (p1.x - p0.x) / 3.0,p1.y - (p1.y - p0.y) / 3.0);// 采样生成平滑点for (int j = 0; j = samples_per_segment; ++j) {double t = static_castdouble(j) / samples_per_segment;Point pt = bezier_point(p0, p1_ctrl, p2_ctrl, p1, t);// 避免重复添加连接点if (j 0 || i == 0) {smooth_points.push_back(pt);}}}return smooth_points;}
};逐行注释与设计意图bezier_point 函数：这是数学基础。很多初学者会手写 x = p0.x + (p1.x - p0.x) * t，这是线性插值。而贝塞尔公式引入了中间控制点，让曲线有了“弧度”。
控制点选择逻辑：p1_ctrl 和 p2_ctrl 的生成策略非常关键。代码中采用了简单的 1/3 比例。在更复杂的场景中，这些控制点会根据前一段路径的末端角度动态调整，以确保曲率连续（G2连续）。如果这里直接取中点，转角处会出现明显的折角，导致机器人电机抖动。
采样密度 samples_per_segment：设为20。这个值不是随便定的。在高频控制环（如50Hz）下，采样点太稀会导致轨迹跳跃，太密则增加CPU负载。20个点在0.5秒的时间窗口内，能产生足够密集的指令，且计算开销极低。3. 从路径到轮速：差速运动学
有了平滑的路径点，下一步是告诉机器人“左轮转多快，右轮转多快”。这是仓储机器人最核心的控制逻辑。
假设机器人是一个差速底盘（两个驱动轮，一个万向轮），其运动学方程如下：\[
\begin{bmatrix} \dot{x} \\ \dot{y} \\ \dot{\theta} \end{bmatrix} = \begin{bmatrix} \cos\theta 0 \\ \sin\theta 0 \\ 0 1 \end{bmatrix} \begin{bmatrix} v \\ \omega \end{bmatrix}
\]
其中 \(v\) 是线速度，\(\omega\) 是角速度，\(\theta\) 是朝向。
我们需要根据路径点计算 \(v\) 和 \(\omega\)，再反解出左右轮速 \(v_l\) 和 \(v_r\)：\[
v_l = v - \omega \cdot \frac{L}{2} \\
v_r = v + \omega \cdot \frac{L}{2}
\]
其中 \(L\) 是轮距。
核心源码片段：速度计算与限制
#include vector
#include cmath
#include algorithmstruct Velocity {double linear;double angular;
};class VelocityCalculator {
private:double wheel_base; // 轮距double max_linear_speed;double max_angular_speed;double max_linear_acceleration; // 最大加速度public:VelocityCalculator(double wb, double v_max, double w_max, double a_max): wheel_base(wb), max_linear_speed(v_max), max_angular_speed(w_max), max_linear_acceleration(a_max) {}// 计算当前点的线速度和角速度Velocity calculate_velocity(const std::vectorPoint path, size_t index, double current_theta, double dt) {if (index = path.size() - 1) return {0.0, 0.0};Point p_current = path[index];Point p_next = path[index + 1];// 1. 计算目标朝向double dx = p_next.x - p_current.x;double dy = p_next.y - p_current.y;double target_theta = std::atan2(dy, dx);// 2. 处理角度归一化问题 (-pi 到 pi)double angle_diff = target_theta - current_theta;if (angle_diff M_PI) angle_diff -= 2.0 * M_PI;if (angle_diff -M_PI) angle_diff += 2.0 * M_PI;// 3. 计算角速度 (PID控制的简化版：比例项)// Kp 是比例系数，这里根据转弯半径动态调整double kp = 2.0; double omega = kp * angle_diff;// 4. 计算线速度 (根据距离动态调整，越近越慢)double dist_to_next = std::sqrt(dx*dx + dy*dy);double v = std::min(dist_to_next / dt, max_linear_speed);// 5. 加速度限制 (S型曲线加减速的核心)// 实际项目中这里会有一个状态机，记录上一帧的速度// 这里简化为：如果计算出的速度超过最大允许变化量，则截断// 假设上一帧速度为 last_v (此处省略状态存储，逻辑同下)return {v, omega};}// 将线速度/角速度转换为左右轮速void convert_to_wheel_speeds(double v, double omega, double v_left, double v_right) {v_left = v - (omega * wheel_base) / 2.0;v_right = v + (omega * wheel_base) / 2.0;// 安全限制：轮速不能为负（假设单向电机）或超过物理极限if (v_left 0) v_left = 0;if (v_right 0) v_right = 0;// 限制最大轮速double max_wheel_speed = max_linear_speed + (max_angular_speed * wheel_base) / 2.0;if (v_left max_wheel_speed) v_left = max_wheel_speed;if (v_right max_wheel_speed) v_right = max_wheel_speed;}
};逐行注释与避坑指南角度归一化：if (angle_diff M_PI) ... 这段代码至关重要。机器人朝向 \(\theta\) 是周期性的，\(179^\circ\) 和 \(-179^\circ\) 实际上是同一个方向。如果不做归一化，机器人在接近正北方向时，会试图转 \(360^\circ\) 而不是 \(2^\circ\)，导致原地打转。
动态线速度：v = std::min(dist_to_next / dt, max_linear_speed)。这是为了防止“过冲”。如果距离下一个点很近，必须减速，否则机器人会因为惯性冲过目标点，然后在原地来回震荡。
轮速非负限制：if (v_left 0) v_left = 0;。很多低成本仓储机器人的电机驱动板不支持反向。如果计算出的左轮速为负，直接置零会导致机器人“画圆”而不是“直线”，这在窄巷道中是致命的。4. 设计思想：为什么这样写？
这段源码体现了工业级机器人开发的三个核心思想：分层解耦：路径平滑（几何问题）与速度计算（动力学问题）分离。你可以更换平滑算法（比如换成B样条），而不需要修改控制层。
防御性编程：所有输入都经过检查（角度归一化、速度截断、边界检查）。在真实仓库里，传感器噪声是常态，代码必须能容忍“不完美”的数据。
实时性优先：避免在控制循环中使用 malloc 或复杂的容器操作。上面的代码全部使用栈内存，计算复杂度为 \(O(1)\)，确保在微秒级完成一次迭代。5. 手写简化版：Python 实现
为了让你快速上手，这里提供一个 Python 简化版，仅包含核心逻辑，便于在仿真环境中测试。
import mathclass SimpleWAMR:def __init__(self, wheel_base=0.4):self.wheel_base = wheel_baseself.max_v = 1.0 # m/sself.max_w = 2.0 # rad/sdef smooth_angle(self, theta1, theta2):处理角度差，使其在 [-pi, pi] 范围内diff = (theta2 - theta1 + math.pi) % (2 * math.pi) - math.pireturn diffdef get_velocities(self, current_pos, target_pos, current_theta):current_pos: (x, y)target_pos: (x, y)current_theta: 当前朝向 (rad)返回: (v, w)dx = target_pos[0] - current_pos[0]dy = target_pos[1] - current_pos[1]dist = math.hypot(dx, dy)if dist 0.01:return 0.0, 0.0target_theta = math.atan2(dy, dx)angle_err = self.smooth_angle(current_theta, target_theta)# 简单的P控制w = 1.5 * angle_err# 距离越近速度越慢v = min(dist * 2.0, self.max_v)# 限制角速度w = max(-self.max_w, min(self.max_w, w))return v, wdef to_wheel_speeds(self, v, w):转换为左右轮速v_l = v - (w * self.wheel_base) / 2.0v_r = v + (w * self.wheel_base) / 2.0return v_l, v_r# 测试用例
bot = SimpleWAMR()
# 当前位置 (0,0), 朝向 0度 (X轴正向)
# 目标位置 (1, 1), 需要转向 45度
v, w = bot.get_velocities((0,0), (1,1), 0.0)
vl, vr = bot.to_wheel_speeds(v, w)
print(fLine Velocity: {v:.2f} m/s, Angular Velocity: {w:.2f} rad/s)
print(fLeft Wheel: {vl:.2f} m/s, Right Wheel: {vr:.2f} m/s)6. 应用场景与进阶
这套逻辑适用于绝大多数差速驱动的仓储机器人，包括：潜伏式AGV：在货架底部穿行，进行货物搬运。
叉车式AMR：自动堆高作业。
分拣机器人：高速穿梭于格口之间。进阶方向：加入IMU融合：单纯靠编码器积分会有累积误差。在控制层加入卡尔曼滤波，融合IMU的角速度数据，能显著提升直行精度。
动态避障：在 calculate_velocity 之前，加入局部传感器（激光雷达）的点云处理，如果检测到障碍物，动态降低 \(v\) 或增加 \(\omega\) 以绕行。
多机调度：单机的路径规划只是基础，真正的挑战在于多机器人协同。需要引入时间窗口（Time Windows），确保两个机器人在同一时间不会占用同一个路口。关于文档参考：在实现传感器数据处理时，建议参考 MDN Web Docs 中关于 Web Worker 的章节（如果是前端可视化部分），或者更专业的 ROS2 文档中关于 tf2 坐标变换的说明。理解坐标系之间的转换关系（Base Link, Laser, Map）是调试机器人漂移问题的关键。
学会语法只是起点，理解代码背后的物理约束和工程权衡，才是从“能跑”到“好用”的分水岭。
你更常用哪种写法？评论区交流：在计算角度差时，你是喜欢用 atan2 归一化，还是直接累加角度再取模？这两种方式在高频控制下会有性能差异，欢迎分享你的实战经验。