简介本资源面向机器人控制、智能车辆路径规划方向的本科生与研究生提供一套完整的阿克曼转向小车纯追踪Pure Pursuit, PP算法仿真与可视化实现方案。针对轮式机器人运动学建模与非线性路径跟踪难点资源集成了运动学建模、PP控制器设计、实时索引搜索、车辆姿态绘制及动画导出等核心功能适用于课程设计、毕业设计及算法验证场景。压缩包共9个文件7个MATLAB脚本用于模型构建、控制器实现与绘图1份Word文档详解运动学公式与参数推导1个MP4实操录屏直观展示仿真效果总大小2.95MB结构紧凑、模块职责清晰便于理解与二次开发。目前已有191人学习下载配套动画生成脚本与完整可运行主程序main.m显著降低上手门槛结合录屏演示与公式说明有效支撑从原理理解到代码复现的全流程学习。 先说明一点这个项目我在做轮式机器人路径跟踪的时候折腾过一版最近又用 Matlab 重新整理了一遍把 Pure PursuitPP算法的代码、仿真模型和动画输出整个串了起来。如果你也要做阿克曼底盘的路径跟踪仿真这篇东西应该能让你少踩几个坑。你不需要装任何复杂的外部工具只要电脑上有 Matlab 就行版本 R2020a 之后的基本都够用。阿克曼小车这个东西和差速小车最大的区别就是它的转向机构决定了车辆转向时所有车轮的瞬时转动中心是同一点因此转弯半径不是你想定多少就能定多少的必须满足阿克曼几何约束。这带来的直接后果就是各种基于差速模型的算法不能直接套得用车辆运动学模型而 Pure Pursuit 恰好是为这一类约束设计的路径跟踪算法原理简单、参数直观、工程上也够用所以我选了它来做跟踪仿真。下文就按我从建模到仿真再到动画生成的实际流程来写重点放在数学模型、代码实现和排错上。1. 项目整体设计思路与方案选型1.1 为什么选阿克曼模型做路径跟踪仿真做路径跟踪仿真前提是先有一个能描述车辆运动状态的对象。市面上常见的室内移动机器人大多是差速驱动两轮独立驱动靠两侧轮速差实现转向数学模型是一个两输入的单车模型控制起来非常直接。但我这个项目面向的其实是类似快递小车、园区巡检车这一类室外轮式机器人它们的底盘通常是阿克曼转向结构也就是前轮负责转向、后轮负责驱动或者是四轮驱动加前轮转向不管驱动形式怎么变转向几何都类似汽车。阿克曼模型的本质约束是车辆转弯时四个轮子要能绕同一个瞬时转动中心做纯滚动否则轮胎会产生明显滑移这一点和差速模型的“原地旋转”特性完全不同。所以在仿真里如果忽略了这个约束算出再漂亮的轨迹在实车上也基本跑不出来。用阿克曼模型做跟踪仿真核心价值就是让你从一开始就在“真实的转向约束”下开发和调试算法避免做出一个只能在仿真里成立的花架子。1.2 跟踪算法选型Pure Pursuit 为什么够用路径跟踪的算法有不少比如 Stanley 法、模型预测控制MPC、线性二次调节器LQR跟踪等。我为什么挑 Pure Pursuit 而不是更“高级”的算法实现成本低。PP 的核心只需要一个几何公式没有复杂的求解器写出来大概不超过 30 行代码。参数语义清晰。核心就一个前视距离相当于“你让车盯着前方多远的地方去追”物理意义非常直观便于调参。对模型误差不敏感。它本身是基于几何关系的控制不需要特别精确的车辆模型适合做算法验证和快速原型。后续换算法也方便。PP 跑通之后整个仿真框架可以直接复用把控制器换成 Stanley 或者 MPC 再对比工作量很小。当然 PP 也有短板比如它对前视距离很敏感前视太短容易振荡太长容易切弯过急的弯道性能会下降。不过对于做仿真验证和入门来说它是性价比最高的选择这也是我把它作为这个项目核心算法的原因。1.3 仿真与动画的整体架构整个项目我把它拆成四个模块车辆模型模块输入速度与转向角通过运动学方程计算下一时刻的车辆位置和航向角。路径模块生成目标路径这里我做了圆形、八字形和直线加弯道三种方便观察算法在不同曲率下的表现。PP 控制器模块根据当前位姿和目标路径求解前轮转角再输入给车辆模型。可视化模块实时绘制车辆位置、参考路径和跟踪误差同时按帧导出动画。这四个模块对应到代码上就是四个函数加上一个主脚本逻辑非常清楚也方便后面单独替换某一部分做对比实验。2. 阿克曼转向模型建模与数学推导2.1 从四轮模型到自行车模型的简化阿克曼小车完整建模可以用四轮模型但四轮模型的轮胎受力和侧偏特性很复杂用于纯几何运动学仿真时反而会引入不必要的参数。实际上很多时候我们不需要算到那个精度直接把左右前轮等效成一个位于车辆中轴线上的虚拟前轮左右后轮等效成一个虚拟后轮这就得到了经典的单车模型Bicycle Model。为什么可以这样简化因为阿克曼转向的设计目标就是保证在转向过程中所有车轮绕同一瞬时中心转动。既然绕同一个中心转那么用一个位于车辆中轴线上的等效轮就能代表左右轮的几何效果转向角取内外轮的某个等效角度即可。这样处理之后模型从四个轮子变成了“前轮-后轮”两个点计算量大幅减少精度对路径跟踪仿真也基本够用。2.2 运动学方程推导简化后的模型取后轴中心作为车辆参考点这一点很重要后面 Pure Pursuit 的公式也基于这个参考点。状态量定义为x后轴中心的横坐标y后轴中心的纵坐标θ车辆航向角车头方向与 x 轴正方向夹角v车辆速度这里默认是后轮驱动的纵向速度δ前轮等效转角在车辆坐标系下前轮到后轴中心的距离就是轴距 L。由于后轮不能侧向滑动所以后轴中心的瞬时速度方向始终沿车辆纵轴方向而前轮的速度方向则要满足前轮转向角约束。通过对几何关系求导得到$$ \dot{x} v \cos \theta $$$$ \dot{y} v \sin \theta $$$$ \dot{\theta} \frac{v \tan \delta}{L} $$这三个方程就是我在仿真里用来做状态递推的核心方程。其中前两个是质心位置的速度分解第三个则是根据“前轮转角决定转弯半径”这一几何关系推导出来的。转弯半径 R 和 δ 的关系是$$ R \frac{L}{\tan \delta} $$这个式子从直观上理解前轮转角越大tan δ 越大转弯半径越小车转得越急轴距越长转弯半径越大车越“稳”。2.3 转向几何中的阿克曼约束严格来说左右前轮的角度不是相等的。如果四轮绕同一个转弯中心内侧前轮的转角应该略大于外侧前轮这个差值就是阿克曼修正。对于普通仿真很多简化的做法是直接给定一个等效的前轮转角 δ认为左右轮的阿克曼修正已经通过转向梯形机构自动完成。但如果你希望仿真做得更细一点可以单独给左右前轮分别计算转向角$$ \delta_{\text{inner}} \arctan\left(\frac{L}{R - \frac{w}{2}}\right) $$$$ \delta_{\text{outer}} \arctan\left(\frac{L}{R \frac{w}{2}}\right) $$其中 w 是轮距。这一步我建议在代码里做成单独的阿克曼转向几何函数这样当你换成四轮可视化模型时可以直接用左右轮转角去画转向效果。不过在运动学层面使用等效 δ 就足够了。2.4 仿真中的状态递推状态方程拿到手后仿真里就是用数值积分不断更新状态。我用了最简单的欧拉法因为采样时间选得足够小0.01 秒欧拉法的精度已经足够观察控制效果没必要一开始就上龙格库塔。x(k1) x(k) v * cos(theta(k)) * dt y(k1) y(k) v * sin(theta(k)) * dt theta(k1) theta(k) v * tan(delta(k)) / L * dt这里有一个细节theta 在 Matlab 里做角度运算时要用弧度制只有在显示的时候才转成度数否则三角函数算出来全是错的。这个坑我一开始踩过后面排查发现是单位混用导致的。3. Pure Pursuit 跟踪算法的核心原理与参数计算3.1 Pure Pursuit 的几何直觉Pure Pursuit 算法可以理解为让车辆始终盯着目标路径上的某个“前方点”去追。这个目标点不是任意选的而是以车辆当前后轴中心为圆心、以前视距离 Ld 为半径画一个圆这个圆和参考路径的交点就是目标点。一旦确定了目标点控制器要做的事情就很简单计算当前车辆航向角到目标点之间夹角然后输出一个能让车辆朝那个方向转弯的前轮转角。整个控制律的直觉非常像人开车时盯着前方某个参照物开过去因此得名“纯追踪”。3.2 目标点选取的具体方法实际实现中参考路径通常是一系列离散点比如我之前设定的路径点间距是 0.1 米。目标点选取逻辑如下遍历路径点集合计算每个点与当前后轴中心的距离。找出距离第一次超过前视距离 Ld 的那个点作为目标点。如果整条路径都走完了则选择最后一个点作为目标点。写成 Matlab 代码就是function targetIdx findTargetPoint(pose, path, Ld) dists sqrt((path(:,1) - pose(1)).^2 (path(:,2) - pose(2)).^2); targetIdx find(dists Ld, 1, first); if isempty(targetIdx) targetIdx size(path, 1); end end需要注意一个问题路径如果是闭合曲线比如圆形车辆跑完一圈后当前点可能离起点很近这时如果还去找“第一次大于 Ld 的点”容易在起点附近出现跳变。我处理的办法是维护一个全局进度计数器目标点索引只能往前移动不允许回退这样就能避免目标点在路径上左右横跳。3.3 转向角公式的推导过程目标和当前车辆位置确定后设目标点在车辆坐标系下的方位角为 α利用正弦定理可以推导出前轮转角。具体推导如下车辆后轴中心记为 O目标点记为 PO 到 P 的距离为 Ld。车辆的瞬时转弯半径为 R车辆转弯时后轴中心轨迹是一个半径为 R 的圆弧。由于后轴中心到瞬时转动中心记为 C的距离是 R所以三角形 OCP 中OC 等于 ROP 等于 Ld角 COP 为 α目标点方向与当前航向的夹角角 OPC 为 90° - α 或是更精确的几何关系利用正弦定理最终得到曲率 κ 和 α 的关系。工程上常用的结果形式为$$ \kappa \frac{2 \sin \alpha}{L_d} $$转弯半径 R 与曲率互为倒数而轮胎转角与转弯半径的关系是 R L / tan δ所以联立得到$$ \delta \arctan \frac{2 L \sin \alpha}{L_d} $$这里有一个特别容易踩坑的点α 的含义。它通常是目标点方向相对车辆航向方向的夹角而不是目标点在世界坐标系下的绝对角度。因此代码里要用 atan2 求出目标点相对车辆的方位角再减去航向角 θ再做角度归一化到 [-π, π]。3.4 前视距离 Ld 的选择与自适应整个 PP 算法里最影响效果的就是 Ld。Ld 取小了车辆会频繁修正方向轨迹跟踪精度看着高但容易出现振荡Ld 取大了车辆转向很柔和但过弯时会明显切内线跟踪误差变大。我调试时用到的经验值轴距 L 1 米速度 v 2 m/s固定 Ld 1.5 米在圆形路径上效果还可以跟踪误差基本在 0.1 米以内。但把速度提到 4 m/s 之后固定 Ld 就会出现明显的“甩尾感”实际轨迹比参考路径滞后很多。更好的做法是让 Ld 和速度挂钩$$ L_d k_{pp} v L_{d,\min} $$我常用 k_pp 0.3Ld_min 1.2 米。这样车速越快预瞄得越远系统不至于因为响应延迟而失控。调节 k_pp 时一般从小往大试小 k_pp 会让跟踪更激进大 k_pp 会让跟踪更保守。我的习惯是先固定速度观察圆形路径跟踪误差曲线如果误差曲线振荡明显就加大 Ld如果弯道内侧切得太明显就减小 Ld。4. Matlab 仿真主程序实现与代码解析4.1 主程序框架设计我最终的代码结构是这样的ackermann_pp_sim/ ├── main_pp_sim.m ├── vehicle_model.m ├── pure_pursuit_controller.m ├── reference_path.m ├── update_animation.m └── export_animation_gif.m主程序只负责初始化参数、循环仿真、保存数据、调用可视化函数。这样模块很干净想换路径生成器或者换控制器都不需要动主循环。主程序里关键的初始化部分我贴出来大家可以直接参考%% 参数初始化 L 1.0; % 轴距单位米 w 0.7; % 轮距单位米主要用于画图和四轮几何 v 2.0; % 车辆速度单位 m/s dt 0.01; % 仿真步长单位 s t_end 30; % 仿真总时长单位 s Ld 1.5; % 固定前视距离单位 m % 如果使用自适应前视距离则 k_pp 0.3; % 前视距离速度系数 Ld_min 1.2; % 最小前视距离 max_steer deg2rad(30); % 最大转向角单位弧度 %% 生成参考路径 path reference_path(circle, 5.0); % 半径5米的圆形路径 %% 初始状态 x0 path(1,1) 0.5; % 故意放一点初始偏差方便观察收敛过程 y0 path(1,2); theta0 deg2rad(0); pose [x0; y0; theta0]; %% 存储数据 x_history []; y_history []; theta_history []; delta_history [];4.2 车辆模型函数的实现车辆模型在我的代码里就是做一步状态递推入参是当前位姿、速度、前轮转角、轴距和时间步长出参是更新后的位姿。function newPose vehicle_model(pose, v, delta, L, dt) x pose(1); y pose(2); theta pose(3); x_new x v * cos(theta) * dt; y_new y v * sin(theta) * dt; theta_new theta v * tan(delta) / L * dt; newPose [x_new; y_new; theta_new]; end这里有几个细节如果 delta 超过执行器限幅需要在传入之前就做饱和处理不然后面动画里转向角会超出车身实际能转的范围。theta_new 要不要归一化我建议每个周期都对航向角做一次角度归一化到 [-π, π]虽然欧拉积分不会因为角度超过 π 而崩但后面用 atan2 计算 α 时如果 θ 和 atan2 返回值不在同一个周期相减之后容易得到错误的 α导致转向角跳变。归一化函数function a wrapToPi(a) a mod(a pi, 2*pi) - pi; end4.3 控制器函数的实现PP 控制器的核心逻辑分三步找目标点、算 α、算 δ。function delta pure_pursuit_controller(pose, path, Ld, L, max_steer) % 1. 找目标点 idx findTargetPoint(pose, path, Ld); target path(idx, :); % 2. 计算目标点在车辆坐标系下的方位角 dx target(1) - pose(1); dy target(2) - pose(2); alpha wrapToPi(atan2(dy, dx) - pose(3)); % 3. 计算前轮转角 delta atan2(2 * L * sin(alpha), Ld); % 限幅 delta max(-max_steer, min(max_steer, delta)); end公式里的 atan2 直接把 2L sin α 和 Ld 两个量传进去等价于对表达式求反正切又能避免 α ±90° 时 atan 的分母无意义问题。实际测试中只要路径和 Ld 合理这个 delta 会给得比较平滑不会频繁打到限幅值。4.4 主循环与数据记录主循环就是正常的时间推进t 0; k 1; while t t_end % 控制器计算转角 [delta, target] pure_pursuit_controller(pose, path, Ld, L, max_steer); % 更新车辆状态 pose vehicle_model(pose, v, delta, L, dt); % 记录历史数据 x_history(k) pose(1); y_history(k) pose(2); theta_history(k) pose(3); delta_history(k) delta; step_target(k,:) target; % 时间推进 t t dt; k k 1; end记录历史数据是为了最后统一画图因为在实时画图的循环里单独绘图会比较耗时影响步长精度。我的策略是在跑主循环的时候只存数据不画图跑完之后用存的这些数据一次性生成动画这样既保证仿真时间严格步进又能得到平滑的动画。4.5 参考路径的生成圆形路径是最好验证算法收敛性的路径因为曲率恒定方便观察稳态误差。我写了一个参考路径生成函数支持圆形、八字形和自定义方程三种function path reference_path(type, param) switch type case circle R param; s linspace(0, 2*pi*R, 1000); path(:,1) R * cos(s / R); path(:,2) R * sin(s / R); case lemniscate % 八字形路径曲率变化较激烈适合测算法极限 a param; t_vals linspace(0, 2*pi, 1000); path(:,1) a * cos(t_vals) ./ (1 sin(t_vals).^2); path(:,2) a * sin(t_vals) .* cos(t_vals) ./ (1 sin(t_vals).^2); case line_curve % 直线段圆弧段模拟真实园区道路 ... end end路径点的稀疏程度会影响目标点查找的连续性。我试过如果路径点太少比如只有 100 个点目标点索引在相邻周期可能跳好几个点导致 α 突变、转角跳变。后来我把路径点加密到 1000 到 2000 个点目标点索引基本是连续移动的转角曲线平滑很多。这算是一个很实用的经验路径点间距不要大于 0.05 到 0.1 米否则 PP 的输入会有明显的高频抖动。5. 动画生成与可视化处理技巧5.1 车辆姿态的可视化方案跟踪仿真的动画效果是项目的重点之一为了让动画既直观又不至于太复杂我画了三层内容参考路径用黑色虚线画出。车辆实际轨迹用蓝色实线画出这样能直观看到跟踪误差。车辆本体用一个矩形表示车身画一条线段表示航向方向。绘制车身的核心是一个旋转矩形的函数。车身长度为 1.2 米、宽度为 0.7 米矩形初始状态是水平放置、中心在原点然后用旋转矩阵把它旋转到当前航向角再平移到车辆位置function drawVehicle(ax, pose, L, w) x pose(1); y pose(2); theta pose(3); % 车身矩形顶点以车辆中心为原点 half_L L / 2; half_w w / 2; corners [-half_L, -half_w; half_L, -half_w; half_L, half_w; -half_L, half_w]; % 旋转矩阵 R [cos(theta), -sin(theta); sin(theta), cos(theta)]; rotated R * corners; % 平移到当前位置 X rotated(1,:) x; Y rotated(2,:) y; patch(ax, X, Y, [0.8 0.3 0.2], EdgeColor, k, LineWidth, 1.5); % 画车头方向线 quiver(ax, x, y, 0.5*cos(theta), 0.5*sin(theta), k, LineWidth, 1.5); end5.2 目标点和前视圆的可视化为了让别人一眼看懂 PP 算法的工作过程我还把当前目标点和前视圆画出来。前视圆是以车辆后轴中心为中心、半径 Ld 的圆。这个圆在动画里用较浅的颜色画只显示当前时刻的不保留历史轨迹。目标点用一个圆点标记这样能清楚地看到算法“盯”着路径的哪一点。这一步看起来不起眼但演示效果提升巨大。很多人看纯文字描述理解不了 PP 的“前视”概念但只要看到那个圆和车辆前方目标点的关系三秒钟就懂了。5.3 高效生成 GIF 动画直接录制视频的话文件一般很大而且不方便放进 PPT。我比较推荐先用 VideoWriter 录制 mp4然后再导出 GIF或者直接用 exportgraphics 逐帧追加生成 GIF。生成 GIF 的代码片段filename pp_follow.gif; for k 1:100:size(x_history,2) update_animation(ax, x_history(k), y_history(k), theta_history(k), delta_history(k), ...); exportgraphics(gcf, filename, Append, true); end这里的 100 表示每隔 100 帧取一帧导出因为原始仿真步长是 0.01 秒30 秒仿真一共 3000 帧全部导出来 GIF 会太大播放也快得看不清。取 100 帧导出一个 30 帧的动画播放速度刚好。如果动画卡顿还有一个办法先把历史数据用 plot 一次性画到图形窗口再通过 set 函数更新 XData、YData 和矩形位置而不要反复执行 plot 和 patch 这类开销大的绘图命令。前者是全新的图形对象创建后者只是修改已有对象的坐标帧率能差一个量级。6. 参数调试与常见问题排查实录6.1 跟踪误差和振荡问题我做圆形路径跟踪时最初固定 Ld 1.0 米、v 2 m/s结果车辆在参考路径两侧来回“画龙”误差曲线呈明显的振荡状。排查后确认是前视距离过小车辆在朝一个目标点转的过程中因为目标点更新太频繁控制器不断“纠正”方向导致振荡。把 Ld 调大到 1.5 米后振荡明显减弱。振荡问题还有一个隐藏来源目标点索引跳变。如果参考路径点的间距不一致或者车辆速度太快导致每个控制周期内车辆前进距离大于路径点间距就会出现多个路径点被目标搜索跳过的情况。这时即使 Ld 合理转角也会突然跳变。我的解决办法是将路径点做均匀重采样保证相邻点间隔一致并且每步维护“当前目标点索引不递减”。6.2 转向角限幅触发后的表现如果你设置的 max_steer 太小比如只有 10°车辆过弯时会明显跟不上参考路径误差持续增大。我把最大转向角设成 30° 后圆形路径跟踪误差显著下降。这个问题的本质是执行器饱和控制器想给出更大的转角但被限幅挡住了车辆只能按最大曲率转弯。遇到这种问题优先检查是否是小转弯半径路径超出了车辆本身的几何通过能力。6.3 动画运行慢的处理技巧动画慢通常有两个原因一是每帧都新建绘图对象二是图形窗口的渲染没有关闭。我的做法是在循环外创建所有图形对象循环内只更新数据。关闭图形窗口的双缓冲或者用 drawnow limitrate 限制刷新率不要让每个仿真步都刷新图形。导出 GIF 时用每隔 N 帧取一帧而不是每一帧都导出。6.4 路径闭合与连续性处理圆形路径是闭合曲线第 1 个点和最后一个点实际上是同一个几何位置但车辆跑完一圈后目标点搜索还是要从头开始。如果不加保护车辆在路径连接处可能会出现目标点突然跳到起点的问题。我维护了全局进度索引目标点索引向后搜索时从当前索引位置继续向后找而不是每次从第 1 个点开始遍历这样闭合路径也能稳定跟踪。对于循环仿真我再额外加一个“圈数”计数超过设定圈数就结束循环避免无限运行。6.5 常见问题速查表这里把我在调试过程中遇到过的问题整理成表方便你对照排查现象可能原因排查与解决办法轨迹画龙、振荡前视距离过小增大 Ld或改用自适应 Ld k_pp * v Ld_min过弯切内线严重前视距离过大减小 Ld注意保持在 1~2 倍轴距附近调试转角频繁跳变目标点索引回退增加全局目标点索引进度保护路径点重采样转角始终贴上限路径曲率超出车辆通过能力检查 max_steer确认车辆最小转弯半径小于路径曲率半径航向角计算错误角度单位混用全局统一用弧度只在显示时转角度动画卡顿每帧重建图形对象改为 set 更新已有图形对象合理抽取帧数闭合路径跟踪到终点后跳变目标点搜索从头开始用“目标点索引不回退”策略解决GIF 文件过大帧数过多每隔 50~100 帧取一帧导出6.6 自适应前视距离的实际调参心得我在同一个圆形路径上对比了固定 Ld 和自适应前视距离的效果。固定 Ld 1.5 时稳态跟踪误差大约 0.08 米自适应 Ld 0.3v 1.2v 2 时 Ld 1.8稳态误差大约 0.06 米而且误差波动幅度更小。速度提高到 4 m/s 后固定 Ld 的误差能到 0.2 米以上自适应 Ld 的误差依然能压在 0.1 米左右。不过自适应前视距离并不是越大越好。k_pp 太大时车辆会丧失“修正能力”因为预瞄距离太远目标点对当前行驶方向的修正作用被稀释。我建议 k_pp 从 0.2 开始调每步加 0.05观察误差曲线的振荡程度和弯道内切量找到最平衡的点。7. 最后的实操建议这个项目做下来我的整体感受是阿克曼模型加 PP 跟踪是非常适合练手的组合难度适中效果直观而且后续扩展空间很大。你可以把路径改成任意自定义形状也可以把控制器从 PP 换成 Stanley 做对比还可以在模型里加入轮胎侧偏角来逼近实车响应。仿真动画做出来之后配合论文或者课程报告展示说服力非常强。如果你后续想往实车迁移记得把仿真里的执行器限幅和延迟单独建模出来这两个因素在仿真里如果不考虑现实部署时你大概率会撞上。另外建议把控制周期固定为 10 到 20 ms和常见底盘控制频率一致这样仿真结果和实车更接近。最后再分享一个实用小技巧在 Matlab 里调 PP 参数之前先在同一个路径和初始条件下做一组“标准实验”把误差曲线存成 mat 文件。之后每调一次参数就和标准实验做对比不要凭肉眼观察动画来判断好坏。数值化评估比肉眼观察靠谱得多这也是我在这个项目里学到的最有价值的一点。本文还有配套的精品资源点击获取 阿克曼模型Pure Pursuit路径跟踪Matlab仿真与实现
简介本资源面向机器人控制、智能车辆路径规划方向的本科生与研究生提供一套完整的阿克曼转向小车纯追踪Pure Pursuit, PP算法仿真与可视化实现方案。针对轮式机器人运动学建模与非线性路径跟踪难点资源集成了运动学建模、PP控制器设计、实时索引搜索、车辆姿态绘制及动画导出等核心功能适用于课程设计、毕业设计及算法验证场景。压缩包共9个文件7个MATLAB脚本用于模型构建、控制器实现与绘图1份Word文档详解运动学公式与参数推导1个MP4实操录屏直观展示仿真效果总大小2.95MB结构紧凑、模块职责清晰便于理解与二次开发。目前已有191人学习下载配套动画生成脚本与完整可运行主程序main.m显著降低上手门槛结合录屏演示与公式说明有效支撑从原理理解到代码复现的全流程学习。 先说明一点这个项目我在做轮式机器人路径跟踪的时候折腾过一版最近又用 Matlab 重新整理了一遍把 Pure PursuitPP算法的代码、仿真模型和动画输出整个串了起来。如果你也要做阿克曼底盘的路径跟踪仿真这篇东西应该能让你少踩几个坑。你不需要装任何复杂的外部工具只要电脑上有 Matlab 就行版本 R2020a 之后的基本都够用。阿克曼小车这个东西和差速小车最大的区别就是它的转向机构决定了车辆转向时所有车轮的瞬时转动中心是同一点因此转弯半径不是你想定多少就能定多少的必须满足阿克曼几何约束。这带来的直接后果就是各种基于差速模型的算法不能直接套得用车辆运动学模型而 Pure Pursuit 恰好是为这一类约束设计的路径跟踪算法原理简单、参数直观、工程上也够用所以我选了它来做跟踪仿真。下文就按我从建模到仿真再到动画生成的实际流程来写重点放在数学模型、代码实现和排错上。1. 项目整体设计思路与方案选型1.1 为什么选阿克曼模型做路径跟踪仿真做路径跟踪仿真前提是先有一个能描述车辆运动状态的对象。市面上常见的室内移动机器人大多是差速驱动两轮独立驱动靠两侧轮速差实现转向数学模型是一个两输入的单车模型控制起来非常直接。但我这个项目面向的其实是类似快递小车、园区巡检车这一类室外轮式机器人它们的底盘通常是阿克曼转向结构也就是前轮负责转向、后轮负责驱动或者是四轮驱动加前轮转向不管驱动形式怎么变转向几何都类似汽车。阿克曼模型的本质约束是车辆转弯时四个轮子要能绕同一个瞬时转动中心做纯滚动否则轮胎会产生明显滑移这一点和差速模型的“原地旋转”特性完全不同。所以在仿真里如果忽略了这个约束算出再漂亮的轨迹在实车上也基本跑不出来。用阿克曼模型做跟踪仿真核心价值就是让你从一开始就在“真实的转向约束”下开发和调试算法避免做出一个只能在仿真里成立的花架子。1.2 跟踪算法选型Pure Pursuit 为什么够用路径跟踪的算法有不少比如 Stanley 法、模型预测控制MPC、线性二次调节器LQR跟踪等。我为什么挑 Pure Pursuit 而不是更“高级”的算法实现成本低。PP 的核心只需要一个几何公式没有复杂的求解器写出来大概不超过 30 行代码。参数语义清晰。核心就一个前视距离相当于“你让车盯着前方多远的地方去追”物理意义非常直观便于调参。对模型误差不敏感。它本身是基于几何关系的控制不需要特别精确的车辆模型适合做算法验证和快速原型。后续换算法也方便。PP 跑通之后整个仿真框架可以直接复用把控制器换成 Stanley 或者 MPC 再对比工作量很小。当然 PP 也有短板比如它对前视距离很敏感前视太短容易振荡太长容易切弯过急的弯道性能会下降。不过对于做仿真验证和入门来说它是性价比最高的选择这也是我把它作为这个项目核心算法的原因。1.3 仿真与动画的整体架构整个项目我把它拆成四个模块车辆模型模块输入速度与转向角通过运动学方程计算下一时刻的车辆位置和航向角。路径模块生成目标路径这里我做了圆形、八字形和直线加弯道三种方便观察算法在不同曲率下的表现。PP 控制器模块根据当前位姿和目标路径求解前轮转角再输入给车辆模型。可视化模块实时绘制车辆位置、参考路径和跟踪误差同时按帧导出动画。这四个模块对应到代码上就是四个函数加上一个主脚本逻辑非常清楚也方便后面单独替换某一部分做对比实验。2. 阿克曼转向模型建模与数学推导2.1 从四轮模型到自行车模型的简化阿克曼小车完整建模可以用四轮模型但四轮模型的轮胎受力和侧偏特性很复杂用于纯几何运动学仿真时反而会引入不必要的参数。实际上很多时候我们不需要算到那个精度直接把左右前轮等效成一个位于车辆中轴线上的虚拟前轮左右后轮等效成一个虚拟后轮这就得到了经典的单车模型Bicycle Model。为什么可以这样简化因为阿克曼转向的设计目标就是保证在转向过程中所有车轮绕同一瞬时中心转动。既然绕同一个中心转那么用一个位于车辆中轴线上的等效轮就能代表左右轮的几何效果转向角取内外轮的某个等效角度即可。这样处理之后模型从四个轮子变成了“前轮-后轮”两个点计算量大幅减少精度对路径跟踪仿真也基本够用。2.2 运动学方程推导简化后的模型取后轴中心作为车辆参考点这一点很重要后面 Pure Pursuit 的公式也基于这个参考点。状态量定义为x后轴中心的横坐标y后轴中心的纵坐标θ车辆航向角车头方向与 x 轴正方向夹角v车辆速度这里默认是后轮驱动的纵向速度δ前轮等效转角在车辆坐标系下前轮到后轴中心的距离就是轴距 L。由于后轮不能侧向滑动所以后轴中心的瞬时速度方向始终沿车辆纵轴方向而前轮的速度方向则要满足前轮转向角约束。通过对几何关系求导得到$$ \dot{x} v \cos \theta $$$$ \dot{y} v \sin \theta $$$$ \dot{\theta} \frac{v \tan \delta}{L} $$这三个方程就是我在仿真里用来做状态递推的核心方程。其中前两个是质心位置的速度分解第三个则是根据“前轮转角决定转弯半径”这一几何关系推导出来的。转弯半径 R 和 δ 的关系是$$ R \frac{L}{\tan \delta} $$这个式子从直观上理解前轮转角越大tan δ 越大转弯半径越小车转得越急轴距越长转弯半径越大车越“稳”。2.3 转向几何中的阿克曼约束严格来说左右前轮的角度不是相等的。如果四轮绕同一个转弯中心内侧前轮的转角应该略大于外侧前轮这个差值就是阿克曼修正。对于普通仿真很多简化的做法是直接给定一个等效的前轮转角 δ认为左右轮的阿克曼修正已经通过转向梯形机构自动完成。但如果你希望仿真做得更细一点可以单独给左右前轮分别计算转向角$$ \delta_{\text{inner}} \arctan\left(\frac{L}{R - \frac{w}{2}}\right) $$$$ \delta_{\text{outer}} \arctan\left(\frac{L}{R \frac{w}{2}}\right) $$其中 w 是轮距。这一步我建议在代码里做成单独的阿克曼转向几何函数这样当你换成四轮可视化模型时可以直接用左右轮转角去画转向效果。不过在运动学层面使用等效 δ 就足够了。2.4 仿真中的状态递推状态方程拿到手后仿真里就是用数值积分不断更新状态。我用了最简单的欧拉法因为采样时间选得足够小0.01 秒欧拉法的精度已经足够观察控制效果没必要一开始就上龙格库塔。x(k1) x(k) v * cos(theta(k)) * dt y(k1) y(k) v * sin(theta(k)) * dt theta(k1) theta(k) v * tan(delta(k)) / L * dt这里有一个细节theta 在 Matlab 里做角度运算时要用弧度制只有在显示的时候才转成度数否则三角函数算出来全是错的。这个坑我一开始踩过后面排查发现是单位混用导致的。3. Pure Pursuit 跟踪算法的核心原理与参数计算3.1 Pure Pursuit 的几何直觉Pure Pursuit 算法可以理解为让车辆始终盯着目标路径上的某个“前方点”去追。这个目标点不是任意选的而是以车辆当前后轴中心为圆心、以前视距离 Ld 为半径画一个圆这个圆和参考路径的交点就是目标点。一旦确定了目标点控制器要做的事情就很简单计算当前车辆航向角到目标点之间夹角然后输出一个能让车辆朝那个方向转弯的前轮转角。整个控制律的直觉非常像人开车时盯着前方某个参照物开过去因此得名“纯追踪”。3.2 目标点选取的具体方法实际实现中参考路径通常是一系列离散点比如我之前设定的路径点间距是 0.1 米。目标点选取逻辑如下遍历路径点集合计算每个点与当前后轴中心的距离。找出距离第一次超过前视距离 Ld 的那个点作为目标点。如果整条路径都走完了则选择最后一个点作为目标点。写成 Matlab 代码就是function targetIdx findTargetPoint(pose, path, Ld) dists sqrt((path(:,1) - pose(1)).^2 (path(:,2) - pose(2)).^2); targetIdx find(dists Ld, 1, first); if isempty(targetIdx) targetIdx size(path, 1); end end需要注意一个问题路径如果是闭合曲线比如圆形车辆跑完一圈后当前点可能离起点很近这时如果还去找“第一次大于 Ld 的点”容易在起点附近出现跳变。我处理的办法是维护一个全局进度计数器目标点索引只能往前移动不允许回退这样就能避免目标点在路径上左右横跳。3.3 转向角公式的推导过程目标和当前车辆位置确定后设目标点在车辆坐标系下的方位角为 α利用正弦定理可以推导出前轮转角。具体推导如下车辆后轴中心记为 O目标点记为 PO 到 P 的距离为 Ld。车辆的瞬时转弯半径为 R车辆转弯时后轴中心轨迹是一个半径为 R 的圆弧。由于后轴中心到瞬时转动中心记为 C的距离是 R所以三角形 OCP 中OC 等于 ROP 等于 Ld角 COP 为 α目标点方向与当前航向的夹角角 OPC 为 90° - α 或是更精确的几何关系利用正弦定理最终得到曲率 κ 和 α 的关系。工程上常用的结果形式为$$ \kappa \frac{2 \sin \alpha}{L_d} $$转弯半径 R 与曲率互为倒数而轮胎转角与转弯半径的关系是 R L / tan δ所以联立得到$$ \delta \arctan \frac{2 L \sin \alpha}{L_d} $$这里有一个特别容易踩坑的点α 的含义。它通常是目标点方向相对车辆航向方向的夹角而不是目标点在世界坐标系下的绝对角度。因此代码里要用 atan2 求出目标点相对车辆的方位角再减去航向角 θ再做角度归一化到 [-π, π]。3.4 前视距离 Ld 的选择与自适应整个 PP 算法里最影响效果的就是 Ld。Ld 取小了车辆会频繁修正方向轨迹跟踪精度看着高但容易出现振荡Ld 取大了车辆转向很柔和但过弯时会明显切内线跟踪误差变大。我调试时用到的经验值轴距 L 1 米速度 v 2 m/s固定 Ld 1.5 米在圆形路径上效果还可以跟踪误差基本在 0.1 米以内。但把速度提到 4 m/s 之后固定 Ld 就会出现明显的“甩尾感”实际轨迹比参考路径滞后很多。更好的做法是让 Ld 和速度挂钩$$ L_d k_{pp} v L_{d,\min} $$我常用 k_pp 0.3Ld_min 1.2 米。这样车速越快预瞄得越远系统不至于因为响应延迟而失控。调节 k_pp 时一般从小往大试小 k_pp 会让跟踪更激进大 k_pp 会让跟踪更保守。我的习惯是先固定速度观察圆形路径跟踪误差曲线如果误差曲线振荡明显就加大 Ld如果弯道内侧切得太明显就减小 Ld。4. Matlab 仿真主程序实现与代码解析4.1 主程序框架设计我最终的代码结构是这样的ackermann_pp_sim/ ├── main_pp_sim.m ├── vehicle_model.m ├── pure_pursuit_controller.m ├── reference_path.m ├── update_animation.m └── export_animation_gif.m主程序只负责初始化参数、循环仿真、保存数据、调用可视化函数。这样模块很干净想换路径生成器或者换控制器都不需要动主循环。主程序里关键的初始化部分我贴出来大家可以直接参考%% 参数初始化 L 1.0; % 轴距单位米 w 0.7; % 轮距单位米主要用于画图和四轮几何 v 2.0; % 车辆速度单位 m/s dt 0.01; % 仿真步长单位 s t_end 30; % 仿真总时长单位 s Ld 1.5; % 固定前视距离单位 m % 如果使用自适应前视距离则 k_pp 0.3; % 前视距离速度系数 Ld_min 1.2; % 最小前视距离 max_steer deg2rad(30); % 最大转向角单位弧度 %% 生成参考路径 path reference_path(circle, 5.0); % 半径5米的圆形路径 %% 初始状态 x0 path(1,1) 0.5; % 故意放一点初始偏差方便观察收敛过程 y0 path(1,2); theta0 deg2rad(0); pose [x0; y0; theta0]; %% 存储数据 x_history []; y_history []; theta_history []; delta_history [];4.2 车辆模型函数的实现车辆模型在我的代码里就是做一步状态递推入参是当前位姿、速度、前轮转角、轴距和时间步长出参是更新后的位姿。function newPose vehicle_model(pose, v, delta, L, dt) x pose(1); y pose(2); theta pose(3); x_new x v * cos(theta) * dt; y_new y v * sin(theta) * dt; theta_new theta v * tan(delta) / L * dt; newPose [x_new; y_new; theta_new]; end这里有几个细节如果 delta 超过执行器限幅需要在传入之前就做饱和处理不然后面动画里转向角会超出车身实际能转的范围。theta_new 要不要归一化我建议每个周期都对航向角做一次角度归一化到 [-π, π]虽然欧拉积分不会因为角度超过 π 而崩但后面用 atan2 计算 α 时如果 θ 和 atan2 返回值不在同一个周期相减之后容易得到错误的 α导致转向角跳变。归一化函数function a wrapToPi(a) a mod(a pi, 2*pi) - pi; end4.3 控制器函数的实现PP 控制器的核心逻辑分三步找目标点、算 α、算 δ。function delta pure_pursuit_controller(pose, path, Ld, L, max_steer) % 1. 找目标点 idx findTargetPoint(pose, path, Ld); target path(idx, :); % 2. 计算目标点在车辆坐标系下的方位角 dx target(1) - pose(1); dy target(2) - pose(2); alpha wrapToPi(atan2(dy, dx) - pose(3)); % 3. 计算前轮转角 delta atan2(2 * L * sin(alpha), Ld); % 限幅 delta max(-max_steer, min(max_steer, delta)); end公式里的 atan2 直接把 2L sin α 和 Ld 两个量传进去等价于对表达式求反正切又能避免 α ±90° 时 atan 的分母无意义问题。实际测试中只要路径和 Ld 合理这个 delta 会给得比较平滑不会频繁打到限幅值。4.4 主循环与数据记录主循环就是正常的时间推进t 0; k 1; while t t_end % 控制器计算转角 [delta, target] pure_pursuit_controller(pose, path, Ld, L, max_steer); % 更新车辆状态 pose vehicle_model(pose, v, delta, L, dt); % 记录历史数据 x_history(k) pose(1); y_history(k) pose(2); theta_history(k) pose(3); delta_history(k) delta; step_target(k,:) target; % 时间推进 t t dt; k k 1; end记录历史数据是为了最后统一画图因为在实时画图的循环里单独绘图会比较耗时影响步长精度。我的策略是在跑主循环的时候只存数据不画图跑完之后用存的这些数据一次性生成动画这样既保证仿真时间严格步进又能得到平滑的动画。4.5 参考路径的生成圆形路径是最好验证算法收敛性的路径因为曲率恒定方便观察稳态误差。我写了一个参考路径生成函数支持圆形、八字形和自定义方程三种function path reference_path(type, param) switch type case circle R param; s linspace(0, 2*pi*R, 1000); path(:,1) R * cos(s / R); path(:,2) R * sin(s / R); case lemniscate % 八字形路径曲率变化较激烈适合测算法极限 a param; t_vals linspace(0, 2*pi, 1000); path(:,1) a * cos(t_vals) ./ (1 sin(t_vals).^2); path(:,2) a * sin(t_vals) .* cos(t_vals) ./ (1 sin(t_vals).^2); case line_curve % 直线段圆弧段模拟真实园区道路 ... end end路径点的稀疏程度会影响目标点查找的连续性。我试过如果路径点太少比如只有 100 个点目标点索引在相邻周期可能跳好几个点导致 α 突变、转角跳变。后来我把路径点加密到 1000 到 2000 个点目标点索引基本是连续移动的转角曲线平滑很多。这算是一个很实用的经验路径点间距不要大于 0.05 到 0.1 米否则 PP 的输入会有明显的高频抖动。5. 动画生成与可视化处理技巧5.1 车辆姿态的可视化方案跟踪仿真的动画效果是项目的重点之一为了让动画既直观又不至于太复杂我画了三层内容参考路径用黑色虚线画出。车辆实际轨迹用蓝色实线画出这样能直观看到跟踪误差。车辆本体用一个矩形表示车身画一条线段表示航向方向。绘制车身的核心是一个旋转矩形的函数。车身长度为 1.2 米、宽度为 0.7 米矩形初始状态是水平放置、中心在原点然后用旋转矩阵把它旋转到当前航向角再平移到车辆位置function drawVehicle(ax, pose, L, w) x pose(1); y pose(2); theta pose(3); % 车身矩形顶点以车辆中心为原点 half_L L / 2; half_w w / 2; corners [-half_L, -half_w; half_L, -half_w; half_L, half_w; -half_L, half_w]; % 旋转矩阵 R [cos(theta), -sin(theta); sin(theta), cos(theta)]; rotated R * corners; % 平移到当前位置 X rotated(1,:) x; Y rotated(2,:) y; patch(ax, X, Y, [0.8 0.3 0.2], EdgeColor, k, LineWidth, 1.5); % 画车头方向线 quiver(ax, x, y, 0.5*cos(theta), 0.5*sin(theta), k, LineWidth, 1.5); end5.2 目标点和前视圆的可视化为了让别人一眼看懂 PP 算法的工作过程我还把当前目标点和前视圆画出来。前视圆是以车辆后轴中心为中心、半径 Ld 的圆。这个圆在动画里用较浅的颜色画只显示当前时刻的不保留历史轨迹。目标点用一个圆点标记这样能清楚地看到算法“盯”着路径的哪一点。这一步看起来不起眼但演示效果提升巨大。很多人看纯文字描述理解不了 PP 的“前视”概念但只要看到那个圆和车辆前方目标点的关系三秒钟就懂了。5.3 高效生成 GIF 动画直接录制视频的话文件一般很大而且不方便放进 PPT。我比较推荐先用 VideoWriter 录制 mp4然后再导出 GIF或者直接用 exportgraphics 逐帧追加生成 GIF。生成 GIF 的代码片段filename pp_follow.gif; for k 1:100:size(x_history,2) update_animation(ax, x_history(k), y_history(k), theta_history(k), delta_history(k), ...); exportgraphics(gcf, filename, Append, true); end这里的 100 表示每隔 100 帧取一帧导出因为原始仿真步长是 0.01 秒30 秒仿真一共 3000 帧全部导出来 GIF 会太大播放也快得看不清。取 100 帧导出一个 30 帧的动画播放速度刚好。如果动画卡顿还有一个办法先把历史数据用 plot 一次性画到图形窗口再通过 set 函数更新 XData、YData 和矩形位置而不要反复执行 plot 和 patch 这类开销大的绘图命令。前者是全新的图形对象创建后者只是修改已有对象的坐标帧率能差一个量级。6. 参数调试与常见问题排查实录6.1 跟踪误差和振荡问题我做圆形路径跟踪时最初固定 Ld 1.0 米、v 2 m/s结果车辆在参考路径两侧来回“画龙”误差曲线呈明显的振荡状。排查后确认是前视距离过小车辆在朝一个目标点转的过程中因为目标点更新太频繁控制器不断“纠正”方向导致振荡。把 Ld 调大到 1.5 米后振荡明显减弱。振荡问题还有一个隐藏来源目标点索引跳变。如果参考路径点的间距不一致或者车辆速度太快导致每个控制周期内车辆前进距离大于路径点间距就会出现多个路径点被目标搜索跳过的情况。这时即使 Ld 合理转角也会突然跳变。我的解决办法是将路径点做均匀重采样保证相邻点间隔一致并且每步维护“当前目标点索引不递减”。6.2 转向角限幅触发后的表现如果你设置的 max_steer 太小比如只有 10°车辆过弯时会明显跟不上参考路径误差持续增大。我把最大转向角设成 30° 后圆形路径跟踪误差显著下降。这个问题的本质是执行器饱和控制器想给出更大的转角但被限幅挡住了车辆只能按最大曲率转弯。遇到这种问题优先检查是否是小转弯半径路径超出了车辆本身的几何通过能力。6.3 动画运行慢的处理技巧动画慢通常有两个原因一是每帧都新建绘图对象二是图形窗口的渲染没有关闭。我的做法是在循环外创建所有图形对象循环内只更新数据。关闭图形窗口的双缓冲或者用 drawnow limitrate 限制刷新率不要让每个仿真步都刷新图形。导出 GIF 时用每隔 N 帧取一帧而不是每一帧都导出。6.4 路径闭合与连续性处理圆形路径是闭合曲线第 1 个点和最后一个点实际上是同一个几何位置但车辆跑完一圈后目标点搜索还是要从头开始。如果不加保护车辆在路径连接处可能会出现目标点突然跳到起点的问题。我维护了全局进度索引目标点索引向后搜索时从当前索引位置继续向后找而不是每次从第 1 个点开始遍历这样闭合路径也能稳定跟踪。对于循环仿真我再额外加一个“圈数”计数超过设定圈数就结束循环避免无限运行。6.5 常见问题速查表这里把我在调试过程中遇到过的问题整理成表方便你对照排查现象可能原因排查与解决办法轨迹画龙、振荡前视距离过小增大 Ld或改用自适应 Ld k_pp * v Ld_min过弯切内线严重前视距离过大减小 Ld注意保持在 1~2 倍轴距附近调试转角频繁跳变目标点索引回退增加全局目标点索引进度保护路径点重采样转角始终贴上限路径曲率超出车辆通过能力检查 max_steer确认车辆最小转弯半径小于路径曲率半径航向角计算错误角度单位混用全局统一用弧度只在显示时转角度动画卡顿每帧重建图形对象改为 set 更新已有图形对象合理抽取帧数闭合路径跟踪到终点后跳变目标点搜索从头开始用“目标点索引不回退”策略解决GIF 文件过大帧数过多每隔 50~100 帧取一帧导出6.6 自适应前视距离的实际调参心得我在同一个圆形路径上对比了固定 Ld 和自适应前视距离的效果。固定 Ld 1.5 时稳态跟踪误差大约 0.08 米自适应 Ld 0.3v 1.2v 2 时 Ld 1.8稳态误差大约 0.06 米而且误差波动幅度更小。速度提高到 4 m/s 后固定 Ld 的误差能到 0.2 米以上自适应 Ld 的误差依然能压在 0.1 米左右。不过自适应前视距离并不是越大越好。k_pp 太大时车辆会丧失“修正能力”因为预瞄距离太远目标点对当前行驶方向的修正作用被稀释。我建议 k_pp 从 0.2 开始调每步加 0.05观察误差曲线的振荡程度和弯道内切量找到最平衡的点。7. 最后的实操建议这个项目做下来我的整体感受是阿克曼模型加 PP 跟踪是非常适合练手的组合难度适中效果直观而且后续扩展空间很大。你可以把路径改成任意自定义形状也可以把控制器从 PP 换成 Stanley 做对比还可以在模型里加入轮胎侧偏角来逼近实车响应。仿真动画做出来之后配合论文或者课程报告展示说服力非常强。如果你后续想往实车迁移记得把仿真里的执行器限幅和延迟单独建模出来这两个因素在仿真里如果不考虑现实部署时你大概率会撞上。另外建议把控制周期固定为 10 到 20 ms和常见底盘控制频率一致这样仿真结果和实车更接近。最后再分享一个实用小技巧在 Matlab 里调 PP 参数之前先在同一个路径和初始条件下做一组“标准实验”把误差曲线存成 mat 文件。之后每调一次参数就和标准实验做对比不要凭肉眼观察动画来判断好坏。数值化评估比肉眼观察靠谱得多这也是我在这个项目里学到的最有价值的一点。本文还有配套的精品资源点击获取