1. Pure Pursuit 算法数学推导
大家好，我是你们的老朋友。今天咱们来啃一块硬骨头——Pure Pursuit 算法的数学推导。说实话，我刚入行那会儿，看到这些几何公式也头疼。但后来我发现，搞懂它，你就能在自动驾驶控制领域横着走。
先问个问题：为什么叫“纯跟踪”？说白了，就是让车像一条狗追着骨头跑一样。狗的眼睛盯着骨头，身体自然调整方向。我们的算法也一样——盯着前方一个目标点，然后算方向盘该打多少。
1.1 几何关系推导
咱们先画个图。想象一下，你的车现在在某个位置，车头朝前。前方有个目标点，你希望车能平滑地开过去。这中间有个关键参数——前视距离。
核心公式：
曲率 κ = 2 * sin(α) / Ld
其中 α 是车头方向与目标点方向的夹角，Ld 是前视距离
这个公式怎么来的？我给大家拆解一下。你看，车、目标点、还有转弯中心，这三者构成了一个等腰三角形。嗯，这里要注意，转弯半径 R 和曲率 κ 是倒数关系：κ = 1/R。
我在项目中遇到过一个问题：如果前视距离太短，车会像喝醉了酒一样左右摇摆。如果太长，车又像反应迟钝的老大爷。所以这个 Ld 的选取，是门学问。
推导过程其实不复杂：
- 从车的位置到目标点，画一条直线，长度就是 Ld
- 车头方向与这条线的夹角是 α
- 根据正弦定理，sin(α) / R = sin(π - 2α) / Ld
- 化简后得到 κ = 2 * sin(α) / Ld
你想想看，这个公式多漂亮。只要知道 α 和 Ld，就能算出需要的转弯半径。这就是 Pure Pursuit 的灵魂。
1.2 前视距离概念
前视距离，说白了就是“你看多远”。这个参数直接决定了车的控制品质。
| 前视距离 | 效果 | 适用场景 |
|---|---|---|
| 太短 | 抖动、震荡 | 低速、急转弯 |
| 适中 | 平滑、稳定 | 一般道路 |
| 太长 | 响应慢、切弯 | 高速、直道 |
我个人习惯的做法是：让前视距离随车速动态变化。车速快，就看远一点；车速慢，就看近一点。公式很简单：
Ld = k * v + Ld_min其中 k 是比例系数，v 是车速，Ld_min 是最小前视距离。这个 k 值怎么调？我建议从 0.3 开始试，然后根据实际效果微调。
我的经验：曾经在一个项目中，前视距离设得太小，车在弯道里疯狂画龙。后来我把 Ld_min 从 2 米调到 4 米，问题立刻解决了。有时候，问题就出在这些小细节上。
1.3 曲率半径计算
曲率半径，就是转弯半径。它决定了方向盘该打多少。我们算出来曲率 κ 后，方向盘转角 δ 就是：
δ = arctan(L * κ)其中 L 是轴距。这个公式来自阿克曼转向几何，我在这里就不展开了。
实际项目中，我一般会加个限幅。比如最大方向盘转角是 30 度，那算出来超过 30 度就截断。为什么？因为物理极限摆在那里，你算得再漂亮，硬件做不到也是白搭。
注意：曲率 κ 不能太大。如果目标点选得太近，算出来的 κ 会非常大，导致方向盘猛打。这在高速上是很危险的。我曾经吃过这个亏，后来加了曲率限幅才解决。
1.4 目标点选取策略
目标点怎么选？这其实是个工程问题。理论上，你只需要在参考轨迹上找一个距离车 Ld 的点就行。但实际操作中，有几种策略：
- 最近点投影法：先找到轨迹上离车最近的点，然后沿着轨迹往前走 Ld 距离
- 直接距离法：从车的位置出发，画一个半径为 Ld 的圆，与轨迹的交点就是目标点
- 混合策略：结合以上两种，根据曲率动态调整
我个人推荐第一种。为什么？因为它稳定。第二种方法在轨迹曲率变化大的地方，可能会找到多个交点，造成跳变。
嗯，这里要注意：目标点不能选在车后面。如果轨迹是环形的，或者车已经过了弯道，目标点可能会跑到后面去。这时候要做个判断，如果目标点在车后方，就跳过这一帧。
核心要点总结：
- Pure Pursuit 的核心是几何关系：κ = 2 * sin(α) / Ld
- 前视距离 Ld 要随车速动态调整
- 方向盘转角 δ = arctan(L * κ)，记得加限幅
- 目标点选取推荐最近点投影法
好了，这一章的内容就到这里。数学推导看着枯燥，但它是整个算法的根基。你把它吃透了，后面写代码、调参数都会顺手很多。
公众号:蓝海资料掘金营，微信deep3321