simulink抖动_Pure Pursuit(纯跟踪算法)与Simulink实现

一，纯跟踪算法数学原理：

如上图所示，A点是车辆后轴中心（也是图中坐标系的原点，图中这种坐标系我们一般称之为：车辆坐标系），B点是车辆要达到的目标点（预瞄点），L是AB之间的距离（预瞄距离），O点是假设存在的圆心使得车辆从位置A以转弯半径r可以达到位置B。圆心位置O的坐标与转弯半径r就是计算目标。此图颇有遗憾，因为AO线段看上去比BO线段长，但二者应该是相等的，都是转弯半径。

理一下已知信息有哪些：

B点坐标，A点坐标，AB之间的距离L

计算目标：转弯半径r

根据图中几何关系：r = x +d

L^2 = x^2 + y^2

一步步推导：

d = r -x

(r-x)^2+y^2=r^2

r^2-2rx+x^2+y^