用python实现Pure Pursuit控制算法

本文主要是介绍用python实现Pure Pursuit控制算法，希望对大家解决编程问题提供一定的参考价值，需要的开发者们随着小编来一起学习吧！

之前对Pure Pursuit控制算法作了介绍，并用Matlab进行了仿真，具体参考：https://blog.csdn.net/Ronnie_Hu/article/details/115817922?spm=1001.2014.3001.5501。

下面改用python对其进行仿真，同样跟踪一个圆形轨迹，具体代码如下：

import numpy as np
import matplotlib.pyplot as plt
import math# set figure size
plt.figure(figsize=(8, 8))# define UGV class
class 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 trajectory
refer_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 trajectory
plt.plot(refer_traj[:,0], refer_traj[:,1], color='b')# an UGV instance
ugv = UGV_model(0, 0, np.pi/2, 1.6, 2.6, 0.5)# define lookahead
ld = ugv.v*2# Pure Pursuit algorithm
flag = 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 np
import matplotlib.pyplot as plt
import math# set figure size
plt.figure(figsize=(8, 8))# define UGV class
class 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 trajectory
refer_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 trajectory
plt.plot(refer_traj[:,0], refer_traj[:,1], color='b')# an UGV instance
ugv = UGV_model(0, 0, np.pi/2, 1.6, 2.6, 0.5)# define lookahead
ld = ugv.v*2# Pure Pursuit algorithm
flag = 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

仿真结果如下图所示，蓝色为参考轨迹、红色为跟踪轨迹。