之前对Pure Pursuit控制算法作了介绍，并用Matlab进行了仿真，具体参考：/Ronnie_Hu/article/details/115817922?spm=1001..3001.5501。
下面改用python对其进行仿真，同样跟踪一个圆形轨迹，具体代码如下：
import numpy as npimport matplotlib.pyplot as pltimport math# set figure sizeplt.figure(figsize=(8, 8))# define UGV classclass UGV_model:def __init__(self, x0, y0, theta0, v0, L, T):self.x = x0self.y = y0self.theta = theta0self.v = v0self.l = Lself.dt = Tdef update(self,deltat):dx_vs_dt = self.v*np.cos(self.theta) dy_vs_dt = self.v*np.sin(self.theta)dtheta_vs_dt = self.v*np.tan(deltat)/self.lself.x += dx_vs_dt*self.dtself.y += dy_vs_dt*self.dtself.theta += dtheta_vs_dt*self.dtdef draw(self):plt.scatter(self.x, self.y, color='r')plt.axis([-20, 20, -20, 20])plt.grid(linestyle=":")# set circle reference trajectoryrefer_traj = np.ones((200,2))for k in range(200):refer_traj[k,0] = 15*math.cos(2*np.pi/200*k)refer_traj[k,1] = 15*math.sin(2*np.pi/200*k)# draw reference trajectoryplt.plot(refer_traj[:,0], refer_traj[:,1], color='b')# an UGV instanceugv = UGV_model(0, 0, np.pi/2, 1.6, 2.6, 0.5)# define lookaheadld = ugv.v*2# Pure Pursuit algorithmflag = 0for i in range(200):vehicle_state = np.zeros(2)vehicle_state[0] = ugv.xvehicle_state[1] = ugv.ycnt = 0;min_ds = 100000000; Q = []for m in range(flag,200):deltax,deltay = refer_traj[m] - vehicle_stateds = math.sqrt(deltax*deltax+deltay*deltay)if(ds >= ld):temp = [ds,refer_traj[m,0],refer_traj[m,1],m]Q.append(temp)cnt += 1else:passpass# catch the nearest reference pointfor j in range(cnt):if(Q[j][0]<min_ds):flag = Q[j][3]min_ds = Q[j][0] else:passpassdx,dy = refer_traj[flag] - vehicle_statealpha = math.atan2(dy,dx) - ugv.thetadelta = math.atan(2*np.sin(alpha)*ugv.l/min_ds)ugv.update(delta)ugv.draw()# pursuit the end reference pointif(flag==199):breakelse:pass
仿真的结果如下图所示，蓝色为参考轨迹、红色为跟踪轨迹。从上面的代码不难看出，仿真中采用了“走捷径”的方法，即每次在剩余跟踪点中挑选距离最近的点来跟踪。
下面的代码就没有采取“走捷径”的方法。
import numpy as npimport matplotlib.pyplot as pltimport math# set figure sizeplt.figure(figsize=(8, 8))# define UGV classclass UGV_model:def __init__(self, x0, y0, theta0, v0, L, T):self.x = x0self.y = y0self.theta = theta0self.v = v0self.l = Lself.dt = Tdef update(self,deltat):dx_vs_dt = self.v*np.cos(self.theta) dy_vs_dt = self.v*np.sin(self.theta)dtheta_vs_dt = self.v*np.tan(deltat)/self.lself.x += dx_vs_dt*self.dtself.y += dy_vs_dt*self.dtself.theta += dtheta_vs_dt*self.dtdef draw(self):plt.scatter(self.x, self.y, color='r')plt.axis([-20, 20, -20, 20])plt.grid(linestyle=":")# set circle reference trajectoryrefer_traj = np.ones((200,2))for k in range(200):refer_traj[k,0] = 15*math.cos(2*np.pi/200*k)refer_traj[k,1] = 15*math.sin(2*np.pi/200*k)# draw reference trajectoryplt.plot(refer_traj[:,0], refer_traj[:,1], color='b')# an UGV instanceugv = UGV_model(0, 0, np.pi/2, 1.6, 2.6, 0.5)# define lookaheadld = ugv.v*2# Pure Pursuit algorithmflag = 0for i in range(200):vehicle_state = np.zeros(2)vehicle_state[0] = ugv.xvehicle_state[1] = ugv.ycnt = 0;Q = []for m in range(flag,200):ds = np.linalg.norm(vehicle_state-refer_traj[m])if(ds >= ld):flag = mbreakelse:passpassds = np.linalg.norm(vehicle_state-refer_traj[flag])dx,dy = refer_traj[flag] - vehicle_statealpha = math.atan2(dy,dx) - ugv.thetadelta = math.atan(2*np.sin(alpha)*ugv.l/ld)ugv.update(delta)ugv.draw()# pursuit the end reference pointif(flag==199):breakelse:pass
仿真结果如下图所示，蓝色为参考轨迹、红色为跟踪轨迹。
不难看出，在计算前轮转角的时候，反正切运算的分母用的是前视距离，而不是实际距离，如果改用实际距离，跟踪会失败，如下图所示。
