文章目录

参考资料1. 几何车辆模型2. Pure Pursuit（纯追踪）算法2.1 算法思想2.2 算法推导2.3 小结1. 算法伪代码2. 模型适配3. 前视距离调整3. python代码实现4. c++代码实现

参考资料

轨迹跟踪Pure Pursuit方法使用pure pursuit实现无人车轨迹追踪论文

1. 几何车辆模型

在前文中讲解了PID实现轨迹跟踪，这篇来讲解纯追踪法。

使用的车辆模型这里依旧采取以后轴中心为车辆中心的单车运动学模型

其满足

tan⁡δf=LR(1)\tag{1} \tan{\delta_f}=\frac{L}{R}tanδf​=RL​(1)

2. Pure Pursuit（纯追踪）算法

2.1 算法思想

纯追踪算法的原理很简单，就是单车模型通过调整前轮转向δ\deltaδ运动，使得车辆后轴中心刚好可以经过当前规划的路点。换句话说，此时的后轴中心为圆弧切点，车辆纵向车身为切线。通过控制前轮转角δ\deltaδ， 使车辆可以沿着一条经过目标路点（goal point）（或者叫预瞄点）的圆弧行驶。

该算法会根据机器人的当前位置在路径上移动预瞄点，直到路径的终点。可以想象成机器人不断追逐它前面的一个点。

总结如下：

基于当前车辆后轴中心位置,在参考路径上向ldl_dld​ (自定义，称为前视距离)的距离匹配一个预瞄点。假设车辆后轴中心点可以按照一定的转弯半径RRR行驶抵达该预瞄点,然后根据预瞄距离ldl_dld​、转弯半径RRR、车辆坐标系下预瞄点的朝向角2α2\alpha2α之间的几何关系确定前轮转角。

2.2 算法推导

δ\deltaδ即为δf\delta_fδf​。

α为路点与车后轴中心连成的向量的角度与车辆偏航角的差值，当路点在车的左边时， α>0\alpha>0α>0 ，反之则 α<0\alpha<0α<0 ； ldl_dld​为车后轴离前视路点的距离，又被称为前视距离，它决定了将预瞄路点放置多远。

根据上图，由正弦定理得

ldsin⁡(2α)=Rsin⁡(π2−α)(2)\tag{2} \frac{l_{d}}{\sin (2 \alpha)}=\frac{R}{\sin \left(\frac{\pi}{2}-\alpha\right)} sin(2α)ld​​=sin(2π​−α)R​(2)

根据三角函数性质，对等式(2)化简得

ld2sin⁡(α)cos⁡(α)=Rcos⁡(α)(3)\tag{3} \frac{l_{d}}{2 \sin (\alpha) \cos (\alpha)}=\frac{R}{\cos (\alpha)} 2sin(α)cos(α)ld​​=cos(α)R​(3)

由于 cos⁡(α)≠0\cos (\alpha) \neq 0cos(α)​=0 ，对等式(3)进一步化简得

R=ld2sin⁡(α)(4)\tag{4} R=\frac{l_{d}}{2 \sin (\alpha)} R=2sin(α)ld​​(4)

故圆弧的曲率表示为

k=2sin⁡(α)ld(5)\tag{5} k=\frac{2 \sin (\alpha)}{l_{d}} k=ld​2sin(α)​(5)

根据等式(1)所示的车辆几何关系得

δ=arctan⁡(LR)=arctan⁡(k⋅L)(6)\tag{6} \delta=\arctan(\frac{L}{R})=\arctan (k \cdot L) δ=arctan(RL​)=arctan(k⋅L)(6)

将等式(5)带入等式(6)得纯追踪算法控制量的的最终表达式：

δ=arctan⁡(2Lsin⁡αld)(7)\tag{7} \delta=\arctan \left(\frac{2 L\sin {\alpha} }{l_{d}}\right) δ=arctan(ld​2Lsinα​)(7)

当然，通过上图可知

sin⁡α=eyld(8)\tag{8} \sin \alpha=\frac{e_{y}}{l_{d}} sinα=ld​ey​​(8)

将式(8)代入式(7)得:

δ=arctan⁡(2Leyld2)(9)\tag{9} \delta=\arctan \left(\frac{2 L e_{y}}{l_{d}^{2}}\right) δ=arctan(ld2​2Ley​​)(9)

分析式(9)，利用小角度近似，我们有:

δ≈2Lld2ey\delta\approx \frac{2 L}{l_{d}^{2}} e_{y} δ≈ld2​2L​ey​

把 2Lld2\frac{2L}{l_d^2}ld2​2L​​看作比例控制器的参数，eye_yey​作为系统误差，那么这就相当于一个以横向跟踪误差CTE作为系统误差的比例控制器。

在pure pursuit方法中，ldl_dld​表示成无人车纵向线速度的形式，即 ld=λvx+c，cl_{d}=\lambda v_{x}+c ， cld​=λvx​+c，c 为常数。

2.3 小结

1. 算法伪代码

算法伪代码可以概括如下：

输入：当前车辆位置、纵向速度vxv_xvx​和当前目标路点计算： ld=λvx+cl_{d}=\lambda v_{x}+cld​=λvx​+c计算α\alphaαδ=arctan⁡(2Lsin⁡αld)\delta=\arctan \left(\frac{2 L\sin {\alpha} }{l_{d}}\right)δ=arctan(ld​2Lsinα​) 输出：车辆转角δ\deltaδ

2. 模型适配

