本文的python源代码来自：
/gameinskysky/PythonRobotics/blob/master/PathTracking/pure_pursuit/pure_pursuit.py
纯跟踪算法的原理，详见/gophae/article/details/100012763
我们对纯跟踪算法进行一次仿真，python 我已经改过，如下：
import numpy as npimport mathimport matplotlib.pyplot as plt#定义常数k = 0.1 # look forward gainLfc = 1.0 # look-ahead distanceKp = 1.0 # speed propotional gaindt = 0.1 # [s]L = 2.9 # [m] wheel base of vehicleshow_animation = Trueclass VehicleState:# 定义一个类，用于调用车辆状态信息def __init__(self, x=0.0, y=0.0, yaw=0.0, v=0.0):self.x = xself.y = yself.yaw = yawself.v = vdef update(state, a, delta):#更新车辆状态信息state.x = state.x + state.v * math.cos(state.yaw) * dtstate.y = state.y + state.v * math.sin(state.yaw) * dtstate.yaw = state.yaw + state.v / L * math.tan(delta) * dtstate.v = state.v + a * dtreturn statedef PIDControl(target, current):#PID控制，定速巡航a = Kp * (target - current)return adef pure_pursuit_control(state, cx, cy, pind):# 纯跟踪控制器ind = calc_target_index(state, cx, cy)#找到最近点的函数，输出最近点位置if pind >= ind:ind = pindif ind < len(cx):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.yawif state.v < 0: # 如果是倒车的话，就要反过来alpha = math.pi - alphaLf = k * state.v + Lfcdelta = math.atan2(2.0 * L * math.sin(alpha) / Lf, 1.0)#核心计算公式return delta, inddef calc_target_index(state, cx, cy):# 找到与车辆当前位置最近点的序号# search nearest point indexdx = [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 + Lfc# search look ahead target point indexwhile 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 inddef main():# target course ，随机出来一条sin函数曲线cx = np.arange(0, 50, 0.1)cy = [math.sin(ix / 5.0) * ix / 2.0 for ix in cx]target_speed = 10.0 / 3.6 # [m/s]T = 100.0 # max simulation time# initial statestate = VehicleState(x=-0.0, y=-3.0, yaw=0.0, 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 = PIDControl(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)if show_animation:plt.cla()plt.plot(cx, cy, ".r", label="course")plt.plot(x, y, "-b", label="trajectory")plt.plot(cx[target_ind], cy[target_ind], "xg", label="target")plt.axis("equal")plt.grid(True)plt.title("Speed[km/h]:" + str(state.v * 3.6)[:4])plt.pause(0.001)plt.show()if __name__ == '__main__':print("Pure pursuit path tracking simulation start")main()
将本段代码改为MATLAB版本：
k = 0.1; % look forward gainLfc = 1.0; % look-ahead distanceKp = 1.0 ; % speed propotional gaindt = 0.1 ;% [s]L = 2.9 ;% [m] wheel base of vehiclecx = 0:0.1:50;cx = cx';for i = 1:length(cx)cy(i) = sin(cx(i)/5)*cx(i)/2;endi = 1;target_speed = 10/3.6;T = 80;lastIndex = length(cx);x = 0; y = -3; yaw = 0; v = 0;time = 0;Lf = k * v + Lfc;figurewhile T > time target_ind= calc_target_index(x,y,cx,cy,Lf)ai = PIDcontrol(target_speed, v,Kp);di = pure_pursuit_control(x,y,yaw,v,cx,cy,target_ind,k,Lfc,L,Lf);[x,y,yaw,v] = update(x,y,yaw,v, ai, di,dt,L)time = time + dt;%pause(0.1)plot(cx,cy,'b',x,y,'r-*')drawnowhold onend% plot(cx,cy,x,y,'*')% hold onfunction [x, y, yaw, v] = update(x, y, yaw, v, a, delta,dt,L)x = x + v * cos(yaw) * dt;y = y + v * sin(yaw) * dt;yaw = yaw + v / L * tan(delta) * dt;v = v + a * dt;endfunction [a] = PIDcontrol(target_v, current_v, Kp)a = Kp * (target_v - current_v);endfunction [delta] = pure_pursuit_control(x,y,yaw,v,cx,cy,ind,k,Lfc,L,Lf)tx = cx(ind);ty = cy(ind);alpha = atan((ty-y)/(tx-x))-yaw;Lf = k * v + Lfc;delta = atan(2*L * sin(alpha)/Lf) ;endfunction [ind] = calc_target_index(x,y, cx,cy,Lf)N = length(cx);Distance = zeros(N,1);for i = 1:NDistance(i) = sqrt((cx(i)-x)^2 + (cy(i)-y)^2);end[~, location]= min(Distance);ind = location;% LL = 0;%while Lf > LL && (ind + 1) < length(cx)% dx = cx(ind + 1 )- cx(ind);% dy = cx(ind + 1) - cx(ind);% LL = LL + sqrt(dx * 2 + dy * 2);% ind = ind + 1;%end%ind = ind + 10end
仿真结果如下：
如果觉得《Pure Pursuit纯跟踪算法Python/Matlab算法实现》对你有帮助，请点赞、收藏，并留下你的观点哦！
