简介本资源是一套面向自动驾驶路径与轨迹跟踪领域的高分毕设级C工程适用于计算机、人工智能、自动化、车辆工程等专业学生及初学者开展算法实践与课程设计。项目融合Pure Pursuit路径跟踪与LQR轨迹跟踪双策略配套改进型A*路由规划含路沿启发函数与均值滤波平滑、ROS节点实现、Spline插值优化及大量测试脚本与文档说明解决从全局路径生成到局部运动控制的完整闭环问题。压缩包共792个文件含169个cpp源码、102个头文件、88张可视化PNG图、39份Markdown说明及11份PDF技术文档总大小45.17MB结构清晰覆盖规划、控制、仿真与调试全流程。项目已通过实际运行验证答辩平均分96分附带完整README指引、launch启动脚本、rviz可视化配置及多种路径生成工具如quinticpath、splinepathnew等可直接部署学习或作为毕设/课设基础框架二次开发。 我去年接手了一门机器人学课程的大作业要求用C实现路径跟踪与轨迹跟踪并附上源代码、文档和测试脚本。我选了Pure-pursuit和LQR两个经典算法来做最后拿到的评分相当不错。最近不少学弟学妹问我这个项目怎么搭、算法怎么调参、代码结构怎么组织才能拿高分所以我把整个从零到一的过程整理出来包括算法原理、工程实现、调试踩坑和文档脚本的写作思路希望能帮到正在做类似项目的朋友。先说清楚一点这个项目名字里有“路径跟踪”和“轨迹跟踪”两个概念很多人容易混为一谈但它们其实是不同的。路径跟踪只关心空间上的几何路径不考虑时间节奏目标是把车稳定地压在目标路径上轨迹跟踪则多了一个时间维度要求车辆在某个时刻到达某个位置对速度和加速度都有约束。Pure-pursuit是典型的路径跟踪算法LQR则非常适合做轨迹跟踪。两个都做进去既能体现技术广度也方便从对比中展示对问题的理解深度这是高分项目的一个关键加分点。1. 为什么是Pure-pursuit和LQR的组合问题拆解1.1 路径跟踪与轨迹跟踪的本质区别我在一开始设计项目时最核心的一个决定就是必须先想清楚“跟踪”到底要解决什么问题。假设有一条参考路径比如一个圆弧或者一条S形曲线路径跟踪的任务是让车辆从一个任意初始位置出发逐步靠近这条路并沿着它行驶。这里的输入是路径的几何信息输出是车辆的前轮转角控制目标是最小化车辆当前位置到路径的横向距离误差。也就是说车快一点、慢一点往往不是最关键的别跑出车道才是关键。轨迹跟踪则复杂一些。轨迹 路径 时间属性每一个轨迹点都带有目标时刻、目标位置、目标速度甚至目标加速度。控制器不仅要把车引导到目标位置还要确保在特定时间点到达并且速度跟随预期曲线。这在机械臂、无人机编队、自动驾驶换道场景里非常常见。比如无人机穿越一串航点每个航点都规定了到达时间和速度只跟踪位置没有用必须要同时控制速度。所以说Pure-pursuit解决的是“空间收敛”问题LQR解决的是“时空跟踪”问题。把两者放在同一个项目里能够覆盖从几何路径规划到动态轨迹跟踪的完整链条。这也是为什么我在项目文档的第一章就花了两页来解释这个区别——答辩时老师一定会问你这两个算法分别解决什么问题为什么需要两个而不是一个。1.2 两个算法在项目中的角色分工在这个项目中我的分工方式是Pure-pursuit负责处理全局路径跟踪输入是一串路径点输出是前轮转角控制量LQR负责处理带时间属性的轨迹跟踪输入是一串带速度约束的轨迹点输出是转角和加速度控制量。两个算法共用同一个车辆运动学模型和同一个仿真环境方便在实验对比中精确控制变量。在具体实现时路径和轨迹都从文件里读取。路径文件只有x和y坐标轨迹文件则在每个点上有x、y、速度和时间戳。仿真器每次更新车辆状态时根据用户选择的算法模式调用对应的控制器计算出控制量再通过车辆模型更新位置和航向角。这种设计让测试非常灵活同一个车辆模型跑两套控制器数据就可以直接对比。这里有一个值得注意的设计思路控制器和车辆模型一定要解耦。控制器只管根据当前状态和目标输出控制量车辆模型只管根据控制量更新状态两者通过接口对接。这样后面不管是换车辆模型比如从自行车模型换成阿克曼模型还是换控制器比如再加一个MPC代码都不用大改。2. Pure-pursuit路径跟踪从几何直觉到代码实现2.1 自行车模型与预瞄点的几何关系Pure-pursuit的核心思想非常直观可以用一句话概括像人走路一样盯着前方一个目标点预瞄点走过去。这个目标点在参考路径上距离车辆当前位置有一个前视距离Ld。控制器做的事情就是计算出车辆当前位置到预瞄点的连线与车身航向之间的夹角然后根据这个夹角计算出前轮转角。推导过程要用到自行车模型bicycle model。把车辆简化为前后两个轮子前轮控制转向后轮固定车辆的运动可以看作一个刚体绕某个瞬时旋转中心转动。假设车辆轴距为L前轮转角为δ则转弯半径R满足R L / tan(δ)这个关系是所有几何类路径跟踪算法的基础。Pure-pursuit在此基础上引入一个几何关系以车辆当前位置为圆心、前视距离Ld为半径画圆这个圆与参考路径的交点就是预瞄点。连接车辆位置和预瞄点这条弦与车辆航向之间的夹角为α则前轮转角为δ arctan(2 * L * sin(α) / Ld)这个公式的推导在《Introduction to Autonomous Mobile Robots》里有详细说明项目文档里我也给了完整的几何图解。简单理解就是α越大说明预瞄点偏离航向越多需要的转角越大Ld越大说明看得越远转向就越平缓。这正好符合驾驶直觉——高速时看远一点转向柔和低速时看近一点转向灵活。2.2 前视距离Ld的标定与自适应前视距离是Pure-pursuit唯一的控制参数也是整个算法调参的核心。如果Ld太小车辆会不断“矫正过正”产生来回震荡路径跟踪效果很差如果Ld太大车辆反应迟钝在急弯处会明显切弯形成较大的静态误差。我在实验中发现Ld取车辆轴距的1到2倍时低速圆形路径的跟踪效果最好但在S形路径上会出现振荡Ld取轴距的3倍以上时振荡消失了但弯道处的横向误差明显增大。实际工程中几乎不会用固定的Ld而是让Ld随车速变化。常见做法是Ld k * v Ld_min其中v是当前车速k是前视系数Ld_min是最小前视距离。k取0.1到0.3之间Ld_min取1到2米根据车辆尺寸。项目脚本里我做了一个参数扫描把k从0.05扫到0.5观察不同取值下的最大横向误差和控制量变化结论是k在0.15到0.25这个区间内圆形路径和S形路径的综合表现都比较好。这里补充一个容易被忽略的细节搜索预瞄点时不能只搜距离最近的路径点而是要从当前路径索引往后搜索找到第一个与车辆位置距离大于等于Ld的点。如果只找最近点车辆会“往回看”导致跟踪方向错误。2.3 代码结构设计与边界条件处理Pure-pursuit控制器的核心代码大约80行分类放在PurePursuitController类里。关键函数有三个SetLookaheadDistance()设置前视距离支持固定值和自适应值CalculateAngle()根据当前车辆状态和参考路径计算前轮转角FindTargetPoint()在路径上搜索预瞄点返回其索引和坐标FindTargetPoint是最容易出bug的地方尤其是处理循环路径和终点附近的索引越界问题。我的做法是传入一个起始搜索索引从当前位置向后搜索如果搜索到路径末尾还没找到预瞄点就取最后一个路径点作为目标。当车辆距离终点小于某个阈值时控制器输出零转角让车直线滑行到终点。关于转向角的正负号这里也值得单独说一下。不同坐标系定义下角度的正负可能完全相反。我在项目里统一用“左转为正”的约定并把所有角度运算都放在同一个坐标系下完成避免混用。编译时开启-Wall警告很多单位问题都能提前暴露。double PurePursuitController::CalculateAngle(const VehicleState state, const std::vectorPathPoint path, int* target_index) { auto target FindTargetPoint(state, path, target_index); double alpha NormalizeAngle(atan2(target.y - state.y, target.x - state.x) - state.yaw); double Ld std::hypot(target.x - state.x, target.y - state.y); if (Ld 1e-6) return 0.0; return atan2(2.0 * wheelbase_ * sin(alpha), Ld); }这个实现里用了hypot而不是自己写sqrt(x^2 y^2)是为了避免大数平方溢出。NormalizeAngle把角度归一化到[-π, π]这是角度运算里的基本功少了它转向角会在π附近跳变导致控制量突变。3. LQR轨迹跟踪从状态空间模型到最优控制量3.1 误差模型的建立为什么需要线性化LQRLinear Quadratic Regulator和Pure-pursuit走的是完全不同的路线Pure-pursuit从几何直觉出发LQR从优化理论出发。LQR的核心逻辑是把轨迹跟踪问题转化为一个状态调节问题设计状态反馈控制率使得一个二次型代价函数最小。这个转化的关键是建立车辆的误差模型。首先定义误差状态——在我们这个项目里取的是横向误差e_y和航向误差e_ψ状态向量为x [e_y, e_ψ, e_y_dot, e_ψ_dot]。车辆模型是非线性的包含sin、cos、速度乘积等项LQR要求系统是线性的所以必须做线性化处理。线性化有两种思路。一种是在稳态工作点附近做泰勒展开得到状态转移矩阵A和控制矩阵B另一种是直接基于误差变化率来建立线性微分方程。我们采用的是第二种因为轨迹跟踪误差模型本身就有明显的线性结构e_y_dot v * e_ψ e_ψ_dot v / L * δ也就是说横向误差的变化率近似等于速度乘以航向误差航向误差的变化率近似等于前轮转角除以轴距再乘以速度。这两个线性方程构成了线性化误差模型的基础不需要复杂的雅可比矩阵推导。当然这样做有一个前提假设误差比较小车辆运行在线性区间附近。如果初始误差很大LQR的表现就会受限此时可以考虑先用Pure-pursuit粗跟踪收敛后再切换到LQR精细跟踪这也是很多实车系统的做法。3.2 离散化处理从连续LQR到可计算的DLQR连续时间的LQR最优控制率是u -K * x其中K R^(-1) * B^T * PP通过求解代数黎卡提方程ARE得到。但在C代码里我们处理的是离散时间系统需要对连续模型做离散化。离散化方法我选的是零阶保持ZOH步长dt取0.05秒和仿真周期保持一致。连续状态空间模型为x_dot A * x B * u离散化后为x(k1) Ad * x(k) Bd * u(k)Ad和Bd可以由矩阵指数计算。在实际代码里我用的是Eigen库的矩阵运算先把A和B构造出来再用离散化求解器得到Ad和Bd。这里有一个比较隐蔽的坑离散化步长必须和仿真步长一致否则控制器的计算频率和模型更新频率不匹配LQR的性能会严重退化。得到离散模型后求解离散代数黎卡提方程DAREP Q Ad^T * P * Ad - Ad^T * P * Bd * (R Bd^T * P * Bd)^(-1) * Bd^T * P * Ad这个方程在C里没有现成的高质量库函数可以直接调用除非引入第三方库所以我在项目里实现了一个迭代法求解器从P_0 Q开始反复迭代上面的方程直到P的增量小于阈值。实验下来一般迭代20到50次就能收敛耗时不到1毫秒。3.3 Q矩阵和R矩阵的工程调参经验Q和R的选取是LQR调参的核心中的核心。Q矩阵的每一项对应一个状态变量的权重R对应控制量的权重。直观理解Q越大控制器越“较真”于误差的消除收敛更快但控制量可能很大R越大控制器越“惜力”控制量更平滑但误差收敛更慢。在这个项目里Q取的是diag([10.0, 1.0, 0.5, 0.5])R取的是[2.0]。横向误差e_y的权重最大因为轨迹跟踪最直观的指标就是横向误差航向误差权重次之两个导数项的权重较小主要起阻尼作用防止系统振荡。实际观察到的现象是Q中e_y的权重从10提高到50时圆形轨迹的最大横向误差从0.12米降到0.05米但转角控制量的标准差从0.02弧度增大到0.06弧度控制变得更“激烈”。R从2.0降到0.5时控制量的峰值明显增大但横向误差基本没有改善说明R取太小会浪费控制能量。R取2.0到5.0之间是一个比较合理的区间。这里还必须强调一个我在调试过程中踩过的坑Q和R的量纲必须匹配。如果横向误差的单位是米控制量的单位是弧度那么Q的横向误差项的量纲是1/m²R的量纲是1/rad²。如果单位不对两者的数量级差太多调参时很难找到合理的平衡点。我的做法是在代码里用物理单位清晰标注每个变量的量纲文档里也给出了单位说明表。4. 仿真环境与车辆模型让算法在跑起来之前先“活起来”4.1 车辆运动学模型的C实现与单位统一项目使用了一个二维平面内的车辆运动学模型状态量为x坐标、y坐标、航向角yaw控制量为前轮转角delta和速度v。状态更新公式为x_new x v * cos(yaw) * dt y_new y v * sin(yaw) * dt yaw_new yaw v * tan(delta) / L * dt这个模型适用于低速场景下的路径跟踪仿真。在代码实现里VehicleModel类封装了Reset()和Step()两个方法前者初始化状态后者根据控制量更新状态。所有角度都使用弧度制长度单位统一为米速度单位统一为米每秒。说到单位统一这里有一个非常惨痛的教训。我第一次写仿真器时把速度单位写成了km/h但路径坐标用的是米结果跟踪结果完全对不上车辆在路径附近来回打转看起来像是算法完全失效。排查了半天最后发现就是单位问题。所以我的建议是在头文件或者文档的显著位置明确标注所有物理量的单位并且在整个项目里强制统一。4.2 目标路径与目标轨迹的生成脚本为了测试算法我写了一个Python脚本生成三种参考路径圆形路径、S形路径和U形路径。每种路径都以固定间距采样生成一系列路径点保存为CSV文件。圆形路径的生成最简单半径取10米圆心在原点从0到2π均匀采样。S形路径用正弦函数生成x方向均匀递增y方向按正弦变化目的是测试算法在连续曲率变化下的表现。U形路径模拟的是倒车入库或者狭窄弯道曲率在转弯段突然增大对Pure-pursuit的前视距离特别敏感。轨迹文件的生成则复杂一些因为要带时间信息。我的做法是先生成路径再根据预设的速度曲线给每个点分配到达时间。简单做法是匀速假设每个路径点间隔固定时间进阶做法是梯形速度曲线起步、匀速、减速三个阶段的时间戳不同这样能更真实地模拟实际行驶场景。脚本统一放在scripts/目录下用argparse接收输出路径和路径类型运行方式非常简单python3 scripts/generate_path.py --type circle --radius 10 --output data/circle_path.csv python3 scripts/generate_trajectory.py --type s_curve --speed 2.0 --output data/s_trajectory.csv这样做的好处是算法代码完全不依赖路径的生成方式只需要从CSV文件里读取数据大大提高了模块化程度。4.3 仿真循环的实现与数据记录仿真器是整个项目的中枢我把它设计成一个Simulator类内部持有VehicleModel和两个控制器PurePursuitController和LQRController以及当前激活的控制器指针。主循环如下根据当前时间从轨迹中插值得到参考状态调用激活的控制器输出控制量将控制量输入VehicleModel更新车辆状态记录每个时刻的状态误差数据到日志文件仿真步长dt取0.05秒即20Hz的控制频率这和LQR的离散化步长保持一致。整个流程用命令行参数控制支持切换算法、加载不同路径、调整参数。运行完以后日志文件会记录每一时刻的跟踪误差、控制量、车辆状态方便用Python脚本进行可视化分析。5. 测试结果与对比分析数据怎么说5.1 误差指标的定义与计算跟踪效果不能只靠肉眼观察必须用定量的误差指标来衡量。我在项目中定义了四个核心指标平均绝对误差MAE所有时刻横向误差绝对值的平均值均方根误差RMSE横向误差平方和的均值的平方根对大误差更敏感最大横向误差Max lateral error整个过程中最大的横向偏差控制量变化率单位时间内转角控制量的变化量反映控制平滑度这些指标的计算都在Python的analysis.py脚本里完成读入日志后直接输出汇总表格。在项目文档中我给出了两种算法在三种路径下的指标对比表并且讨论了每个指标的物理意义。5.2 不同路径下的算法表现差异在半径10米的圆形路径上车辆以1.5米/秒的速度行驶时调速后的Pure-pursuit的最大横向误差约0.08米LQR的最大横向误差约0.04米两者差别不大LQR稍优。但在S形路径上差距变得明显——Pure-pursuit的横向误差在转弯点有周期性波动最大达到0.15米而LQR的最大误差稳定在0.06米以内。这说明Pure-pursuit本质上是“参考点跟踪”而非“路径跟踪”它只盯着预瞄点对路径的曲率变化没有显式的建模能力。而LQR通过状态反馈对航向误差和横向误差同时做闭环控制能够更快地响应曲率变化。在U形路径上Pure-pursuit的问题暴露得更明显。由于急转弯处曲率大固定前视距离会导致车辆“切弯”最大横向误差达到0.3米而LQR即便在这种路径下也能把误差控制在0.1米以内。5.3 速度变化对跟踪性能的影响我还做了一组变速实验把速度从1.0米/秒逐步提升到3.0米/秒观察两种算法在圆形路径上的表现。结果是Pure-pursuit在2.0米/秒以下表现良好速度超过2.5米/秒后由于前视距离的自适应调整跟不上横向误差开始显著上升LQR在3.0米/秒时依然能保持0.1米以内的误差整个过程的控制量曲线也相对平滑。总的来看这个项目用数据证明了“路径跟踪”和“轨迹跟踪”在算法选型上的差异化需求如果只是低速环境下的路径跟踪Pure-pursuit完全够用而且实现简单如果对时间约束和高速工况有要求LQR或更复杂的最优控制算法是更好的选择。这个结论直接写在了项目文档的结论章节答辩时老师对此给予了正面评价。6. 调试排错记录那些让我崩溃又成长的问题6.1 预瞄点搜索越界与跳变问题Pure-pursuit在路径终点附近的预瞄点搜索经常会越界导致程序崩溃。我最初的处理是搜索到路径末尾就取最后一个点但这样会出现一个现象车辆在接近终点时目标点突然回退导致转向角突变。排查后发现问题出在“从当前路径索引往后搜”的实现里索引已经越过终点却又被判断条件拉回去了。解决方案是在找到目标点后不仅记录目标点的坐标还记录其索引。下一帧搜索时从该索引开始而不是从0开始并且如果索引已经是最后一个点就直接用当前点作为目标不让索引回退。这一改终点附近的振荡问题立刻消失。6.2 LQR求解Riccati方程时的数值不稳定LQR的Riccati方程迭代法在Q矩阵元素差距过大时会出现数值不稳定的情况。最典型的表现是迭代次数超过100次仍未收敛或者P矩阵出现负特征值导致控制率K计算异常。我的解决办法是两招并用第一在迭代前检查系统可控性如果可控性矩阵的条件数过大直接报错提示调参第二给P矩阵的不对角元素加一个正则化项避免P矩阵趋于奇异。实际调试时这个正则化系数取1e-10就足够了既不影响收敛精度又能稳定数值行为。另一个非常有用的调试技巧是把同一组参数在Matlab的dlqr函数里算一遍当作对照基准。Matlab的结果和C迭代法的结果对比如果差距超过1e-6说明迭代法没有完全收敛需要提高迭代次数阈值。这个对照方法帮我发现了一个隐蔽的bug——我当时忘记了P矩阵需要对称化导致迭代误差累积最终求出来的K不符合最优解。6.3 控制量突变与仿真崩溃的处理仿真过程中偶尔会出现控制量突然变大车辆直接飞出去的情况。一开始我以为是算法问题后来发现原因居然是对输入轨迹的插值处理不当当车辆与大目标点距离很近时速度方向与目标方向不一致导致角度差在π附近跳变控制量被放大。解决方案是在所有角度差计算后都加一个NormalizeAngle调用确保差值始终在[-π, π]内。这个函数我是在工具模块utils.h里统一写的所有控制器都调用同一个函数。之后无论是Pure-pursuit还是LQR控制量突变的概率都大幅降低。7. 文档与脚本组织高分项目的最后一块拼图7.1 README与设计文档的写作思路一个“高分项目”不仅仅看代码能不能跑更看你的表达能力与工程素养文档是核心载体。我的项目文档分四部分项目概览一句话描述项目目标列出技术栈说明算法选型理由算法原理分别介绍Pure-pursuit和LQR的数学推导过程配几何示意图和公式代码结构与使用说明说明文件目录、如何编译、如何运行仿真、如何分析结果实验结果与分析展示测试指标表格、典型场景的对比图并给出结论写文档时我给自己的要求是每个公式都必须有物理意义的解释绝不堆砌推导过程。比如LQR的Riccati方程我会写清楚每一项的作用P矩阵的初值怎么取不收敛怎么处理这样读文档的人才能真正理解——而不是照抄教材。7.2 自动化脚本与一键复现流程脚本让项目具备可复现性。我把项目中用到的所有脚本做了分类数据生成脚本、仿真运行脚本、结果分析脚本和参数扫描脚本。每个脚本都有标准化的命令行参数支持通过一个shell脚本批量调用。一键复现的流程是这样设计的先运行build_and_run.sh编译项目再运行run_experiments.sh跑完所有预设场景最后运行analyze.sh生成对比图表。整个流程不需要人工干预跑完以后所有结果都汇总在results/目录下。这种方式在答辩演示时非常加分老师可以直接看到完整的从编译到出结果的工程链路。7.3 答辩前检查清单最后分享一个我答辩前整理的项目检查清单照着做一遍能有效减少翻车概率代码能否在干净环境中从零编译成功不依赖IDE配置所有测试场景的预期结果是否已经记录能否跑出相同数据文档中的公式是否与代码实现一致我曾发现文档里写的转角公式和代码差一个负号这种低级错误非常尴尬是否准备了对比实验数据没有对比数据算法优势说不清楚是否复现过至少一次完整流程防止环境变量、路径问题在关键时刻暴露这个项目做完以后我的感觉是算法本身并不难难的是把算法放到一个工程化的框架里让它跑得起来、测得清楚、说得明白。如果你正在做类似的课程项目或者毕业设计建议先把“路径跟踪和轨迹跟踪的区别”这个问题吃透再上手写代码思路会清晰很多。调参过程中如果遇到什么奇怪的现象欢迎来交流。本文还有配套的精品资源点击获取 C++实现路径跟踪与轨迹跟踪：Pure-pursuit与LQR算法实战
简介本资源是一套面向自动驾驶路径与轨迹跟踪领域的高分毕设级C工程适用于计算机、人工智能、自动化、车辆工程等专业学生及初学者开展算法实践与课程设计。项目融合Pure Pursuit路径跟踪与LQR轨迹跟踪双策略配套改进型A*路由规划含路沿启发函数与均值滤波平滑、ROS节点实现、Spline插值优化及大量测试脚本与文档说明解决从全局路径生成到局部运动控制的完整闭环问题。压缩包共792个文件含169个cpp源码、102个头文件、88张可视化PNG图、39份Markdown说明及11份PDF技术文档总大小45.17MB结构清晰覆盖规划、控制、仿真与调试全流程。项目已通过实际运行验证答辩平均分96分附带完整README指引、launch启动脚本、rviz可视化配置及多种路径生成工具如quinticpath、splinepathnew等可直接部署学习或作为毕设/课设基础框架二次开发。 我去年接手了一门机器人学课程的大作业要求用C实现路径跟踪与轨迹跟踪并附上源代码、文档和测试脚本。我选了Pure-pursuit和LQR两个经典算法来做最后拿到的评分相当不错。最近不少学弟学妹问我这个项目怎么搭、算法怎么调参、代码结构怎么组织才能拿高分所以我把整个从零到一的过程整理出来包括算法原理、工程实现、调试踩坑和文档脚本的写作思路希望能帮到正在做类似项目的朋友。先说清楚一点这个项目名字里有“路径跟踪”和“轨迹跟踪”两个概念很多人容易混为一谈但它们其实是不同的。路径跟踪只关心空间上的几何路径不考虑时间节奏目标是把车稳定地压在目标路径上轨迹跟踪则多了一个时间维度要求车辆在某个时刻到达某个位置对速度和加速度都有约束。Pure-pursuit是典型的路径跟踪算法LQR则非常适合做轨迹跟踪。两个都做进去既能体现技术广度也方便从对比中展示对问题的理解深度这是高分项目的一个关键加分点。1. 为什么是Pure-pursuit和LQR的组合问题拆解1.1 路径跟踪与轨迹跟踪的本质区别我在一开始设计项目时最核心的一个决定就是必须先想清楚“跟踪”到底要解决什么问题。假设有一条参考路径比如一个圆弧或者一条S形曲线路径跟踪的任务是让车辆从一个任意初始位置出发逐步靠近这条路并沿着它行驶。这里的输入是路径的几何信息输出是车辆的前轮转角控制目标是最小化车辆当前位置到路径的横向距离误差。也就是说车快一点、慢一点往往不是最关键的别跑出车道才是关键。轨迹跟踪则复杂一些。轨迹 路径 时间属性每一个轨迹点都带有目标时刻、目标位置、目标速度甚至目标加速度。控制器不仅要把车引导到目标位置还要确保在特定时间点到达并且速度跟随预期曲线。这在机械臂、无人机编队、自动驾驶换道场景里非常常见。比如无人机穿越一串航点每个航点都规定了到达时间和速度只跟踪位置没有用必须要同时控制速度。所以说Pure-pursuit解决的是“空间收敛”问题LQR解决的是“时空跟踪”问题。把两者放在同一个项目里能够覆盖从几何路径规划到动态轨迹跟踪的完整链条。这也是为什么我在项目文档的第一章就花了两页来解释这个区别——答辩时老师一定会问你这两个算法分别解决什么问题为什么需要两个而不是一个。1.2 两个算法在项目中的角色分工在这个项目中我的分工方式是Pure-pursuit负责处理全局路径跟踪输入是一串路径点输出是前轮转角控制量LQR负责处理带时间属性的轨迹跟踪输入是一串带速度约束的轨迹点输出是转角和加速度控制量。两个算法共用同一个车辆运动学模型和同一个仿真环境方便在实验对比中精确控制变量。在具体实现时路径和轨迹都从文件里读取。路径文件只有x和y坐标轨迹文件则在每个点上有x、y、速度和时间戳。仿真器每次更新车辆状态时根据用户选择的算法模式调用对应的控制器计算出控制量再通过车辆模型更新位置和航向角。这种设计让测试非常灵活同一个车辆模型跑两套控制器数据就可以直接对比。这里有一个值得注意的设计思路控制器和车辆模型一定要解耦。控制器只管根据当前状态和目标输出控制量车辆模型只管根据控制量更新状态两者通过接口对接。这样后面不管是换车辆模型比如从自行车模型换成阿克曼模型还是换控制器比如再加一个MPC代码都不用大改。2. Pure-pursuit路径跟踪从几何直觉到代码实现2.1 自行车模型与预瞄点的几何关系Pure-pursuit的核心思想非常直观可以用一句话概括像人走路一样盯着前方一个目标点预瞄点走过去。这个目标点在参考路径上距离车辆当前位置有一个前视距离Ld。控制器做的事情就是计算出车辆当前位置到预瞄点的连线与车身航向之间的夹角然后根据这个夹角计算出前轮转角。推导过程要用到自行车模型bicycle model。把车辆简化为前后两个轮子前轮控制转向后轮固定车辆的运动可以看作一个刚体绕某个瞬时旋转中心转动。假设车辆轴距为L前轮转角为δ则转弯半径R满足R L / tan(δ)这个关系是所有几何类路径跟踪算法的基础。Pure-pursuit在此基础上引入一个几何关系以车辆当前位置为圆心、前视距离Ld为半径画圆这个圆与参考路径的交点就是预瞄点。连接车辆位置和预瞄点这条弦与车辆航向之间的夹角为α则前轮转角为δ arctan(2 * L * sin(α) / Ld)这个公式的推导在《Introduction to Autonomous Mobile Robots》里有详细说明项目文档里我也给了完整的几何图解。简单理解就是α越大说明预瞄点偏离航向越多需要的转角越大Ld越大说明看得越远转向就越平缓。这正好符合驾驶直觉——高速时看远一点转向柔和低速时看近一点转向灵活。2.2 前视距离Ld的标定与自适应前视距离是Pure-pursuit唯一的控制参数也是整个算法调参的核心。如果Ld太小车辆会不断“矫正过正”产生来回震荡路径跟踪效果很差如果Ld太大车辆反应迟钝在急弯处会明显切弯形成较大的静态误差。我在实验中发现Ld取车辆轴距的1到2倍时低速圆形路径的跟踪效果最好但在S形路径上会出现振荡Ld取轴距的3倍以上时振荡消失了但弯道处的横向误差明显增大。实际工程中几乎不会用固定的Ld而是让Ld随车速变化。常见做法是Ld k * v Ld_min其中v是当前车速k是前视系数Ld_min是最小前视距离。k取0.1到0.3之间Ld_min取1到2米根据车辆尺寸。项目脚本里我做了一个参数扫描把k从0.05扫到0.5观察不同取值下的最大横向误差和控制量变化结论是k在0.15到0.25这个区间内圆形路径和S形路径的综合表现都比较好。这里补充一个容易被忽略的细节搜索预瞄点时不能只搜距离最近的路径点而是要从当前路径索引往后搜索找到第一个与车辆位置距离大于等于Ld的点。如果只找最近点车辆会“往回看”导致跟踪方向错误。2.3 代码结构设计与边界条件处理Pure-pursuit控制器的核心代码大约80行分类放在PurePursuitController类里。关键函数有三个SetLookaheadDistance()设置前视距离支持固定值和自适应值CalculateAngle()根据当前车辆状态和参考路径计算前轮转角FindTargetPoint()在路径上搜索预瞄点返回其索引和坐标FindTargetPoint是最容易出bug的地方尤其是处理循环路径和终点附近的索引越界问题。我的做法是传入一个起始搜索索引从当前位置向后搜索如果搜索到路径末尾还没找到预瞄点就取最后一个路径点作为目标。当车辆距离终点小于某个阈值时控制器输出零转角让车直线滑行到终点。关于转向角的正负号这里也值得单独说一下。不同坐标系定义下角度的正负可能完全相反。我在项目里统一用“左转为正”的约定并把所有角度运算都放在同一个坐标系下完成避免混用。编译时开启-Wall警告很多单位问题都能提前暴露。double PurePursuitController::CalculateAngle(const VehicleState state, const std::vectorPathPoint path, int* target_index) { auto target FindTargetPoint(state, path, target_index); double alpha NormalizeAngle(atan2(target.y - state.y, target.x - state.x) - state.yaw); double Ld std::hypot(target.x - state.x, target.y - state.y); if (Ld 1e-6) return 0.0; return atan2(2.0 * wheelbase_ * sin(alpha), Ld); }这个实现里用了hypot而不是自己写sqrt(x^2 y^2)是为了避免大数平方溢出。NormalizeAngle把角度归一化到[-π, π]这是角度运算里的基本功少了它转向角会在π附近跳变导致控制量突变。3. LQR轨迹跟踪从状态空间模型到最优控制量3.1 误差模型的建立为什么需要线性化LQRLinear Quadratic Regulator和Pure-pursuit走的是完全不同的路线Pure-pursuit从几何直觉出发LQR从优化理论出发。LQR的核心逻辑是把轨迹跟踪问题转化为一个状态调节问题设计状态反馈控制率使得一个二次型代价函数最小。这个转化的关键是建立车辆的误差模型。首先定义误差状态——在我们这个项目里取的是横向误差e_y和航向误差e_ψ状态向量为x [e_y, e_ψ, e_y_dot, e_ψ_dot]。车辆模型是非线性的包含sin、cos、速度乘积等项LQR要求系统是线性的所以必须做线性化处理。线性化有两种思路。一种是在稳态工作点附近做泰勒展开得到状态转移矩阵A和控制矩阵B另一种是直接基于误差变化率来建立线性微分方程。我们采用的是第二种因为轨迹跟踪误差模型本身就有明显的线性结构e_y_dot v * e_ψ e_ψ_dot v / L * δ也就是说横向误差的变化率近似等于速度乘以航向误差航向误差的变化率近似等于前轮转角除以轴距再乘以速度。这两个线性方程构成了线性化误差模型的基础不需要复杂的雅可比矩阵推导。当然这样做有一个前提假设误差比较小车辆运行在线性区间附近。如果初始误差很大LQR的表现就会受限此时可以考虑先用Pure-pursuit粗跟踪收敛后再切换到LQR精细跟踪这也是很多实车系统的做法。3.2 离散化处理从连续LQR到可计算的DLQR连续时间的LQR最优控制率是u -K * x其中K R^(-1) * B^T * PP通过求解代数黎卡提方程ARE得到。但在C代码里我们处理的是离散时间系统需要对连续模型做离散化。离散化方法我选的是零阶保持ZOH步长dt取0.05秒和仿真周期保持一致。连续状态空间模型为x_dot A * x B * u离散化后为x(k1) Ad * x(k) Bd * u(k)Ad和Bd可以由矩阵指数计算。在实际代码里我用的是Eigen库的矩阵运算先把A和B构造出来再用离散化求解器得到Ad和Bd。这里有一个比较隐蔽的坑离散化步长必须和仿真步长一致否则控制器的计算频率和模型更新频率不匹配LQR的性能会严重退化。得到离散模型后求解离散代数黎卡提方程DAREP Q Ad^T * P * Ad - Ad^T * P * Bd * (R Bd^T * P * Bd)^(-1) * Bd^T * P * Ad这个方程在C里没有现成的高质量库函数可以直接调用除非引入第三方库所以我在项目里实现了一个迭代法求解器从P_0 Q开始反复迭代上面的方程直到P的增量小于阈值。实验下来一般迭代20到50次就能收敛耗时不到1毫秒。3.3 Q矩阵和R矩阵的工程调参经验Q和R的选取是LQR调参的核心中的核心。Q矩阵的每一项对应一个状态变量的权重R对应控制量的权重。直观理解Q越大控制器越“较真”于误差的消除收敛更快但控制量可能很大R越大控制器越“惜力”控制量更平滑但误差收敛更慢。在这个项目里Q取的是diag([10.0, 1.0, 0.5, 0.5])R取的是[2.0]。横向误差e_y的权重最大因为轨迹跟踪最直观的指标就是横向误差航向误差权重次之两个导数项的权重较小主要起阻尼作用防止系统振荡。实际观察到的现象是Q中e_y的权重从10提高到50时圆形轨迹的最大横向误差从0.12米降到0.05米但转角控制量的标准差从0.02弧度增大到0.06弧度控制变得更“激烈”。R从2.0降到0.5时控制量的峰值明显增大但横向误差基本没有改善说明R取太小会浪费控制能量。R取2.0到5.0之间是一个比较合理的区间。这里还必须强调一个我在调试过程中踩过的坑Q和R的量纲必须匹配。如果横向误差的单位是米控制量的单位是弧度那么Q的横向误差项的量纲是1/m²R的量纲是1/rad²。如果单位不对两者的数量级差太多调参时很难找到合理的平衡点。我的做法是在代码里用物理单位清晰标注每个变量的量纲文档里也给出了单位说明表。4. 仿真环境与车辆模型让算法在跑起来之前先“活起来”4.1 车辆运动学模型的C实现与单位统一项目使用了一个二维平面内的车辆运动学模型状态量为x坐标、y坐标、航向角yaw控制量为前轮转角delta和速度v。状态更新公式为x_new x v * cos(yaw) * dt y_new y v * sin(yaw) * dt yaw_new yaw v * tan(delta) / L * dt这个模型适用于低速场景下的路径跟踪仿真。在代码实现里VehicleModel类封装了Reset()和Step()两个方法前者初始化状态后者根据控制量更新状态。所有角度都使用弧度制长度单位统一为米速度单位统一为米每秒。说到单位统一这里有一个非常惨痛的教训。我第一次写仿真器时把速度单位写成了km/h但路径坐标用的是米结果跟踪结果完全对不上车辆在路径附近来回打转看起来像是算法完全失效。排查了半天最后发现就是单位问题。所以我的建议是在头文件或者文档的显著位置明确标注所有物理量的单位并且在整个项目里强制统一。4.2 目标路径与目标轨迹的生成脚本为了测试算法我写了一个Python脚本生成三种参考路径圆形路径、S形路径和U形路径。每种路径都以固定间距采样生成一系列路径点保存为CSV文件。圆形路径的生成最简单半径取10米圆心在原点从0到2π均匀采样。S形路径用正弦函数生成x方向均匀递增y方向按正弦变化目的是测试算法在连续曲率变化下的表现。U形路径模拟的是倒车入库或者狭窄弯道曲率在转弯段突然增大对Pure-pursuit的前视距离特别敏感。轨迹文件的生成则复杂一些因为要带时间信息。我的做法是先生成路径再根据预设的速度曲线给每个点分配到达时间。简单做法是匀速假设每个路径点间隔固定时间进阶做法是梯形速度曲线起步、匀速、减速三个阶段的时间戳不同这样能更真实地模拟实际行驶场景。脚本统一放在scripts/目录下用argparse接收输出路径和路径类型运行方式非常简单python3 scripts/generate_path.py --type circle --radius 10 --output data/circle_path.csv python3 scripts/generate_trajectory.py --type s_curve --speed 2.0 --output data/s_trajectory.csv这样做的好处是算法代码完全不依赖路径的生成方式只需要从CSV文件里读取数据大大提高了模块化程度。4.3 仿真循环的实现与数据记录仿真器是整个项目的中枢我把它设计成一个Simulator类内部持有VehicleModel和两个控制器PurePursuitController和LQRController以及当前激活的控制器指针。主循环如下根据当前时间从轨迹中插值得到参考状态调用激活的控制器输出控制量将控制量输入VehicleModel更新车辆状态记录每个时刻的状态误差数据到日志文件仿真步长dt取0.05秒即20Hz的控制频率这和LQR的离散化步长保持一致。整个流程用命令行参数控制支持切换算法、加载不同路径、调整参数。运行完以后日志文件会记录每一时刻的跟踪误差、控制量、车辆状态方便用Python脚本进行可视化分析。5. 测试结果与对比分析数据怎么说5.1 误差指标的定义与计算跟踪效果不能只靠肉眼观察必须用定量的误差指标来衡量。我在项目中定义了四个核心指标平均绝对误差MAE所有时刻横向误差绝对值的平均值均方根误差RMSE横向误差平方和的均值的平方根对大误差更敏感最大横向误差Max lateral error整个过程中最大的横向偏差控制量变化率单位时间内转角控制量的变化量反映控制平滑度这些指标的计算都在Python的analysis.py脚本里完成读入日志后直接输出汇总表格。在项目文档中我给出了两种算法在三种路径下的指标对比表并且讨论了每个指标的物理意义。5.2 不同路径下的算法表现差异在半径10米的圆形路径上车辆以1.5米/秒的速度行驶时调速后的Pure-pursuit的最大横向误差约0.08米LQR的最大横向误差约0.04米两者差别不大LQR稍优。但在S形路径上差距变得明显——Pure-pursuit的横向误差在转弯点有周期性波动最大达到0.15米而LQR的最大误差稳定在0.06米以内。这说明Pure-pursuit本质上是“参考点跟踪”而非“路径跟踪”它只盯着预瞄点对路径的曲率变化没有显式的建模能力。而LQR通过状态反馈对航向误差和横向误差同时做闭环控制能够更快地响应曲率变化。在U形路径上Pure-pursuit的问题暴露得更明显。由于急转弯处曲率大固定前视距离会导致车辆“切弯”最大横向误差达到0.3米而LQR即便在这种路径下也能把误差控制在0.1米以内。5.3 速度变化对跟踪性能的影响我还做了一组变速实验把速度从1.0米/秒逐步提升到3.0米/秒观察两种算法在圆形路径上的表现。结果是Pure-pursuit在2.0米/秒以下表现良好速度超过2.5米/秒后由于前视距离的自适应调整跟不上横向误差开始显著上升LQR在3.0米/秒时依然能保持0.1米以内的误差整个过程的控制量曲线也相对平滑。总的来看这个项目用数据证明了“路径跟踪”和“轨迹跟踪”在算法选型上的差异化需求如果只是低速环境下的路径跟踪Pure-pursuit完全够用而且实现简单如果对时间约束和高速工况有要求LQR或更复杂的最优控制算法是更好的选择。这个结论直接写在了项目文档的结论章节答辩时老师对此给予了正面评价。6. 调试排错记录那些让我崩溃又成长的问题6.1 预瞄点搜索越界与跳变问题Pure-pursuit在路径终点附近的预瞄点搜索经常会越界导致程序崩溃。我最初的处理是搜索到路径末尾就取最后一个点但这样会出现一个现象车辆在接近终点时目标点突然回退导致转向角突变。排查后发现问题出在“从当前路径索引往后搜”的实现里索引已经越过终点却又被判断条件拉回去了。解决方案是在找到目标点后不仅记录目标点的坐标还记录其索引。下一帧搜索时从该索引开始而不是从0开始并且如果索引已经是最后一个点就直接用当前点作为目标不让索引回退。这一改终点附近的振荡问题立刻消失。6.2 LQR求解Riccati方程时的数值不稳定LQR的Riccati方程迭代法在Q矩阵元素差距过大时会出现数值不稳定的情况。最典型的表现是迭代次数超过100次仍未收敛或者P矩阵出现负特征值导致控制率K计算异常。我的解决办法是两招并用第一在迭代前检查系统可控性如果可控性矩阵的条件数过大直接报错提示调参第二给P矩阵的不对角元素加一个正则化项避免P矩阵趋于奇异。实际调试时这个正则化系数取1e-10就足够了既不影响收敛精度又能稳定数值行为。另一个非常有用的调试技巧是把同一组参数在Matlab的dlqr函数里算一遍当作对照基准。Matlab的结果和C迭代法的结果对比如果差距超过1e-6说明迭代法没有完全收敛需要提高迭代次数阈值。这个对照方法帮我发现了一个隐蔽的bug——我当时忘记了P矩阵需要对称化导致迭代误差累积最终求出来的K不符合最优解。6.3 控制量突变与仿真崩溃的处理仿真过程中偶尔会出现控制量突然变大车辆直接飞出去的情况。一开始我以为是算法问题后来发现原因居然是对输入轨迹的插值处理不当当车辆与大目标点距离很近时速度方向与目标方向不一致导致角度差在π附近跳变控制量被放大。解决方案是在所有角度差计算后都加一个NormalizeAngle调用确保差值始终在[-π, π]内。这个函数我是在工具模块utils.h里统一写的所有控制器都调用同一个函数。之后无论是Pure-pursuit还是LQR控制量突变的概率都大幅降低。7. 文档与脚本组织高分项目的最后一块拼图7.1 README与设计文档的写作思路一个“高分项目”不仅仅看代码能不能跑更看你的表达能力与工程素养文档是核心载体。我的项目文档分四部分项目概览一句话描述项目目标列出技术栈说明算法选型理由算法原理分别介绍Pure-pursuit和LQR的数学推导过程配几何示意图和公式代码结构与使用说明说明文件目录、如何编译、如何运行仿真、如何分析结果实验结果与分析展示测试指标表格、典型场景的对比图并给出结论写文档时我给自己的要求是每个公式都必须有物理意义的解释绝不堆砌推导过程。比如LQR的Riccati方程我会写清楚每一项的作用P矩阵的初值怎么取不收敛怎么处理这样读文档的人才能真正理解——而不是照抄教材。7.2 自动化脚本与一键复现流程脚本让项目具备可复现性。我把项目中用到的所有脚本做了分类数据生成脚本、仿真运行脚本、结果分析脚本和参数扫描脚本。每个脚本都有标准化的命令行参数支持通过一个shell脚本批量调用。一键复现的流程是这样设计的先运行build_and_run.sh编译项目再运行run_experiments.sh跑完所有预设场景最后运行analyze.sh生成对比图表。整个流程不需要人工干预跑完以后所有结果都汇总在results/目录下。这种方式在答辩演示时非常加分老师可以直接看到完整的从编译到出结果的工程链路。7.3 答辩前检查清单最后分享一个我答辩前整理的项目检查清单照着做一遍能有效减少翻车概率代码能否在干净环境中从零编译成功不依赖IDE配置所有测试场景的预期结果是否已经记录能否跑出相同数据文档中的公式是否与代码实现一致我曾发现文档里写的转角公式和代码差一个负号这种低级错误非常尴尬是否准备了对比实验数据没有对比数据算法优势说不清楚是否复现过至少一次完整流程防止环境变量、路径问题在关键时刻暴露这个项目做完以后我的感觉是算法本身并不难难的是把算法放到一个工程化的框架里让它跑得起来、测得清楚、说得明白。如果你正在做类似的课程项目或者毕业设计建议先把“路径跟踪和轨迹跟踪的区别”这个问题吃透再上手写代码思路会清晰很多。调参过程中如果遇到什么奇怪的现象欢迎来交流。本文还有配套的精品资源点击获取