纯跟踪算法是把车看成一个阿克曼模型进行几何解算的，本质是不适合差速车或者麦轮车的

3. 前视距离调整

前视距离的调整需要根据机器人运行的实际情况来进行调整。

前视距离是整个Pure Pursuit控制器的重要参数。往前看的距离是机器人从当前位置应沿着路径观察的距离，以计算转角控制命令。

在低速的情况下合理调整前视距离可以实现较好的路径跟踪效果，较小的前视距离能使机器人更加精确地追踪路径，但可能会引起机器人控制的不稳定甚至震荡；

较大的前视距离可以使机器人跟踪路径更加平滑，但不能精确地跟踪原始的路径，在大转角处会出现曲率大、转向不足的情况。

3. python代码实现

该部分有参考python robotics代码

以后轴中心为车辆中心的单车运动学模型为（具体可参考笔者之前的博客）

{x˙=Vcos⁡(ψ)y˙=Vsin⁡(ψ)ψ˙=VLtan⁡δfV˙=a\left\{\begin{array}{l} \dot{x}=V \cos (\psi) \\ \dot{y}=V \sin (\psi) \\ \dot{\psi}=\frac{V}{L}\tan{\delta_f}\\ \dot{V}=a \end{array}\right. ⎩⎪⎪⎨⎪⎪⎧​x˙=Vcos(ψ)y˙​=Vsin(ψ)ψ˙​=LV​tanδf​V˙=a​

单车运动学模型实现如下。

import mathclass KinematicModel_3:"""假设控制量为转向角delta_f和加速度a"""def __init__(self, x, y, psi, v, L, dt):self.x = xself.y = yself.psi = psiself.v = vself.L = L# 实现是离散的模型self.dt = dtdef update_state(self, a, delta_f):self.x = self.x+self.v*math.cos(self.psi)*self.dtself.y = self.y+self.v*math.sin(self.psi)*self.dtself.psi = self.psi+self.v/self.L*math.tan(delta_f)*self.dtself.v = self.v+a*self.dtdef get_state(self):return self.x, self.y, self.psi, self.v

相关参数：

L=2 # 车辆轴距，单位：mv = 2 # 初始速度x_0=0 # 初始xy_0=-1 #初始ypsi_0=0 # 初始航向角dt=0.1 # 时间间隔，单位：slam = 0.1 # 前视距离系数c=2 # 前视距离

计算参考轨迹上车辆当前位置的前视目标点：

def cal_target_index(robot_state, refer_path, l_d):"""得到前视目标点Args:robot_state (_type_): 当前车辆位置refer_path (_type_): 参考轨迹（数组）l_d：前视距离Returns:_type_: 前视目标点的索引"""dists = []for xy in refer_path:dis = np.linalg.norm(robot_state-xy)dists.append(dis)min_index = np.argmin(dists)delta_l = np.linalg.norm(refer_path[min_index]-robot_state)# 搜索前视目标点while l_d > delta_l and (min_index+1) < len(refer_path):delta_l = np.linalg.norm(refer_path[min_index+1]-robot_state)min_index += 1return min_index

Pure pursuit算法：

def pure_pursuit_control(robot_state,current_ref_point,l_d):"""pure pursuitArgs:robot_state (_type_): 车辆位置current_ref_point (_type_): 当前参考路点l_d：前视距离return:返回前轮转向角delta"""alpha = math.atan2(current_ref_point[1]-robot_state[1], current_ref_point[0]-robot_state[0])-ugv.psidelta = math.atan2(2*L*np.sin(alpha),l_d)return delta

主函数

from celluloid import Camera # 保存动图时用，pip install celluloid# set reference trajectoryrefer_path = np.zeros((1000, 2))refer_path[:,0] = np.linspace(0, 100, 1000) # 直线refer_path[:,1] = 2*np.sin(refer_path[:,0]/3.0)+2.5*np.cos(refer_path[:,0]/2.0) # 生成正弦轨迹ugv = KinematicModel_3(x_0,y_0,psi_0,v,L,dt)x_ = []y_ = []fig = plt.figure(1)# 保存动图用camera = Camera(fig)for i in range(600):robot_state = np.zeros(2)robot_state[0] = ugv.xrobot_state[1] = ugv.yl_d = lam*ugv.v+c # 注意，这里的运动学模型使用的速度v就是车身纵向速度vxind = cal_target_index(robot_state,refer_path,l_d) # 搜索前视路点delta = pure_pursuit_control(robot_state,refer_path[ind],l_d)ugv.update_state(0,delta) # 加速度设为0，恒速x_.append(ugv.x)y_.append(ugv.y)# 显示动图plt.cla()plt.plot(refer_path[:, 0], refer_path[:, 1], '-.b', linewidth=1.0)plt.plot(x_, y_, "-r", label="trajectory")plt.plot(refer_path[ind, 0], refer_path[ind, 1], "go", label="target")# plt.axis("equal")plt.grid(True)plt.pause(0.001)#camera.snap()# animation = camera.animate()# animation.save('trajectory.gif')plt.figure(2)plt.plot(refer_path[:,0], refer_path[:,1], '-.b', linewidth=1.0)plt.plot(x_,y_,'r')plt.show()

结果如下：

完整代码见github仓库

4. c++代码实现

由于在自动驾驶中算法实现一般使用C++，所以我也使用C++实现了相关功能，代码结构与python代码实现类似，这边不做相关代码解释。完整代码详见另一个github仓库。