一、Pure Pursuit算法模型图解和公式推导
下图分析的双舵轮AGV的运动学模型，是按照前后舵轮均安装在车体中心线上的车型分析的，当然其它结构的车型也可以按照这样的分析方法画出如下图解。
通过正弦定理推导：
即
则可推导出转弯圆弧的曲率k：
还可得到：
即可推导出：（重要）
由上式可知，本控制器的本质是对转角进行控制，以减少横向误差为目标的横向控制器。其中，
可视为控制器的P参数。L为车辆的轴距，Ld为设定的预瞄距离。 本控制器的控制效果主要取决于预瞄距离的选取，一般来说预瞄距离越长，控制效果会越平滑，预瞄距离越短，控制效果会越精确（同时也会带来一定的震荡）。预瞄距离的选取也和当前车速有关。
通常来说ld 被认为是车速的函数，在不同的车速下需要选择不同的前视距离。
一种最常见的调整前视距离的方法就是将前视距离表示成车辆纵向速度的线形函数，即Ld=k*Vx，那么前轮的转角公式就变成了：
于是纯追踪控制器的参数调整就变成了调整前视系数k。常来说，会使用最大、最小前视距离来约束前视距离，越大的前视距离意味着轨迹的追踪越平滑，小的前视距离会使得追踪更加精确（当然也会带来控制的震荡）。
而两轮转角对应的角速度W，根据当前控制速度V可得：
非线性的预瞄距离选取方式
在有些文献中，预瞄距离的选取方式表现为如下形式（二次方程）：
上式中，对于常数A：
amax为最大制动加速度。Av2表示最短车辆制动距离。B表示车辆遇到异常时需要的反应时间，Bv则为对应的反应距离，C表示车辆的最小转弯半径。
实际应用总结
在实际应用中，Pure Pursuit算法通常不要求跟踪的目标点到本车中心的距离切实等于预瞄距离。而是会选择采样好的一系列目标点中到中心距离最接近预瞄距离的那个点来近似跟踪。这样做的好处是可以不需要目标轨迹的函数方程来求解真实预瞄距离坐标，极大地提升了算法的效率。
实践中，纵向控制Vx通常使用一个简单的P控制器，横向控制（即转角控制）我们使用纯追踪控制器。
二、python实现三阶贝塞尔曲线跟踪
三阶贝塞尔曲线路径生成以及轨迹跟踪仿真：
import numpy as npimport mathimport matplotlib.pyplot as pltk = 0.1 # 前视距离系数Lfc = 0.5 # 前视距离Kp = 1.0 # 速度P控制器系数dt = 0.1 # 时间间隔，单位：sL = 0.68 # 车辆轴距，单位：mclass VehicleState:def __init__(self, x=0.0, y=0.0, yaw=0.0, v=0.0):self.x = xself.y = yself.yaw = yawself.v = v# 双舵轮车型的状态更新函数（模拟里程计）def update(state, a, delta):##state.x = state.x + state.v * math.cos(state.yaw) * dtstate.y = state.y + state.v * math.sin(state.yaw) * dt# state.yaw = state.yaw + state.v / L * math.tan(delta) * dtstate.yaw = state.yaw + state.v / (L*0.5) * math.tan(delta) * dt #TODO:车体中心为运动半径, v + w*dtstate.v = state.v + a * dtreturn state#p速度控制器，平滑小车前向速度def PControl(target, current):a = Kp * (target - current)return a# 纯跟踪算法控制器，输出为角速度（双舵轮的两个舵向角）、路点索引值def pure_pursuit_control(state, cx, cy, pind):ind = calc_target_index(state, cx, cy)# pind：上一个路点索引值if pind >= ind:ind = pindif ind < len(cx):# tx、ty：车子所在（近似）路径上的点tx = cx[ind]ty = cy[ind]else:tx = cx[-1]ty = cy[-1]ind = len(cx) - 1alpha = math.atan2(ty - state.y, tx - state.x) - state.yaw #车身与预瞄点夹角if state.v < 0: # backalpha = math.pi - alphaLf = k * state.v + Lfc # 前视距离*时间系数# delta = math.atan2(2.0 * L * math.sin(alpha) / Lf, 1.0)delta = math.atan2(L * math.sin(alpha) / (Lf*Lf), 1.0) #TODO:双舵轮转角进行控制return delta, ind# 搜索最临近车体的路点def calc_target_index(state, cx, cy):dx = [state.x - icx for icx in cx]dy = [state.y - icy for icy in cy]d = [abs(math.sqrt(idx ** 2 + idy ** 2)) for (idx, idy) in zip(dx, dy)]ind = d.index(min(d))L = 0.0Lf = k * state.v + Lfcwhile Lf > L and (ind + 1) < len(cx):dx = cx[ind + 1] - cx[ind]dy = cx[ind + 1] - cx[ind]L += math.sqrt(dx ** 2 + dy ** 2)ind += 1return ind"""三阶贝塞尔曲线"""def bezier_get(start_p, control1_p, control2_p, end_p, t_in):# t为微分系数，0.01表示temp = 1.0 - t_int = t_incx = []cy = []while(t < 1.0):t = t_in + ttemp = 1.0 - tpot_x = start_p[0]*temp*temp*temp + 3*control1_p[0]*t*temp*temp + 3*control2_p[0]*t*t*temp + end_p[0]*t*t*tcx.append(pot_x)pot_y = start_p[1]*temp*temp*temp + 3*control1_p[1]*t*temp*temp + 3*control2_p[1]*t*t*temp + end_p[1]*t*t*tcy.append(pot_y)return cx, cydef main():# n bezier pathstart_p = [1.0, 5.0]control1_p = [4.0, 8.0]control2_p = [7.0, 5.0]end_p = [11.0, 10.0]cx, cy = bezier_get(start_p, control1_p, control2_p, end_p, 0.01)########### bezier #################### target_speed = 1.5 # [m/s]T = 10.0 # 最大模拟时间[s]# 设置车辆的init状态state = VehicleState(x = start_p[0], y = start_p[1], yaw=0.5, v=0.0)lastIndex = len(cx) - 1time = 0.0x = [state.x]y = [state.y]yaw = [state.yaw]v = [state.v]t = [0.0]target_ind = calc_target_index(state, cx, cy)while T >= time and lastIndex > target_ind:ai = PControl(target_speed, state.v)di, target_ind = pure_pursuit_control(state, cx, cy, target_ind)state = update(state, ai, di)time = time + dtx.append(state.x)y.append(state.y)yaw.append(state.yaw)v.append(state.v)t.append(time)plt.cla()plt.plot(cx, cy, ".r", label="course")plt.plot(x, y, "-b", label="trajectory")plt.plot(cx[target_ind], cy[target_ind], "go", label="target")plt.axis("equal")plt.grid(True)plt.title("Speed[m/s]:" + str(state.v)[:4])plt.pause(0.001) # odom 1000Hzplt.show()if __name__ == '__main__':main()
注意点：
需要pip安装numpy、matplotlib等 ，可以直接按照我的另一篇博文推介的开源项目安装（强烈安利这个开源项目，机器人各方面的算法都有涉及）。
顺便放一个成品效果演示，工程化实现，用任务管理方式实现了直线、贝塞尔曲线、旋转、自然导航等子任务的顺序执行，任务指令使用json方式下达（代码不开放，有问题可以问）。
双舵轮AGV纯跟踪算法仿真演示
参考链接：
1）无人驾驶之车辆控制（1）-- 纯跟踪（Pure Pursuit）算法与Stanley算法：
/zxxxxxxy/article/details/103665245
2）无人驾驶汽车系统入门（十八）——使用pure pursuit实现无人车轨迹追踪：
/AdamShan/article/details/80555174
3）LaTex在线公式编辑器：
/eqnedit.php
