简介面向机器人控制学习者的Simulink仿真资源包围绕双轮机器人轨迹跟踪控制重点展示如何在Simulink中搭建差动驱动模型实现圆形与曲线轨迹的精确跟随。压缩包共3个文件含2个Simulink模型.slx和1个MATLAB脚本.m整体仅56KB轻量易用两个模型分别对应圆形与曲线运动控制脚本用于绘制仿真轨迹与误差曲线便于直观分析控制效果。当前已有735人学习/下载适合需要将控制理论快速落地到Simulink仿真、验证轨迹跟踪算法的学生、工程师及爱好者。通过运行模型读者可以观察双轮机器人在预设参考轨迹下的运动响应理解两轮转速差如何影响转向半径与路径曲率并借助模块化设计调整PID控制器参数或替换为滑模控制等算法开展扩展实验是学习机器人运动控制与Simulink建模的实用素材。1. 双轮机器人轨迹跟踪控制误差才是控制对象双轮差速机器人的轨迹跟踪控制经常被误当成“让机器人走得准”的调速问题。真正上手做一圈就会发现直线路径随便一个PID都能稳一到圆弧或者S弯横向误差和航向误差互相耦合调出来的参数换个速度又失效。原因在于差速机器人的运动学带非完整约束输入是(v, ω)两个自由度输出是位姿误差微分方程里带旋转耦合单环PID压不住。下面按“误差状态”来组织先从差速运动学模型出发设计误差方程用纯追踪配合前馈补偿快速搭出控制器再交代前视距离、限幅、积分分离这几个关键参数怎么定最后说实车验证的边界。适合做AGV、比赛机器人和低速轮式平台想从“能跑”跨到“跟踪得上”的工程师。2. 差速运动学与误差状态方程轨迹跟踪控制从哪里下嘴2.1 双轮差速的运动学模型与坐标约定双轮机器人最常见的是两轮独立驱动加一个从动轮或万向轮直线行走靠两侧轮速相等转弯靠轮速差。把左右轮转速记作ωl、ωr轮半径r轮距L机器人的线速度与角速度按下式合成v (ωr·r ωl·r) / 2 ω (ωr·r − ωl·r) / L注意轮距L是左右轮接地点的中心线距离不是底盘宽度。很多底盘标称宽度30cm实际轮距只有24cm左右差几公分会在角速度上放大误差。控制器无论设计成什么样最终都要换算成这两个量要么给(v, ω)要么直接给左右轮速。我一般在上位机算v和ω底层驱动再把v、ω拆成左右轮速。轨迹跟踪控制的目标是让机器人当前位置[x, y, θ]逼近一条参考轨迹。参考轨迹通常由带时间戳的路径点给出纯路径跟踪只要求“压着线走”轨迹跟踪还要求进度对齐。这个区别决定了误差状态必须包含弧长方向的进度偏差。2.2 参考轨迹与误差状态横向误差、航向误差、弧长进度从路径里取参考点最靠近机器人位置的点机器人坐标系下的误差可以拆成三项误差项符号含义测量来源横向误差e_y参考点处的横向偏差最近点投影航向误差e_theta航向与参考切向之差IMU或里程计弧长误差s_e沿路径方向的进度偏差里程累计这三项不互相独立。横向误差大时航向误差通常也大弧长误差又影响参考点选取。把误差从世界坐标系投影到机器人本体坐标系时会用到旋转矩阵[e_y; e_theta] [−sin(θ_ref) cos(θ_ref); 0 1] · (pose − pose_ref)这里θ_ref是参考点切向角。这个投影说明e_y是在参考点坐标系下看的横向分量它在弯道里同时受机器人速度和旋转影响。2.2.1 参考点与最近点搜索误差状态计算的第一步是找参考点。常见做法是对路径点做最近邻搜索计算每个点到机器人当前位置的欧氏距离取最小值再在相邻两点之间做线性插值得到参考点的位置和切向角。路径点间隔大时最近点会在拐角处跳变所以路径点密度应该和速度匹配一般保证相邻点间距不超过0.1倍前视距离。2.2.2 为什么线性PID压不住横向误差如果把e_y单独拎出来做PID得到的是直线路径下可行的控制器但圆弧路径上会一直存在静态误差。原因从e_y的微分方程能看到e_dot_y −v·sin(e_theta)控制量v直接耦合进横向误差而不是通过一个独立的执行器。要消除静态误差就得在控制律里同时处理v和ω这正是后面控制律设计要解决的问题。2.3 误差微分方程与李雅普诺夫候选函数控制律设计先从李雅普诺夫函数入手。构造最简单的一个V (1/2)·e_y² (1/2)·e_theta²对V求导后代入误差方程会发现如果把v设成正常前进速度角速度设计成ω ω_ref − k1·v·e_y·(sin(e_theta)/e_theta) − k2·e_theta那么V_dot近似为−(k1·v·e_y² k2·e_theta²)只要k1、k2为正误差就指数收敛。这个推导解释了三个工程现象k1乘以v出现说明提高前进速度会让横向误差收敛更快但代价是航向噪声被放大ω里必须带ω_ref没有它圆弧跟踪会有恒定航向误偏角速度增益k2只作用于e_theta航向环带宽决定整体稳定性。写一个简单的数值递推来验证这套误差方程和控制律避免直接上实车调试import numpy as np # 参考轨迹半径2.0m的圆弧线速度1.0m/s R 2.0 omega_ref 1.0 / R dt 0.01 T 8.0 n int(T / dt) t_arr np.linspace(0, T, n) # 初始误差横向偏差0.3m航向偏差0.2rad ey 0.3 etheta 0.2 k1 1.0 k2 2.0 history [] for t in t_arr: v 1.0 # 控制律参考角速度前馈 横向误差反馈 航向误差反馈 omega_cmd omega_ref - k1 * v * ey * np.sinc(etheta / np.pi) - k2 * etheta # 用简化误差方程递推 d_ey -v * np.sin(etheta) d_etheta omega_cmd - omega_ref ey d_ey * dt etheta d_etheta * dt history.append((ey, etheta)) history np.array(history) print(最终横向误差: {:.4f} m.format(history[-1, 0])) print(最终航向误差: {:.4f} rad.format(history[-1, 1]))这段代码用前向欧拉法递推误差方程验证控制律是否让两个误差收敛而不是模拟完整位姿。omega_ref是参考轨迹角速度来自v除以半径Rnp.sinc(etheta / np.pi)用于计算sin(e_theta)/e_theta避免航向误差为0时除零。工程上可以直接用sin(etheta)做小角度近似但初始误差必须小于π/2否则李雅普诺夫推导里的近似会失效。跑出来的典型结果是横向误差在1秒内从0.3m收敛到毫米级航向误差的收敛速度由k2决定k2调大时收敛更快但噪声也会被一起放大。3. 基于纯追踪与前馈补偿的双轮机器人轨迹跟踪控制实现3.1 为什么选纯追踪最小实现与可解释性误差方程设计完工程上最常用的落地方案是纯追踪。选择理由有三点第一只需要一个前视距离参数调参路径短第二对路径点没有导数要求离散点直接可用第三输出的角速度指令天然对应曲率不会像PID那样在拐弯处猛打方向。纯追踪的思路是以机器人当前位置为圆心画一个半径为前视距离Ld的圆与参考路径相交选择机器人前方的交点作为目标点然后计算当前位置到目标点的一段圆弧用这段圆弧的曲率作为转向指令。注意纯追踪在曲率较大的地方会截弯取直这是前视距离带来的固有特性所以后面要配合前馈补偿才能把圆弧上的稳态误差压下去。3.2 核心实现目标点搜索到轮速指令假设路径点列表path为numpy数组每行是[x, y, theta_ref]机器人当前位置pose前视距离Ldimport numpy as np def find_goal_point(path, pose, Ld): # 最近点索引用逐点计算欧氏距离 dist np.hypot(path[:, 0] - pose[0], path[:, 1] - pose[1]) idx int(np.argmin(dist)) # 从最近点向前找第一个距离超过Ld的点 for i in range(idx, len(path)): if np.hypot(path[i, 0] - pose[0], path[i, 1] - pose[1]) Ld: return path[i], i return path[-1], len(path) - 1 def pure_pursuit_control(path, pose, v, Ld): goal, idx find_goal_point(path, pose, Ld) dx goal[0] - pose[0] dy goal[1] - pose[1] alpha np.arctan2(dy, dx) - pose[2] # 目标点在机器人横向轴上的偏移 lateral np.sin(alpha) * Ld # 纯追踪核心公式曲率 2 * 横向偏移 / 前视距离平方 curvature 2.0 * lateral / (Ld * Ld) omega v * curvature # 差速轮速分配 wheel_base 0.5 # 轮距0.5m vr (2.0 * v omega * wheel_base) / 2.0 vl (2.0 * v - omega * wheel_base) / 2.0 return omega, vr, vlfind_goal_point从最近点向前搜索避免全量扫描路径点路径几百个点时这个优化很重要。alpha是目标点相对于机器人航向的角度lateral是目标点在机器人横向轴上的偏移curvature 2*lateral/Ld²是纯追踪的核心公式。omega v * curvature把路径几何曲率换算成角速度指令。最后分配左右轮速时wheel_base必须和2.1节里的L一致否则轮速分配出的实际角速度和控制律计算值对不上。3.2.1 主控制循环与路径点预处理实际工程不会每周期都重新计算整条路径。通常在收到目标路径后先做一次重采样把离散点按固定弧长间距排好控制循环里只负责找目标点和计算控制量。主循环大致如下import time dt 0.02 # 控制周期20ms50Hz max_speed 1.0 # 最大线速度 m/s Ld 0.6 # 前视距离 running True while running: pose get_odom() # 从里程计读取位姿 ref_v min(max_speed, compute_speed_profile(pose)) omega, vr, vl pure_pursuit_control(ref_path, pose, ref_v, Ld) # 这里可以插入限幅和斜坡处理 send_wheel_speed(vr, vl) time.sleep(dt)compute_speed_profile根据前方曲率调整期望速度最简单的策略是目标点附近参考曲率大于阈值时降速保证前视距离对应的圆弧半径还成立。控制周期我一般用20ms也就是50Hz既能覆盖普通AGV的机动需求也给底层电机驱动留出响应窗口。如果控制周期超过50ms前视距离要相应加大不然每步之间目标点跳变会非常明显。3.3 前馈补偿把曲率直接加进角速度指令纯追踪本质是比例控制跟踪的是“一个前视点”在圆弧上会形成稳态误差机器人实际轨迹的曲率总是小于参考曲率。解决方法是把参考点曲率直接前馈进角速度指令omega_cmd v / R_ref k_p * e_theta其中R_ref是参考点处的曲率半径e_theta是航向误差。没有前馈时纯追踪在圆弧上有恒定横向偏置加了前馈后横向偏差只来自扰动和量化误差。def ref_curvature(path, idx): if idx 1 or idx len(path) - 1: return 0.0 yaw1 path[idx - 1, 2] yaw2 path[idx 1, 2] d_yaw np.arctan2(np.sin(yaw2 - yaw1), np.cos(yaw2 - yaw1)) ds np.hypot(path[idx 1, 0] - path[idx - 1, 0], path[idx 1, 1] - path[idx - 1, 1]) return d_yaw / ds ref_omega v * ref_curvature(ref_path, idx) omega_final omega_pure * 0.7 ref_omega * 0.3前馈权重不需要太大30%左右就能消除稳态曲率偏差。权重过大反而会在路径点稀疏的地方引入突变。另一种常见做法是保留纯追踪的横向比例反馈把它的曲率项全部替换为ref_omega横向误差只作为微调。我自己的调参习惯是先让前馈完全生效再把权重逐渐降下来观察横向稳态误差何时开始明显增大取那个临界点往回留20%余量。4. 轨迹跟踪控制器的必调参数前视距离、限幅与积分分离4.1 前视距离Ld决定“看多远”与速度强相关纯追踪的前视距离不能拍脑袋填。太小会让机器人频繁修正方向走S形太大会在弯道里直接切掉弯心走外围。常用做法是让前视距离按速度线性变化Ld kv * v Ld_minv单位是m/s推荐kv在0.5到1.2之间Ld_min取0.3到0.5m。低速时全靠Ld_min保证机器人至少“看得见”前方一段路高速时按比例加长。参数选择和现象对照Ld取值典型现象适用场景远大于标准值弯道切内线直线收敛慢大曲率变化少的园区路等于Ld_min直线走直弯道内切严重低速到点停车按kv比例调整直线和弯道表现均衡结构化路径注意Ld对控制周期很敏感。控制频率10Hz时Ld最好不超过单周期行驶距离的3倍否则机器人持续对旧目标点闭环表现就是来回摆头。如果必须用低频控制就把Ld加大一档接受曲线内切。4.2 限幅与加速度约束防止轮速指令突变差速机器人的电机指令突变轻则电流冲击重则打滑。控制器算出ω_cmd后要在最终输出前套一个斜坡限幅def clamp_and_ramp(omega_cmd, omega_prev, max_step, max_omega): # 先做绝对值限幅 omega np.clip(omega_cmd, -max_omega, max_omega) # 再做每周期步长限幅限制角加速度 delta omega - omega_prev if abs(delta) max_step: omega omega_prev np.sign(delta) * max_step return omegamax_omega是角速度绝对上限0.5m轮距、1m/s线速度的平台取2.0rad/s左右max_step是每一步允许的角速度增量控制频率50Hz时取0.2~0.5rad/s比较稳妥。这个限幅比在PID里加低通滤波直观因为它直接对应物理上的加速度约束。左右轮速也要单独限幅防止某个轮子被算成负值导致原地打转。4.3 积分分离的PID修正横向误差闭环很多工程实现会在纯追踪外层挂一个横向误差PID用来补偿轮径差异和环境扰动。关键点是积分分离。横向误差超过0.2m时机器人处于大偏差矫正状态积分只会带来超调必须关掉误差小于0.1m进入稳态后再开启积分消残留偏差。典型参数参数取值范围说明Kp0.5~2.0横向误差比例越小越顺滑Ki0.1~0.5在|e_y|0.1m时启用Kd0.1~0.3低噪声编码器上才有效调参顺序先Kp后Ki再视抖动情况降Kd。这个PID的输出作为ω_cmd的附加量而不是直接乘进去。如果实车噪声大Kd直接设0转速环自身的阻尼往往已经够用。积分分离实现时可以加一道滞回进入条件0.1m退出条件0.15m避免在阈值附近反复开启关闭积分。4.3.1 从仿真参数到实车参数的迁移规则仿真里调好的k1、k2、Ld不能直接搬到实车。仿真用的是理想轮速模型实车有轮径差、电机响应和底盘悬挂时延。迁移规则是先按仿真参数的60%起步跑直线确认无抖动再逐步提高比例前视距离里的kv要从0.5开始实车轮胎抓地力差就降到0.3同时把积分分离阈值从仿真的0.1m放宽到0.15m因为实车里程计的噪声会让误差在阈值边缘抖动。经验是实车参数和仿真参数差20%~40%都算正常超过50%就要回头检查轮距和轮径标定。5. 验证闭环离线回放、硬实时检查与打滑边界5.1 用CSV回放检验控制延时实车调试第一步不是上路而是把指令速度和里程计速度录成CSV离线分析控制延时。具体做法是给一个速度阶跃指令看里程计响应延迟同时把控制线程实际周期和指令周期对比。如果指令周期10ms控制线程实际15ms才发一次指令前视距离就要按实际周期重算。回放时画两条曲线指令速度和里程计速度两者的延迟就是控制回路主要滞后。低于50ms才适合跑轨迹跟踪超过100ms要先修控制线程。5.2 打滑边界与曲率异常检测差速机器人在快速转弯时打滑最直接的观测是轮速推算的角速度与IMU角速度不一致wheel_omega (vr − vl) / wheel_base slip_ratio abs(wheel_omega − imu_omega) / max(abs(imu_omega), 1e-3)当slip_ratio连续0.5秒超过0.3判定打滑。急转弯时这个比值容易瞬间跳到0.8控制器还按轮速当作真实反馈航向误差会越纠越偏。检测到打滑后把横向误差PID的积分清零同时降低v和ω_cmd以重新获得抓地力。纯追踪对这种打滑抑制很弱所以实车限幅参数里max_step要留足余量。5.3 用RMS误差做回归基线验证阶段还要有一个能反复比较的指标我习惯用横向误差的RMS作为回归基线。录一段包含直线、圆弧、S弯的测试路径跑完计算def rms_error(ey_history): return np.sqrt(np.mean(np.square(ey_history)))每次改参数后跑同一段路RMS下降说明改动方向对上升则回滚。可以给这个指标定一个验收阈值直线路段RMS小于2cm、圆弧路段小于5cm、S弯小于6cm算是工程上可用的跟踪水平。低于这个标准优先检查前视距离和积分分离高于标准再看底层轮速响应。最后留一个启动动作从路径一侧0.5m外横向切入看系统能不能快速回到参考轨迹。能收敛且没有明显超调说明前视距离和积分分离匹配是合理的这时记录一版误差曲线作为后续改动的基线。本文还有配套的精品资源点击获取 
长河君 · 2026/9/10 4:50:39 · 阅读 1.2k 双轮差速机器人轨迹跟踪控制：从误差方程到纯追踪与前馈补偿
简介面向机器人控制学习者的Simulink仿真资源包围绕双轮机器人轨迹跟踪控制重点展示如何在Simulink中搭建差动驱动模型实现圆形与曲线轨迹的精确跟随。压缩包共3个文件含2个Simulink模型.slx和1个MATLAB脚本.m整体仅56KB轻量易用两个模型分别对应圆形与曲线运动控制脚本用于绘制仿真轨迹与误差曲线便于直观分析控制效果。当前已有735人学习/下载适合需要将控制理论快速落地到Simulink仿真、验证轨迹跟踪算法的学生、工程师及爱好者。通过运行模型读者可以观察双轮机器人在预设参考轨迹下的运动响应理解两轮转速差如何影响转向半径与路径曲率并借助模块化设计调整PID控制器参数或替换为滑模控制等算法开展扩展实验是学习机器人运动控制与Simulink建模的实用素材。1. 双轮机器人轨迹跟踪控制误差才是控制对象双轮差速机器人的轨迹跟踪控制经常被误当成“让机器人走得准”的调速问题。真正上手做一圈就会发现直线路径随便一个PID都能稳一到圆弧或者S弯横向误差和航向误差互相耦合调出来的参数换个速度又失效。原因在于差速机器人的运动学带非完整约束输入是(v, ω)两个自由度输出是位姿误差微分方程里带旋转耦合单环PID压不住。下面按“误差状态”来组织先从差速运动学模型出发设计误差方程用纯追踪配合前馈补偿快速搭出控制器再交代前视距离、限幅、积分分离这几个关键参数怎么定最后说实车验证的边界。适合做AGV、比赛机器人和低速轮式平台想从“能跑”跨到“跟踪得上”的工程师。2. 差速运动学与误差状态方程轨迹跟踪控制从哪里下嘴2.1 双轮差速的运动学模型与坐标约定双轮机器人最常见的是两轮独立驱动加一个从动轮或万向轮直线行走靠两侧轮速相等转弯靠轮速差。把左右轮转速记作ωl、ωr轮半径r轮距L机器人的线速度与角速度按下式合成v (ωr·r ωl·r) / 2 ω (ωr·r − ωl·r) / L注意轮距L是左右轮接地点的中心线距离不是底盘宽度。很多底盘标称宽度30cm实际轮距只有24cm左右差几公分会在角速度上放大误差。控制器无论设计成什么样最终都要换算成这两个量要么给(v, ω)要么直接给左右轮速。我一般在上位机算v和ω底层驱动再把v、ω拆成左右轮速。轨迹跟踪控制的目标是让机器人当前位置[x, y, θ]逼近一条参考轨迹。参考轨迹通常由带时间戳的路径点给出纯路径跟踪只要求“压着线走”轨迹跟踪还要求进度对齐。这个区别决定了误差状态必须包含弧长方向的进度偏差。2.2 参考轨迹与误差状态横向误差、航向误差、弧长进度从路径里取参考点最靠近机器人位置的点机器人坐标系下的误差可以拆成三项误差项符号含义测量来源横向误差e_y参考点处的横向偏差最近点投影航向误差e_theta航向与参考切向之差IMU或里程计弧长误差s_e沿路径方向的进度偏差里程累计这三项不互相独立。横向误差大时航向误差通常也大弧长误差又影响参考点选取。把误差从世界坐标系投影到机器人本体坐标系时会用到旋转矩阵[e_y; e_theta] [−sin(θ_ref) cos(θ_ref); 0 1] · (pose − pose_ref)这里θ_ref是参考点切向角。这个投影说明e_y是在参考点坐标系下看的横向分量它在弯道里同时受机器人速度和旋转影响。2.2.1 参考点与最近点搜索误差状态计算的第一步是找参考点。常见做法是对路径点做最近邻搜索计算每个点到机器人当前位置的欧氏距离取最小值再在相邻两点之间做线性插值得到参考点的位置和切向角。路径点间隔大时最近点会在拐角处跳变所以路径点密度应该和速度匹配一般保证相邻点间距不超过0.1倍前视距离。2.2.2 为什么线性PID压不住横向误差如果把e_y单独拎出来做PID得到的是直线路径下可行的控制器但圆弧路径上会一直存在静态误差。原因从e_y的微分方程能看到e_dot_y −v·sin(e_theta)控制量v直接耦合进横向误差而不是通过一个独立的执行器。要消除静态误差就得在控制律里同时处理v和ω这正是后面控制律设计要解决的问题。2.3 误差微分方程与李雅普诺夫候选函数控制律设计先从李雅普诺夫函数入手。构造最简单的一个V (1/2)·e_y² (1/2)·e_theta²对V求导后代入误差方程会发现如果把v设成正常前进速度角速度设计成ω ω_ref − k1·v·e_y·(sin(e_theta)/e_theta) − k2·e_theta那么V_dot近似为−(k1·v·e_y² k2·e_theta²)只要k1、k2为正误差就指数收敛。这个推导解释了三个工程现象k1乘以v出现说明提高前进速度会让横向误差收敛更快但代价是航向噪声被放大ω里必须带ω_ref没有它圆弧跟踪会有恒定航向误偏角速度增益k2只作用于e_theta航向环带宽决定整体稳定性。写一个简单的数值递推来验证这套误差方程和控制律避免直接上实车调试import numpy as np # 参考轨迹半径2.0m的圆弧线速度1.0m/s R 2.0 omega_ref 1.0 / R dt 0.01 T 8.0 n int(T / dt) t_arr np.linspace(0, T, n) # 初始误差横向偏差0.3m航向偏差0.2rad ey 0.3 etheta 0.2 k1 1.0 k2 2.0 history [] for t in t_arr: v 1.0 # 控制律参考角速度前馈 横向误差反馈 航向误差反馈 omega_cmd omega_ref - k1 * v * ey * np.sinc(etheta / np.pi) - k2 * etheta # 用简化误差方程递推 d_ey -v * np.sin(etheta) d_etheta omega_cmd - omega_ref ey d_ey * dt etheta d_etheta * dt history.append((ey, etheta)) history np.array(history) print(最终横向误差: {:.4f} m.format(history[-1, 0])) print(最终航向误差: {:.4f} rad.format(history[-1, 1]))这段代码用前向欧拉法递推误差方程验证控制律是否让两个误差收敛而不是模拟完整位姿。omega_ref是参考轨迹角速度来自v除以半径Rnp.sinc(etheta / np.pi)用于计算sin(e_theta)/e_theta避免航向误差为0时除零。工程上可以直接用sin(etheta)做小角度近似但初始误差必须小于π/2否则李雅普诺夫推导里的近似会失效。跑出来的典型结果是横向误差在1秒内从0.3m收敛到毫米级航向误差的收敛速度由k2决定k2调大时收敛更快但噪声也会被一起放大。3. 基于纯追踪与前馈补偿的双轮机器人轨迹跟踪控制实现3.1 为什么选纯追踪最小实现与可解释性误差方程设计完工程上最常用的落地方案是纯追踪。选择理由有三点第一只需要一个前视距离参数调参路径短第二对路径点没有导数要求离散点直接可用第三输出的角速度指令天然对应曲率不会像PID那样在拐弯处猛打方向。纯追踪的思路是以机器人当前位置为圆心画一个半径为前视距离Ld的圆与参考路径相交选择机器人前方的交点作为目标点然后计算当前位置到目标点的一段圆弧用这段圆弧的曲率作为转向指令。注意纯追踪在曲率较大的地方会截弯取直这是前视距离带来的固有特性所以后面要配合前馈补偿才能把圆弧上的稳态误差压下去。3.2 核心实现目标点搜索到轮速指令假设路径点列表path为numpy数组每行是[x, y, theta_ref]机器人当前位置pose前视距离Ldimport numpy as np def find_goal_point(path, pose, Ld): # 最近点索引用逐点计算欧氏距离 dist np.hypot(path[:, 0] - pose[0], path[:, 1] - pose[1]) idx int(np.argmin(dist)) # 从最近点向前找第一个距离超过Ld的点 for i in range(idx, len(path)): if np.hypot(path[i, 0] - pose[0], path[i, 1] - pose[1]) Ld: return path[i], i return path[-1], len(path) - 1 def pure_pursuit_control(path, pose, v, Ld): goal, idx find_goal_point(path, pose, Ld) dx goal[0] - pose[0] dy goal[1] - pose[1] alpha np.arctan2(dy, dx) - pose[2] # 目标点在机器人横向轴上的偏移 lateral np.sin(alpha) * Ld # 纯追踪核心公式曲率 2 * 横向偏移 / 前视距离平方 curvature 2.0 * lateral / (Ld * Ld) omega v * curvature # 差速轮速分配 wheel_base 0.5 # 轮距0.5m vr (2.0 * v omega * wheel_base) / 2.0 vl (2.0 * v - omega * wheel_base) / 2.0 return omega, vr, vlfind_goal_point从最近点向前搜索避免全量扫描路径点路径几百个点时这个优化很重要。alpha是目标点相对于机器人航向的角度lateral是目标点在机器人横向轴上的偏移curvature 2*lateral/Ld²是纯追踪的核心公式。omega v * curvature把路径几何曲率换算成角速度指令。最后分配左右轮速时wheel_base必须和2.1节里的L一致否则轮速分配出的实际角速度和控制律计算值对不上。3.2.1 主控制循环与路径点预处理实际工程不会每周期都重新计算整条路径。通常在收到目标路径后先做一次重采样把离散点按固定弧长间距排好控制循环里只负责找目标点和计算控制量。主循环大致如下import time dt 0.02 # 控制周期20ms50Hz max_speed 1.0 # 最大线速度 m/s Ld 0.6 # 前视距离 running True while running: pose get_odom() # 从里程计读取位姿 ref_v min(max_speed, compute_speed_profile(pose)) omega, vr, vl pure_pursuit_control(ref_path, pose, ref_v, Ld) # 这里可以插入限幅和斜坡处理 send_wheel_speed(vr, vl) time.sleep(dt)compute_speed_profile根据前方曲率调整期望速度最简单的策略是目标点附近参考曲率大于阈值时降速保证前视距离对应的圆弧半径还成立。控制周期我一般用20ms也就是50Hz既能覆盖普通AGV的机动需求也给底层电机驱动留出响应窗口。如果控制周期超过50ms前视距离要相应加大不然每步之间目标点跳变会非常明显。3.3 前馈补偿把曲率直接加进角速度指令纯追踪本质是比例控制跟踪的是“一个前视点”在圆弧上会形成稳态误差机器人实际轨迹的曲率总是小于参考曲率。解决方法是把参考点曲率直接前馈进角速度指令omega_cmd v / R_ref k_p * e_theta其中R_ref是参考点处的曲率半径e_theta是航向误差。没有前馈时纯追踪在圆弧上有恒定横向偏置加了前馈后横向偏差只来自扰动和量化误差。def ref_curvature(path, idx): if idx 1 or idx len(path) - 1: return 0.0 yaw1 path[idx - 1, 2] yaw2 path[idx 1, 2] d_yaw np.arctan2(np.sin(yaw2 - yaw1), np.cos(yaw2 - yaw1)) ds np.hypot(path[idx 1, 0] - path[idx - 1, 0], path[idx 1, 1] - path[idx - 1, 1]) return d_yaw / ds ref_omega v * ref_curvature(ref_path, idx) omega_final omega_pure * 0.7 ref_omega * 0.3前馈权重不需要太大30%左右就能消除稳态曲率偏差。权重过大反而会在路径点稀疏的地方引入突变。另一种常见做法是保留纯追踪的横向比例反馈把它的曲率项全部替换为ref_omega横向误差只作为微调。我自己的调参习惯是先让前馈完全生效再把权重逐渐降下来观察横向稳态误差何时开始明显增大取那个临界点往回留20%余量。4. 轨迹跟踪控制器的必调参数前视距离、限幅与积分分离4.1 前视距离Ld决定“看多远”与速度强相关纯追踪的前视距离不能拍脑袋填。太小会让机器人频繁修正方向走S形太大会在弯道里直接切掉弯心走外围。常用做法是让前视距离按速度线性变化Ld kv * v Ld_minv单位是m/s推荐kv在0.5到1.2之间Ld_min取0.3到0.5m。低速时全靠Ld_min保证机器人至少“看得见”前方一段路高速时按比例加长。参数选择和现象对照Ld取值典型现象适用场景远大于标准值弯道切内线直线收敛慢大曲率变化少的园区路等于Ld_min直线走直弯道内切严重低速到点停车按kv比例调整直线和弯道表现均衡结构化路径注意Ld对控制周期很敏感。控制频率10Hz时Ld最好不超过单周期行驶距离的3倍否则机器人持续对旧目标点闭环表现就是来回摆头。如果必须用低频控制就把Ld加大一档接受曲线内切。4.2 限幅与加速度约束防止轮速指令突变差速机器人的电机指令突变轻则电流冲击重则打滑。控制器算出ω_cmd后要在最终输出前套一个斜坡限幅def clamp_and_ramp(omega_cmd, omega_prev, max_step, max_omega): # 先做绝对值限幅 omega np.clip(omega_cmd, -max_omega, max_omega) # 再做每周期步长限幅限制角加速度 delta omega - omega_prev if abs(delta) max_step: omega omega_prev np.sign(delta) * max_step return omegamax_omega是角速度绝对上限0.5m轮距、1m/s线速度的平台取2.0rad/s左右max_step是每一步允许的角速度增量控制频率50Hz时取0.2~0.5rad/s比较稳妥。这个限幅比在PID里加低通滤波直观因为它直接对应物理上的加速度约束。左右轮速也要单独限幅防止某个轮子被算成负值导致原地打转。4.3 积分分离的PID修正横向误差闭环很多工程实现会在纯追踪外层挂一个横向误差PID用来补偿轮径差异和环境扰动。关键点是积分分离。横向误差超过0.2m时机器人处于大偏差矫正状态积分只会带来超调必须关掉误差小于0.1m进入稳态后再开启积分消残留偏差。典型参数参数取值范围说明Kp0.5~2.0横向误差比例越小越顺滑Ki0.1~0.5在|e_y|0.1m时启用Kd0.1~0.3低噪声编码器上才有效调参顺序先Kp后Ki再视抖动情况降Kd。这个PID的输出作为ω_cmd的附加量而不是直接乘进去。如果实车噪声大Kd直接设0转速环自身的阻尼往往已经够用。积分分离实现时可以加一道滞回进入条件0.1m退出条件0.15m避免在阈值附近反复开启关闭积分。4.3.1 从仿真参数到实车参数的迁移规则仿真里调好的k1、k2、Ld不能直接搬到实车。仿真用的是理想轮速模型实车有轮径差、电机响应和底盘悬挂时延。迁移规则是先按仿真参数的60%起步跑直线确认无抖动再逐步提高比例前视距离里的kv要从0.5开始实车轮胎抓地力差就降到0.3同时把积分分离阈值从仿真的0.1m放宽到0.15m因为实车里程计的噪声会让误差在阈值边缘抖动。经验是实车参数和仿真参数差20%~40%都算正常超过50%就要回头检查轮距和轮径标定。5. 验证闭环离线回放、硬实时检查与打滑边界5.1 用CSV回放检验控制延时实车调试第一步不是上路而是把指令速度和里程计速度录成CSV离线分析控制延时。具体做法是给一个速度阶跃指令看里程计响应延迟同时把控制线程实际周期和指令周期对比。如果指令周期10ms控制线程实际15ms才发一次指令前视距离就要按实际周期重算。回放时画两条曲线指令速度和里程计速度两者的延迟就是控制回路主要滞后。低于50ms才适合跑轨迹跟踪超过100ms要先修控制线程。5.2 打滑边界与曲率异常检测差速机器人在快速转弯时打滑最直接的观测是轮速推算的角速度与IMU角速度不一致wheel_omega (vr − vl) / wheel_base slip_ratio abs(wheel_omega − imu_omega) / max(abs(imu_omega), 1e-3)当slip_ratio连续0.5秒超过0.3判定打滑。急转弯时这个比值容易瞬间跳到0.8控制器还按轮速当作真实反馈航向误差会越纠越偏。检测到打滑后把横向误差PID的积分清零同时降低v和ω_cmd以重新获得抓地力。纯追踪对这种打滑抑制很弱所以实车限幅参数里max_step要留足余量。5.3 用RMS误差做回归基线验证阶段还要有一个能反复比较的指标我习惯用横向误差的RMS作为回归基线。录一段包含直线、圆弧、S弯的测试路径跑完计算def rms_error(ey_history): return np.sqrt(np.mean(np.square(ey_history)))每次改参数后跑同一段路RMS下降说明改动方向对上升则回滚。可以给这个指标定一个验收阈值直线路段RMS小于2cm、圆弧路段小于5cm、S弯小于6cm算是工程上可用的跟踪水平。低于这个标准优先检查前视距离和积分分离高于标准再看底层轮速响应。最后留一个启动动作从路径一侧0.5m外横向切入看系统能不能快速回到参考轨迹。能收敛且没有明显超调说明前视距离和积分分离匹配是合理的这时记录一版误差曲线作为后续改动的基线。本文还有配套的精品资源点击获取